From 6023295880c5bbf093cbe26654eeca4e03e2c47a Mon Sep 17 00:00:00 2001 From: Florian Taillard Date: Tue, 3 May 2011 17:25:59 +0200 Subject: [PATCH] [ARDrone] Add algorythm for moving Add calcul for drone command, and command function. Fix compilation warning. --- owlps-ardrone/owlps-drone.c | 256 +++++++++++++++++++++++++++++++++--- owlps-ardrone/owlps-drone.h | 62 +++++++-- 2 files changed, 290 insertions(+), 28 deletions(-) diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index e9f06a3..243bd59 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -14,12 +14,13 @@ #include #include -#include #include #include #include +#include + #define SEPARATOR ',' #define EOL '$' #define TYPE "GPS" @@ -66,10 +67,28 @@ uint8_t *packet = NULL ; // Packet to send uint_fast16_t packet_size ; // Packet size // Shared variables for OwlSIG (Geo Information System) -double share_x, share_y, share_z; -double share_lat, share_lon, share_alt=0; -double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0; -double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0; +//double share_x, share_y, share_z; + +struct point share_gps; +struct point share_gps_retro; +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; + +/* Socket drone */ + +int sockcmdfd; +struct sockaddr_in servcmd; +char sendline[100]; +char count[100]; + +//double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0; +//double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0; +double share_t_cons=-999.00; int main(int argc, char *argv[]) { @@ -100,7 +119,10 @@ int main(int argc, char *argv[]) return -1; } - + while(1) + { + usleep(200000); + } @@ -217,10 +239,14 @@ void traficGPS(int gps) { if (datagps.orilat=='S') datagps.latitude=-datagps.latitude; if (datagps.orilon=='W') datagps.longitude=-datagps.longitude; - share_lat=datagps.latitude; - share_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; printf("lat %s lon %s\n",data[3], data[5]); - printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_lat, share_lon); + + printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_gps.lat, share_gps.lon); + } } } @@ -297,20 +323,20 @@ void* thread_control(void* NULL_value) if (strcmp(cmd, "ReadPosition")==0) sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++); else if (strcmp(cmd, "ReadGeoPosition")==0) - sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt); + sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_gps.lat,share_gps.lon,share_gps.alt); else if (strcmp(cmd, "SendGeoSetpoint")==0) { - share_lat_cons=atof(sx); - share_lon_cons=atof(sy); - share_alt_cons=atof(sz); + share_gps_cons.lat=atof(sx); + share_gps_cons.lon=atof(sy); + share_gps_cons.alt=atof(sz); share_t_cons=atof(st); - sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_lat_cons,share_lon_cons,share_alt_cons,share_t_cons); + sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_gps_cons.lat,share_gps_cons.lon,share_gps_cons.alt,share_t_cons); } else if (strcmp(cmd, "SendSetpoint")==0) { - share_x_cons=atof(sx); - share_y_cons=atof(sy); - share_z_cons=atof(sz); + share_relatif_cons.x=atof(sx); + share_relatif_cons.y=atof(sy); + share_relatif_cons.z=atof(sz); share_t_cons=atof(st); - sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_x_cons,share_y_cons,share_z_cons,share_t_cons); + sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_relatif_cons.x,share_relatif_cons.y,share_relatif_cons.z,share_t_cons); } else strcat(response,"Not understand\n"); @@ -810,3 +836,197 @@ void print_version() #endif // OWLPS_VERSION:w ) ; } + + +/* Commands for drone */ + + + +float calcul_trajectory(void) +{ + float dist_av, dist_ar, dep ; + float angle ; + 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; + + return angle; +} + +float calcul_speed(float angle) +{ + float time; + time = angle/180; + return time; +} + +int read_position(char *type) +{ + if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n"); + else if (!strcmp(type,"GPS")) + { + printf("Localisation by GPS\n"); + while((share_gps.lat==0||share_gps.lon==0)&&(share_gps_origin.lat==0||share_gps_origin.lon==0)) + { + if(share_gps.lat!=0 && share_gps.lon!=0) + { + share_gps_origin.lat = share_gps.lat; + share_gps_origin.lon = share_gps.lon; + } + sleep(1); + } + 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; + return 0; +} + +int cmd_drone_take_off(void) +{ + printf("Take-off...\n"); + state_flying=1; + return 0; +} +int cmd_drone_landing(void) +{ + printf("Landing....\n"); + state_flying=0; + return 0; +} + +int cmd_drone_init_direction(void) +{ + int boucle; + float speed; + assert(sizeof(float) == 4) ; + + if(state_flying==0) + return -1; + //Implemented socket communication + printf("Calibration...\n"); + speed=-0.5; + while(lock==1); + lock=1; + for(boucle=0;boucle<3;boucle++) + { + int *floatint_speed = (int32_t*) &speed ; + sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed); + } + lock=0; + return 1; +} + + int cmd_drone_move(float speed_pitch, float speed_yaw) +{ + if(state_flying==0) + return -1; + else + { + while(lock==1); + 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); + i++; + sendto(sockcmdfd,sendline,strlen(sendline),0, + (struct sockaddr *)&servcmd,sizeof(servcmd)); + + } + lock=0; + return 0; +} + +void drone_err(int err) +{ + char msg[100]; + + switch(err) + { + case 1 : + strcpy(msg,"Position is not determined or type of localisation is invalid, canceled\n"); + break; + case 2 : + strcpy(msg,"Taking-off failed, canceled\n"); + break; + case 3 : + strcpy(msg,"Calibration failed, canceled\n"); + break; + case 4 : + strcpy(msg,"Can not move drone, it's not flying, canceled\n"); + break; + } + + printf("%s",msg); + exit(0); +} + +int drone_socket_create(void) +{ + sockcmdfd=socket(AF_INET,SOCK_DGRAM,0); + + bzero(&servcmd,sizeof(servcmd)); + servcmd.sin_family = AF_INET; + servcmd.sin_addr.s_addr=inet_addr(IP); + servcmd.sin_port=htons(PORT); + + return sockcmdfd; +} + +int drone_socket_close(void) +{ + return close(sockcmdfd); +} + +void* drone_socket_watchdog(void* NULL_value) +{ + while(1) + { + 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\",\"3.14\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR); + i=4; + } + else + { + while(lock==1); + lock=1; + snprintf(count, 100, "%d", i); + strncat(sendline, ATREF, LENSTR); + strncat(sendline, count, LENSTR); + if(state_flying==0) + strncat(sendline, LANDING, LENSTR); + else if(state_flying==1) + strncat(sendline, TAKEOFF, LENSTR); + else + strncat(sendline, LANDING, LENSTR); + strncat(sendline, "\r", LENSTR); + } + i++; + if(i>=101) + { + strncat(sendline, "AT*COMWDG=", LENSTR); + snprintf(count, 100, "%d", i); + strncat(sendline, count, LENSTR); + strncat(sendline, "\r", LENSTR); + i=1; + } + sendto(sockcmdfd,sendline,strlen(sendline),0, + (struct sockaddr *)&servcmd,sizeof(servcmd)); + lock=0; + } + pthread_exit(NULL_value); +} diff --git a/owlps-ardrone/owlps-drone.h b/owlps-ardrone/owlps-drone.h index 1ae94be..ecf2b79 100644 --- a/owlps-ardrone/owlps-drone.h +++ b/owlps-ardrone/owlps-drone.h @@ -1,4 +1,8 @@ #include +#include +#include +/* include OpenCoordinate */ +#include "oc.c" #define DEBUG @@ -18,6 +22,24 @@ /* Program arguments (getopt string) */ #define OPTIONS "d:hi:l::n:p:t:V" +/* Type of position (WIFI or GPS) */ +#define TYPE "GPS" + +/* Define M_PI if is not define */ +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +//Define send command +#define LENSTR 1000 +#define ATREF "AT*REF=" +#define LANDING ",290717696" +#define TAKEOFF ",290718208" +//Define socket +//#define IP "172.20.105.4" +#define IP "192.168.1.1" +#define PORT 5556 + /* Function headers */ void parse_command_line(int argc, char **argv) ; @@ -54,14 +76,34 @@ struct result struct gps { -float time; -char state; -double latitude; -char orilat; -double longitude; -char orilon; -float speed; -float cap; -int date; -float decmag; + float time; + char state; + double latitude; + char orilat; + double longitude; + char orilon; + float speed; + float cap; + int date; + float decmag; }; + + + +/* Prototype drone */ + +void drone_err(int); +//Position +int read_position(char*); +//Socket +int drone_socket_create(void); +void* drone_socket_watchdog(void*); +int drone_socket_close(void); +//Commands drone +int cmd_drone_take_off(void); //Add socket +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); +float calcul_speed(float);