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

[ARDrone] Fix bug command 

Change measure angle to measure cap. Add flag 1 on command AT*PCMD.
This commit is contained in:
Florian Taillard 2011-05-31 14:29:14 +02:00 committed by Matteo Cypriani
parent d6598c52a1
commit 4ac4216ce0
2 changed files with 109 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,6 +17,7 @@
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/time.h>
#include <fcntl.h> #include <fcntl.h>
#include <assert.h> #include <assert.h>
@ -25,7 +26,7 @@
#include <semaphore.h> #include <semaphore.h>
#define SEPARATOR ',' #define SEPARATOR ','
#define EOL '$' #define EOL '$'
#define TYPE "WIFI" #define TYPE "GPS"
/* Options */ /* Options */
@ -98,7 +99,7 @@ int main(int argc, char *argv[])
FILE* log = NULL; FILE* log = NULL;
float angle, distance, speed_pitch, distance_total; float angle, distance, speed_pitch, distance_total;
struct sigaction action; struct sigaction action;
int boot=0;
program_name = argv[0] ; program_name = argv[0] ;
parse_command_line(argc, argv) ; parse_command_line(argc, argv) ;
@ -116,13 +117,19 @@ int main(int argc, char *argv[])
memset(&share_relatif_cons,0,sizeof(share_relatif_cons)); memset(&share_relatif_cons,0,sizeof(share_relatif_cons));
memset(&share_relatif_retro,0,sizeof(share_relatif_retro)); memset(&share_relatif_retro,0,sizeof(share_relatif_retro));
//Set unknow value gps
share_gps_retro.lat=-999.00; share_gps_retro.lat=-999.00;
share_gps_retro.lon=-999.00; share_gps_retro.lon=-999.00;
share_gps_cons.lat = -999.00;
share_gps_cons.lon = -999.00;
// Thread communication TCP OwlSIG // Thread communication TCP OwlSIG
pthread_t control ; pthread_t control ;
pthread_create(&control, NULL, thread_control, NULL) ; pthread_create(&control, NULL, thread_control, NULL) ;
//Launch thread or wifi depend of TYPE
if(!strcmp(TYPE,"WIFI")) if(!strcmp(TYPE,"WIFI"))
{ {
pthread_t send ; pthread_t send ;
@ -143,17 +150,22 @@ int main(int argc, char *argv[])
return -1; 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) ;
log = fopen("vol.log", "w");
fprintf(log, "Date;Heure;Latitude;Longitude;Latitude_cons;Longitude_cons;DistanceTotal;Distance;SpeedPitch;angle;x;y;x_retro;y_retro;x_cons;y_cons\n");
while(run) while(run)
{ {
while(start==0) usleep(200000); while(start==0) usleep(200000);
//Wait GPS Signal
if(share_gps.lat==0||share_gps.lon==0) if(share_gps.lat==0||share_gps.lon==0)
printf("[GPS] GPS not fix! Wait...\n"); printf("[GPS] GPS not fix! Wait...\n");
while(share_gps.lat==0||share_gps.lon==0); while(share_gps.lat==0||share_gps.lon==0);
log = fopen("vol.log", "w"); /* Log File */
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"); printf("[GPS] GPS fix OK!\n");
speed_pitch=-1; speed_pitch=-1;
@ -162,31 +174,34 @@ int main(int argc, char *argv[])
if(cmd_drone_take_off()<0) if(cmd_drone_take_off()<0)
drone_err(2); drone_err(2);
sleep(2); while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00);
sleep(8);
distance_total=fabs(oc_distance_between(share_gps, share_gps_cons)); distance_total=fabs(oc_distance_between(share_gps, share_gps_cons));
if(cmd_drone_init_direction()<0) if(cmd_drone_init_direction()<0)
drone_err(3); drone_err(3);
boot=0;
while(start==1&&!check_destination(share_gps, share_gps_cons)) while(start==1&&!check_destination(share_gps, share_gps_cons))
{ {
if(read_position(TYPE)<0) if(read_position(TYPE)<0)
drone_err(1); drone_err(1);
calcul_trajectory(&angle, &distance); //calcul_trajectory(&angle, &distance_old);
angle = calcul_angle();
if(speed_pitch < -1) speed_pitch = -1; distance = fabs(oc_distance_between(share_gps, share_gps_cons));
else if(speed_pitch > 1) speed_pitch = 1;
if(speed_pitch<-(distance/distance_total)) if(speed_pitch<-(distance/distance_total))
speed_pitch=speed_pitch; speed_pitch=speed_pitch;
else else
speed_pitch=-(distance/distance_total); speed_pitch=-(distance/distance_total);
if(speed_pitch < -1) speed_pitch = -1;
else if(speed_pitch > 1) speed_pitch = 1;
cmd_drone_move(speed_pitch, angle); cmd_drone_move((speed_pitch/5), (angle/2));
sleep(1); usleep(10000);
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); //boot++;
printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying); printf("Vitesse : %f Angle : %f\n", (speed_pitch/5), angle/2);
fprintf(log,"%d;%f;%f;%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, share_gps_cons.lat, share_gps_cons.lon, distance_total, distance, -(speed_pitch/2), angle/2, 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); fclose(log);
cmd_drone_landing(); cmd_drone_landing();
@ -224,7 +239,7 @@ void traficGPS(int gps) {
do do
{ {
c=read(gps, buffer, 4096); c=read(gps, buffer, 4096);
printf("lecture intiale %d\n",c); //printf("lecture intiale %d\n",c);
} }
while (c>99); while (c>99);
do do
@ -266,7 +281,7 @@ void traficGPS(int gps) {
} }
y++; y++;
} }
printf("\nTaille finale : %d\n", length); //printf("\nTaille finale : %d\n", length);
y=0; y=0;
while(y<=length) while(y<=length)
@ -311,10 +326,11 @@ void traficGPS(int gps) {
share_gps_retro.lon=share_gps.lon; share_gps_retro.lon=share_gps.lon;
share_gps.lat=datagps.latitude; share_gps.lat=datagps.latitude;
share_gps.lon=datagps.longitude; share_gps.lon=datagps.longitude;
share_gps.cap=datagps.cap;
} }
share_gps.date=datagps.date; share_gps.date=datagps.date;
share_gps.time=datagps.time; share_gps.time=datagps.time;
printf("lat %s lon %s\n",data[3], data[5]); //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_gps.lat, share_gps.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);
@ -327,7 +343,6 @@ void* thread_gps(void* NULL_value)
char buffer[1024]; char buffer[1024];
int gps_fd = open("/dev/ttyPA0",O_RDONLY); int gps_fd = open("/dev/ttyPA0",O_RDONLY);
read(gps_fd, buffer, 80); read(gps_fd, buffer, 80);
traficGPS(gps_fd); traficGPS(gps_fd);
pthread_exit(NULL_value); pthread_exit(NULL_value);
@ -421,6 +436,11 @@ void* thread_control(void* NULL_value)
cmd_drone_landing(); cmd_drone_landing();
sprintf(response,"%sLanding;OK;",response); sprintf(response,"%sLanding;OK;",response);
} }
else if(strcmp(cmd, "eraseTrace")==0)
{
num_point=0;
sprintf(response,"%seraseTrace;OK;",response);
}
else else
strcat(response,"Not understand\n"); strcat(response,"Not understand\n");
n = write(newsockfd,response,strlen(response)); n = write(newsockfd,response,strlen(response));
@ -841,7 +861,7 @@ void string2data(char* string_data)
} }
} }
results[count_algo].z = atof(ptr) ; results[count_algo].z = atof(ptr) ;
if(error==1) if(error==1)
{ {
//Lecture du message Error //Lecture du message Error
ptr = strtok(NULL, delims) ; ptr = strtok(NULL, delims) ;
@ -992,19 +1012,28 @@ void calcul_trajectory(float *distance, float *angle)
/* T T | F */ /* T T | F */
/* ********** */ /* ********** */
if((a<0)!=(b<0)) if(a*b>=0) (*angle) = -(*angle);
(*angle)=(*angle);
else (*angle) = -(*angle);
} }
int check_destination(struct point drone, struct point target) int check_destination(struct point drone, struct point target)
{ {
if(oc_distance_between(drone, target)<1.8) if(oc_distance_between(drone, target)<3)
return 1; return 1;
else return 0; else return 0;
} }
float calcul_angle()
{
float captarget = oc_cap(share_gps, share_gps_cons);
if((share_gps.cap-captarget-180)<0)
return ((int)(360 - share_gps.cap + (captarget-180))%360)-180;
else
return ((int)(360 - share_gps.cap + (180 - captarget))%360)-180;
}
int read_position(char *type) int read_position(char *type)
{ {
if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n"); if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n");
@ -1020,6 +1049,7 @@ int read_position(char *type)
} }
usleep(100000); usleep(100000);
} }
share_relatif = oc_convert(share_gps_origin, share_gps); share_relatif = oc_convert(share_gps_origin, share_gps);
if(share_gps_retro.lat!=-999.00&&share_gps_retro.lon!=-999.00) 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_retro = oc_convert(share_gps_origin, share_gps_retro);
@ -1059,36 +1089,38 @@ int cmd_drone_init_direction(void)
sem_wait(&lock); sem_wait(&lock);
//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) while(share_gps_retro.lat==-999.00||share_gps_retro.lon==-999.00)
{ {
int *floatint_speed = (int32_t*) &speed ; int *floatint_speed = (int32_t*) &speed ;
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed); sprintf(sendline,"AT*PCMD=%d,1,0,%d,0,0\r", i, *floatint_speed);
sendto(sockcmdfd,sendline,strlen(sendline),0, sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd)); (struct sockaddr *)&servcmd,sizeof(servcmd));
usleep(100000); usleep(100000);
} }
sprintf(sendline,"AT*PCMD=%d,0,0,0,0,0\r", i); sprintf(sendline,"AT*PCMD=%d,1,0,0,0,0\r", i);
sendto(sockcmdfd,sendline,strlen(sendline),0, sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd)); (struct sockaddr *)&servcmd,sizeof(servcmd));
sem_post(&lock); sem_post(&lock);
return 1; return 1;
} }
int cmd_drone_move(float speed_pitch, float angle) int cmd_drone_move(float speed_pitch, int angle)
{ {
float speed_yaw; float speed_yaw;
if(state_flying==0) //if(state_flying==0)
return -1; //return -1;
else //else
{ //{
speed_yaw=(1/180)*angle; speed_yaw=((float)angle/180);
sem_wait(&lock); sem_wait(&lock);
int *floatint_speed_pitch = (int32_t*) &speed_pitch; int *floatint_speed_pitch = (int32_t*) &speed_pitch;
int *floatint_speed_yaw = (int32_t*) &speed_yaw; 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); sprintf(sendline,"AT*PCMD=%d,1,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw);
i++; //printf("%s\n", sendline);
sendto(sockcmdfd,sendline,strlen(sendline),0, i++;
(struct sockaddr *)&servcmd,sizeof(servcmd)); sendto(sockcmdfd,sendline,strlen(sendline),0,
} (struct sockaddr *)&servcmd,sizeof(servcmd));
//printf("[ATPCMD] pitch : %f yaw : %f\n", speed_pitch, speed_yaw);
//}
sem_post(&lock); sem_post(&lock);
return 0; return 0;
} }
@ -1136,14 +1168,18 @@ int drone_socket_close(void)
void* drone_socket_watchdog(void* NULL_value) void* drone_socket_watchdog(void* NULL_value)
{ {
struct timeval start, end;
long max, calc;
max = 0;
while(run) while(run)
{ {
usleep(50000); gettimeofday(&start, NULL);
usleep(8000);
memset(sendline, 0, sizeof(sendline)); memset(sendline, 0, sizeof(sendline));
if(i==-1) 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); strncpy(sendline,"AT*CONFIG=1,\"control:outdoor\",\"TRUE\"\rAT*CONFIG=2,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=3,\"control:control_yaw\",\"2.5\"\rAT*PMODE=4,2\rAT*MISC=5,2,20,2000,3000\r", LENSTR);
i=5; i=6;
} }
else else
{ {
@ -1170,7 +1206,12 @@ void* drone_socket_watchdog(void* NULL_value)
} }
sendto(sockcmdfd,sendline,strlen(sendline),0, sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd)); (struct sockaddr *)&servcmd,sizeof(servcmd));
//printf("[WATCHDOG] %s\n", sendline);
sem_post(&lock); sem_post(&lock);
gettimeofday(&end, NULL);
calc = ((end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec));
//printf("[WATCHDOG] Time usec : %ld\n", calc);
} }
pthread_exit(NULL_value); pthread_exit(NULL_value);
} }
@ -1193,40 +1234,40 @@ void drone_handler_sigint(const int num)
consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude) consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude)
{ {
consigne* new_consigne = malloc(sizeof(consigne)); consigne* new_consigne = malloc(sizeof(consigne));
new_consigne->lat = latitude; new_consigne->lat = latitude;
new_consigne->lon = longitude; new_consigne->lon = longitude;
new_consigne->alt = altitude; new_consigne->alt = altitude;
new_consigne->next = list; new_consigne->next = list;
return new_consigne; return new_consigne;
} }
consigne* list_add_back(consigne* list, double lat, double lon, double alt) consigne* list_add_back(consigne* list, double lat, double lon, double alt)
{ {
consigne* new_consigne = malloc(sizeof(consigne)); consigne* new_consigne = malloc(sizeof(consigne));
new_consigne->lat = lat; new_consigne->lat = lat;
new_consigne->lon = lon; new_consigne->lon = lon;
new_consigne->alt = alt; new_consigne->alt = alt;
new_consigne->next = NULL; new_consigne->next = NULL;
if(list == NULL) if(list == NULL)
{ {
return new_consigne; return new_consigne;
} }
else else
{ {
consigne* tmp=list; consigne* tmp=list;
while(tmp->next != NULL) while(tmp->next != NULL)
{ {
tmp = tmp->next; tmp = tmp->next;
} }
tmp->next = new_consigne; tmp->next = new_consigne;
return list; return list;
} }
} }

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

@ -110,13 +110,13 @@ int drone_socket_close(void);
//Commands drone //Commands drone
int cmd_drone_take_off(void); //Add socket int cmd_drone_take_off(void); //Add socket
int cmd_drone_landing(void); //Add socket int cmd_drone_landing(void); //Add socket
int cmd_drone_move(float, float); int cmd_drone_move(float, int);
int cmd_drone_init_direction(void); //Add socket int cmd_drone_init_direction(void); //Add socket
//Algo //Algo
void calcul_trajectory(float*, float*); void calcul_trajectory(float*, float*);
float calcul_speed(float); float calcul_speed(float);
int check_destination(struct point, struct point); int check_destination(struct point, struct point);
float calcul_angle(void);
//Liste chaine //Liste chaine
consigne* list_add_front(consigne*, double, double, double); consigne* list_add_front(consigne*, double, double, double);
consigne* list_add_back(consigne*, double, double, double); consigne* list_add_back(consigne*, double, double, double);