[ARDrone] Change format list consigne

Change destination array to linked list.
Fix thread priority witch semaphore.

owlps-ardrone/owlps-drone.c

@@ -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,15 +143,17 @@ int main(int argc, char *argv[])
       return -1;
     }
 
-  drone_socket_create() ;
+  //drone_socket_create() ;
-  pthread_t drone_watchdog ;
+  //pthread_t drone_watchdog ;
-  pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
+  //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;
       
@@ -155,7 +162,7 @@ int main(int argc, char *argv[])
       
       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)
@@ -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);
+          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\n",speed_pitch, angle);
+          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;
+        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,6 +1021,7 @@ int read_position(char *type)
           usleep(100000);
         }
       share_relatif = oc_convert(share_gps_origin, share_gps);
+      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);
 
@@ -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,9 +1056,9 @@ int cmd_drone_init_direction(void)
   //Implemented socket communication
   printf("Initialisation...\n");
   speed=-0.5;
-  while(lock==1);
+  sem_wait(&lock);
-  lock=1;
+  //for(boucle=0;boucle<3;boucle++)
-  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);
@@ -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);
+      sem_wait(&lock);
-      lock=1;
       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);
+          sem_wait(&lock);
-          lock=1;
           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;
+    }
+}

owlps-ardrone/owlps-drone.h

@@ -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*);