[ARDrone] Add control for speed of pitch

The pitch speed is compute witch total distance and real-time distance.

owlps-ardrone/owlps-drone.c

@@ -93,11 +93,13 @@ double share_t_cons=-999.00;
 
 int main(int argc, char *argv[])
 {
+  float angle, distance, speed_pitch, distance_total;
   struct sigaction action;
 
   program_name = argv[0] ;
   parse_command_line(argc, argv) ;
 
+
   action.sa_flags = 0;
   sigemptyset(&action.sa_mask);
   action.sa_handler = drone_handler_sigint;
@@ -140,6 +142,7 @@ int main(int argc, char *argv[])
   pthread_t drone_watchdog ;
   pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
   while(start==0) usleep(200000);
+  speed_pitch=-1;
 
   if(read_position(TYPE)<0)
     drone_err(1);
@@ -150,12 +153,24 @@ int main(int argc, char *argv[])
   if(cmd_drone_init_direction()<0)
     drone_err(3);
 
+  calcul_trajectory(&angle, &distance);
+  distance_total = distance;
   while(!check_destination(share_gps, share_gps_cons))
     {
       if(read_position(TYPE)<0)
         drone_err(1);
 
-      cmd_drone_move(-0.2,calcul_trajectory());
+      calcul_trajectory(&angle, &distance);
+
+      if(speed_pitch < -1) speed_pitch = -1;
+      else if(speed_pitch > 1) speed_pitch = 1;
+
+      if(speed_pitch<-(distance/distance_total))
+        speed_pitch=speed_pitch;
+      else
+        speed_pitch=-(distance/distance_total);
+
+      cmd_drone_move(speed_pitch, angle);
       sleep(1);
     }
 
@@ -801,6 +816,7 @@ void string2data(char* string_data)
         }
       results[count_algo].z = atof(ptr) ;
 
+
       count_algo++;
 
     }
@@ -885,19 +901,19 @@ void print_version()
 
 
 
-float calcul_trajectory(void)
+void calcul_trajectory(float *distance, float *angle)
 {
   int a, b;
   float dist_av, dist_ar, dep ;
-  float angle ;
-  dist_av = dist_ar = dep = angle = 0 ;
+  (*distance) = dist_av = dist_ar = dep = (*angle) = 0 ;
 
   dist_av = sqrt(pow(share_relatif_cons.x-share_relatif.x,2)+pow(share_relatif_cons.y-share_relatif.y,2));
   dist_ar = sqrt(pow(share_relatif_cons.x-share_relatif_retro.x,2)+pow(share_relatif_cons.x-share_relatif_retro.y,2));
   dep = sqrt(pow(share_relatif.x-share_relatif_retro.x,2)+pow(share_relatif.y-share_relatif_retro.y,2));
 
-  angle = acos((pow(dist_ar,2)+pow(dep,2)-pow(dist_av,2))/(2*dist_ar*dep));
-  angle = angle*180/M_PI;
+  *angle = acos((pow(dist_ar,2)+pow(dep,2)-pow(dist_av,2))/(2*dist_ar*dep));
+  *angle = (*angle)*180/M_PI;
+  *distance = dist_av;
 
   a=share_relatif_cons.x-share_relatif.x;
   b=share_relatif_cons.y-share_relatif.y;
@@ -911,14 +927,14 @@ float calcul_trajectory(void)
   /* ********** */
 
   if((a<0)!=(b<0))
-    return angle;
-  else return -angle;
+    (*angle)=(*angle);
+  else (*angle) = -(*angle);
 
 }
 
 int check_destination(struct point drone, struct point target)
 {
-  if(oc_distance_between(drone, target)<1)
+  if(oc_distance_between(drone, target)<1.8)
     return 1;
   else return 0;
 }
@@ -936,15 +952,13 @@ int read_position(char *type)
               share_gps_origin.lat = share_gps.lat;
               share_gps_origin.lon = share_gps.lon;
             }
-          sleep(1);
+          usleep(100000);
         }
       share_relatif = oc_convert(share_gps_origin, share_gps);
       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);
-      double angle=oc_cap(share_gps_retro, share_gps);
-      printf("Angle : %f\n",angle);
 
     }
   else return -1;
@@ -1057,7 +1071,7 @@ void* drone_socket_watchdog(void* NULL_value)
       memset(sendline, 0, sizeof(sendline));
       if(i==-1)
         {
-          strncpy(sendline,"AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=2,\"control:control_yaw\",\"3.14\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR);
+          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;
         }
       else

owlps-ardrone/owlps-drone.h

@@ -70,6 +70,7 @@ struct result
   float x;
   float y;
   float z;
+  float err;
 };
 
 struct gps
@@ -103,6 +104,6 @@ int cmd_drone_landing(void); //Add socket
 int cmd_drone_move(float, float);
 int cmd_drone_init_direction(void); //Add socket
 //Algo
-float calcul_trajectory(void);
+void calcul_trajectory(float*, float*);
 float calcul_speed(float);
 int check_destination(struct point, struct point);