mcy
/
owlps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[ARDrone] Change format list consigne 

Change destination array to linked list.
Fix thread priority witch semaphore.
This commit is contained in:
Florian Taillard 2011-05-18 14:28:54 +02:00 committed by Matteo Cypriani
parent 3ba38df483
commit ad284d0b56
2 changed files with 100 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
owlps-ardrone/owlps-drone.c
View File

@ -22,10 +22,10 @@
#include <assert.h>
#include <signal.h>
#include <semaphore.h>
#define SEPARATOR ','
#define EOL '$'
#define TYPE "GPS"
#define TYPE "WIFI"
/* Options */
@ -72,14 +72,15 @@ uint_fast16_t packet_size ; // Packet size
struct point share_gps;
struct point share_gps_retro;
consigne* list_share_gps_cons = NULL;
struct point share_gps_cons;
struct point share_gps_origin;
struct point_relatif share_relatif;
struct point_relatif share_relatif_retro;
struct point_relatif share_relatif_cons;
int state_flying=0, lock=0, i=-1, start=0, run=1;
int state_flying=0, i=-1, start=0, run=1;
sem_t lock;
/* Socket drone */
int sockcmdfd;
@ -93,6 +94,7 @@ double share_t_cons=-999.00;
int main(int argc, char *argv[])
{
sem_init(&lock, 0, 1);
FILE* log = NULL;
float angle, distance, speed_pitch, distance_total;
struct sigaction action;
@ -114,6 +116,9 @@ int main(int argc, char *argv[])
memset(&share_relatif_cons,0,sizeof(share_relatif_cons));
memset(&share_relatif_retro,0,sizeof(share_relatif_retro));
share_gps_retro.lat=-999.00;
share_gps_retro.lon=-999.00;
// Thread communication TCP OwlSIG
pthread_t control ;
pthread_create(&control, NULL, thread_control, NULL) ;
@ -138,29 +143,31 @@ int main(int argc, char *argv[])
return -1;
}
drone_socket_create() ;
pthread_t drone_watchdog ;
pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
//drone_socket_create() ;
//pthread_t drone_watchdog ;
//pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
while(run)
{
while(start==0) usleep(200000);
if(share_gps.lat==0||share_gps.lon==0)
printf("[GPS] GPS not fix! Wait...\n");
while(share_gps.lat==0||share_gps.lon==0);
log = fopen("vol.log", "w");
fprintf(log, "Date;Heure;Latitude;Longitude;DistanceTotal;Distance;SpeedPitch;angle\n");
fprintf(log, "Date;Heure;Latitude;Longitude;DistanceTotal;Distance;SpeedPitch;angle;x;y;x_retro;y_retro;x_cons;y_cons\n");
printf("[GPS] GPS fix OK!\n");
speed_pitch=-1;
if(read_position(TYPE)<0)
drone_err(1);
if(cmd_drone_take_off()<0)
drone_err(2);
sleep(2);
distance_total=fabs(oc_distance_between(share_gps, share_gps_cons));
if(cmd_drone_init_direction()<0)
drone_err(3);
while(start==1&&!check_destination(share_gps, share_gps_cons))
{
if(read_position(TYPE)<0)
@ -178,12 +185,13 @@ int main(int argc, char *argv[])
cmd_drone_move(speed_pitch, angle);
sleep(1);
fprintf(log,"%d;%f;%f;%f;%f;%f;%f;%f\n",share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, distance_total, distance, speed_pitch, angle);
printf("[DEBUG] Vitesse %f Angle %f\n",speed_pitch, angle);
fprintf(log,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n",share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, distance_total, distance, speed_pitch, angle, share_relatif.x, share_relatif.y, share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);
printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying);
}
fclose(log);
cmd_drone_landing();
}
sem_destroy(&lock);
(void) close(socksendfd) ;
(void) close(sockreceivefd) ;
@ -297,10 +305,13 @@ void traficGPS(int gps) {
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
share_gps_retro.lat=share_gps.lat;
share_gps_retro.lon=share_gps.lon;
share_gps.lat=datagps.latitude;
share_gps.lon=datagps.longitude;
if(share_gps.lat!=datagps.latitude||share_gps.lon!=datagps.longitude)
{
share_gps_retro.lat=share_gps.lat;
share_gps_retro.lon=share_gps.lon;
share_gps.lat=datagps.latitude;
share_gps.lon=datagps.longitude;
}
share_gps.date=datagps.date;
share_gps.time=datagps.time;
printf("lat %s lon %s\n",data[3], data[5]);
@ -407,7 +418,7 @@ void* thread_control(void* NULL_value)
else if(strcmp(cmd, "SendLanding")==0)
{
start=0;
//cmd_drone_landing();
cmd_drone_landing();
sprintf(response,"%sLanding;OK;",response);
}
else
@ -1010,7 +1021,8 @@ int read_position(char *type)
usleep(100000);
}
share_relatif = oc_convert(share_gps_origin, share_gps);
share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro);
if(share_gps_retro.lat!=-999.00&&share_gps_retro.lon!=-999.00)
share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro);
share_relatif_cons = oc_convert(share_gps_origin, share_gps_cons);
//printf("X current : %f Y current : %f X old : %f Y old : %f X target : %f Y target : %f\n",share_relatif.x, share_relatif.y, -share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);
@ -1035,7 +1047,7 @@ int cmd_drone_landing(void)
int cmd_drone_init_direction(void)
{
int boucle;
//int boucle;
float speed;
assert(sizeof(float) == 4) ;
@ -1044,10 +1056,10 @@ int cmd_drone_init_direction(void)
//Implemented socket communication
printf("Initialisation...\n");
speed=-0.5;
while(lock==1);
lock=1;
for(boucle=0;boucle<3;boucle++)
{
sem_wait(&lock);
//for(boucle=0;boucle<3;boucle++)
while(share_gps_retro.lat==-999.00||share_gps_retro.lon==-999.00)
{
int *floatint_speed = (int32_t*) &speed ;
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed);
sendto(sockcmdfd,sendline,strlen(sendline),0,
@ -1057,7 +1069,7 @@ int cmd_drone_init_direction(void)
sprintf(sendline,"AT*PCMD=%d,0,0,0,0,0\r", i);
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
lock=0;
sem_post(&lock);
return 1;
}
@ -1069,8 +1081,7 @@ int cmd_drone_move(float speed_pitch, float angle)
else
{
speed_yaw=(1/180)*angle;
while(lock==1);
lock=1;
sem_wait(&lock);
int *floatint_speed_pitch = (int32_t*) &speed_pitch;
int *floatint_speed_yaw = (int32_t*) &speed_yaw;
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw);
@ -1078,7 +1089,7 @@ int cmd_drone_move(float speed_pitch, float angle)
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
}
lock=0;
sem_post(&lock);
return 0;
}
@ -1127,17 +1138,16 @@ void* drone_socket_watchdog(void* NULL_value)
{
while(run)
{
usleep(20000);
usleep(50000);
memset(sendline, 0, sizeof(sendline));
if(i==-1)
{
strncpy(sendline,"AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=2,\"control:control_yaw\",\"2.5\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR);
i=4;
i=5;
}
else
{
while(lock==1);
lock=1;
sem_wait(&lock);
snprintf(count, 100, "%d", i);
strncat(sendline, ATREF, LENSTR);
strncat(sendline, count, LENSTR);
@ -1160,7 +1170,7 @@ void* drone_socket_watchdog(void* NULL_value)
}
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
lock=0;
sem_post(&lock);
}
pthread_exit(NULL_value);
}
@ -1178,3 +1188,45 @@ void drone_handler_sigint(const int num)
exit(0);
}
}
/* Liste chaine */
consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude)
{
consigne* new_consigne = malloc(sizeof(consigne));
new_consigne->lat = latitude;
new_consigne->lon = longitude;
new_consigne->alt = altitude;
new_consigne->next = list;
return new_consigne;
}
consigne* list_add_back(consigne* list, double lat, double lon, double alt)
{
consigne* new_consigne = malloc(sizeof(consigne));
new_consigne->lat = lat;
new_consigne->lon = lon;
new_consigne->alt = alt;
new_consigne->next = NULL;
if(list == NULL)
{
return new_consigne;
}
else
{
consigne* tmp=list;
while(tmp->next != NULL)
{
tmp = tmp->next;
}
tmp->next = new_consigne;
return list;
}
}

13
owlps-ardrone/owlps-drone.h
View File

@ -88,6 +88,14 @@ struct gps
float decmag;
};
typedef struct _consigne
{
double lat;
double lon;
double alt;
struct _consigne *next;
} consigne ;
/* Prototype drone */
@ -108,3 +116,8 @@ int cmd_drone_init_direction(void); //Add socket
void calcul_trajectory(float*, float*);
float calcul_speed(float);
int check_destination(struct point, struct point);
//Liste chaine
consigne* list_add_front(consigne*, double, double, double);
consigne* list_add_back(consigne*, double, double, double);
void list_print(consigne*);