[ARDrone] Fix rotation direction

Fix sign of angle value.
Add landing by interrupt SIGINT.
Add check_destination() for control arrival.
This commit is contained in:
Florian Taillard 2011-05-04 17:17:30 +02:00
parent 6023295880
commit 9380678782
2 changed files with 101 additions and 28 deletions

View File

@ -21,11 +21,12 @@
#include <assert.h> #include <assert.h>
#include <signal.h>
#define SEPARATOR ',' #define SEPARATOR ','
#define EOL '$' #define EOL '$'
#define TYPE "GPS" #define TYPE "GPS"
/* Options */ /* Options */
gps datagps; gps datagps;
@ -77,7 +78,7 @@ struct point_relatif share_relatif;
struct point_relatif share_relatif_retro; struct point_relatif share_relatif_retro;
struct point_relatif share_relatif_cons; struct point_relatif share_relatif_cons;
int state_flying=0, lock=0, i=-1; int state_flying=0, lock=0, i=-1, start=0, run=1;
/* Socket drone */ /* Socket drone */
@ -92,9 +93,25 @@ double share_t_cons=-999.00;
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
struct sigaction action;
program_name = argv[0] ; program_name = argv[0] ;
parse_command_line(argc, argv) ; parse_command_line(argc, argv) ;
action.sa_flags = 0;
sigemptyset(&action.sa_mask);
action.sa_handler = drone_handler_sigint;
sigaction(SIGINT, &action, NULL);
// Reset value share variables
memset(&share_gps,0,sizeof(share_gps));
memset(&share_gps_cons,0,sizeof(share_gps_cons));
memset(&share_gps_retro,0,sizeof(share_gps_retro));
memset(&share_gps_origin,0,sizeof(share_gps_origin));
memset(&share_relatif,0,sizeof(share_relatif));
memset(&share_relatif_cons,0,sizeof(share_relatif_cons));
memset(&share_relatif_retro,0,sizeof(share_relatif_retro));
// 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) ;
@ -119,13 +136,29 @@ int main(int argc, char *argv[])
return -1; return -1;
} }
while(1) drone_socket_create() ;
pthread_t drone_watchdog ;
pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
while(start==0) usleep(200000);
if(read_position(TYPE)<0)
drone_err(1);
if(cmd_drone_take_off()<0)
drone_err(2);
if(cmd_drone_init_direction()<0)
drone_err(3);
while(!check_destination(share_gps, share_gps_cons))
{ {
usleep(200000); if(read_position(TYPE)<0)
drone_err(1);
cmd_drone_move(-0.2,calcul_trajectory());
sleep(1);
} }
(void) close(socksendfd) ; (void) close(socksendfd) ;
(void) close(sockreceivefd) ; (void) close(sockreceivefd) ;
@ -170,7 +203,7 @@ void traficGPS(int gps) {
//buffer[offset]='\0'; //buffer[offset]='\0';
//printf("phrase x%sx\n",buffer); //printf("phrase x%sx\n",buffer);
// Debut de phrase GPS sans le dollar // Debut de phrase GPS sans le dollar
while (1) { while (run) {
i=0; i=0;
offset=0; offset=0;
do do
@ -264,7 +297,7 @@ void* thread_gps(void* NULL_value)
void* thread_send(void* NULL_value) void* thread_send(void* NULL_value)
{ {
while(1) while(run)
{ {
make_packet() ; make_packet() ;
send_request() ; send_request() ;
@ -297,7 +330,7 @@ void* thread_control(void* NULL_value)
memset(request,0,1500); memset(request,0,1500);
cmd=malloc(256); cmd=malloc(256);
strcpy(cmd,""); strcpy(cmd,"");
while ( strcmp(cmd,"end")!=0) { while (strcmp(cmd,"end")!=0) {
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen); newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
if (newsockfd < 0) if (newsockfd < 0)
perror("ERROR on accept"); perror("ERROR on accept");
@ -338,6 +371,17 @@ void* thread_control(void* NULL_value)
share_t_cons=atof(st); share_t_cons=atof(st);
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); 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 if(strcmp(cmd, "SendStart")==0)
{
start=1;
sprintf(response,"%sStarted;OK;",response);
}
else if(strcmp(cmd, "SendLanding")==0)
{
start=0;
cmd_drone_landing();
sprintf(response,"%sLanding;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));
@ -626,10 +670,9 @@ void* receive_position(void* NULL_value)
// Position of the mobile as computed by the infrastructure: // Position of the mobile as computed by the infrastructure:
char timestampXYZ[128] ; char timestampXYZ[128] ;
char *data_cpy ; char *data_cpy ;
while(1) while(run)
{ {
recvfrom(sockreceivefd, &timestampXYZ, 128, 0, NULL, NULL) ; recvfrom(sockreceivefd, &timestampXYZ, 128, 0, NULL, NULL) ;
//send_labview(timestampXYZ) ; // Use for future
data_cpy = strdup(timestampXYZ) ; data_cpy = strdup(timestampXYZ) ;
string2data(data_cpy) ; string2data(data_cpy) ;
usleep(100000); usleep(100000);
@ -697,7 +740,7 @@ void string2data(char* string_data)
} }
result results[10]; result results[10];
while(1) while(run)
{ {
//Lecture de l'algorythme utilisé //Lecture de l'algorythme utilisé
ptr = strtok(NULL, delims) ; ptr = strtok(NULL, delims) ;
@ -844,6 +887,7 @@ void print_version()
float calcul_trajectory(void) float calcul_trajectory(void)
{ {
int a, b;
float dist_av, dist_ar, dep ; float dist_av, dist_ar, dep ;
float angle ; float angle ;
dist_av = dist_ar = dep = angle = 0 ; dist_av = dist_ar = dep = angle = 0 ;
@ -855,14 +899,28 @@ float calcul_trajectory(void)
angle = acos((pow(dist_ar,2)+pow(dep,2)-pow(dist_av,2))/(2*dist_ar*dep)); angle = acos((pow(dist_ar,2)+pow(dep,2)-pow(dist_av,2))/(2*dist_ar*dep));
angle = angle*180/M_PI; angle = angle*180/M_PI;
return angle; a=share_relatif_cons.x-share_relatif.x;
b=share_relatif_cons.y-share_relatif.y;
/* ********** */
/* a b | R */
/* F F | F */
/* F T | T */
/* T F | T */
/* T T | F */
/* ********** */
if((a<0)!=(b<0))
return angle;
else return -angle;
} }
float calcul_speed(float angle) int check_destination(struct point drone, struct point target)
{ {
float time; if(oc_distance_between(drone, target)<1)
time = angle/180; return 1;
return time; else return 0;
} }
int read_position(char *type) int read_position(char *type)
@ -915,7 +973,7 @@ int cmd_drone_init_direction(void)
if(state_flying==0) if(state_flying==0)
return -1; return -1;
//Implemented socket communication //Implemented socket communication
printf("Calibration...\n"); printf("Initialisation...\n");
speed=-0.5; speed=-0.5;
while(lock==1); while(lock==1);
lock=1; lock=1;
@ -928,12 +986,14 @@ int cmd_drone_init_direction(void)
return 1; return 1;
} }
int cmd_drone_move(float speed_pitch, float speed_yaw) int cmd_drone_move(float speed_pitch, float angle)
{ {
float speed_yaw;
if(state_flying==0) if(state_flying==0)
return -1; return -1;
else else
{ {
speed_yaw=(1/180)*angle;
while(lock==1); while(lock==1);
lock=1; lock=1;
int *floatint_speed_pitch = (int32_t*) &speed_pitch; int *floatint_speed_pitch = (int32_t*) &speed_pitch;
@ -944,8 +1004,8 @@ int cmd_drone_init_direction(void)
(struct sockaddr *)&servcmd,sizeof(servcmd)); (struct sockaddr *)&servcmd,sizeof(servcmd));
} }
lock=0; lock=0;
return 0; return 0;
} }
void drone_err(int err) void drone_err(int err)
@ -969,7 +1029,7 @@ void drone_err(int err)
} }
printf("%s",msg); printf("%s",msg);
exit(0); //drone_handler_sigint(SIGINT);
} }
int drone_socket_create(void) int drone_socket_create(void)
@ -991,9 +1051,9 @@ int drone_socket_close(void)
void* drone_socket_watchdog(void* NULL_value) void* drone_socket_watchdog(void* NULL_value)
{ {
while(1) while(run)
{ {
usleep(50000); usleep(20000);
memset(sendline, 0, sizeof(sendline)); memset(sendline, 0, sizeof(sendline));
if(i==-1) if(i==-1)
{ {
@ -1014,7 +1074,7 @@ void* drone_socket_watchdog(void* NULL_value)
else else
strncat(sendline, LANDING, LENSTR); strncat(sendline, LANDING, LENSTR);
strncat(sendline, "\r", LENSTR); strncat(sendline, "\r", LENSTR);
} }
i++; i++;
if(i>=101) if(i>=101)
{ {
@ -1030,3 +1090,17 @@ void* drone_socket_watchdog(void* NULL_value)
} }
pthread_exit(NULL_value); pthread_exit(NULL_value);
} }
void drone_handler_sigint(const int num)
{
if(num==SIGINT)
{
(void)cmd_drone_landing();
sleep(2);
run=0;
(void) close(socksendfd) ;
(void) close(sockreceivefd) ;
(void) drone_socket_close ;
exit(0);
}
}

View File

@ -22,9 +22,6 @@
/* Program arguments (getopt string) */ /* Program arguments (getopt string) */
#define OPTIONS "d:hi:l::n:p:t:V" #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 */ /* Define M_PI if is not define */
#ifndef M_PI #ifndef M_PI
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846
@ -62,6 +59,7 @@ void* thread_send(void*) ;
void* thread_control(void*) ; void* thread_control(void*) ;
void print_error(char*) ; void print_error(char*) ;
void print_version(void) ; void print_version(void) ;
void drone_handler_sigint(const int);
typedef struct result result; typedef struct result result;
typedef struct gps gps; typedef struct gps gps;
/* Struct */ /* Struct */
@ -107,3 +105,4 @@ int cmd_drone_init_direction(void); //Add socket
//Algo //Algo
float calcul_trajectory(void); float calcul_trajectory(void);
float calcul_speed(float); float calcul_speed(float);
int check_destination(struct point, struct point);