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

[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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,11 +21,12 @@
#include <assert.h>
#include <signal.h>
#define SEPARATOR ','
#define EOL '$'
#define TYPE "GPS"
/* Options */
gps datagps;
@ -77,7 +78,7 @@ 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;
int state_flying=0, lock=0, i=-1, start=0, run=1;
/* Socket drone */
@ -92,9 +93,25 @@ double share_t_cons=-999.00;
int main(int argc, char *argv[])
{
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;
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
pthread_t control ;
pthread_create(&control, NULL, thread_control, NULL) ;
@ -119,13 +136,29 @@ int main(int argc, char *argv[])
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(sockreceivefd) ;
@ -170,7 +203,7 @@ void traficGPS(int gps) {
//buffer[offset]='\0';
//printf("phrase x%sx\n",buffer);
// Debut de phrase GPS sans le dollar
while (1) {
while (run) {
i=0;
offset=0;
do
@ -264,7 +297,7 @@ void* thread_gps(void* NULL_value)
void* thread_send(void* NULL_value)
{
while(1)
while(run)
{
make_packet() ;
send_request() ;
@ -297,7 +330,7 @@ void* thread_control(void* NULL_value)
memset(request,0,1500);
cmd=malloc(256);
strcpy(cmd,"");
while ( strcmp(cmd,"end")!=0) {
while (strcmp(cmd,"end")!=0) {
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
if (newsockfd < 0)
perror("ERROR on accept");
@ -338,6 +371,17 @@ void* thread_control(void* NULL_value)
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);
}
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
strcat(response,"Not understand\n");
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:
char timestampXYZ[128] ;
char *data_cpy ;
while(1)
while(run)
{
recvfrom(sockreceivefd, &timestampXYZ, 128, 0, NULL, NULL) ;
//send_labview(timestampXYZ) ; // Use for future
data_cpy = strdup(timestampXYZ) ;
string2data(data_cpy) ;
usleep(100000);
@ -697,7 +740,7 @@ void string2data(char* string_data)
}
result results[10];
while(1)
while(run)
{
//Lecture de l'algorythme utilisé
ptr = strtok(NULL, delims) ;
@ -844,6 +887,7 @@ void print_version()
float calcul_trajectory(void)
{
int a, b;
float dist_av, dist_ar, dep ;
float angle ;
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 = 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;
time = angle/180;
return time;
if(oc_distance_between(drone, target)<1)
return 1;
else return 0;
}
int read_position(char *type)
@ -915,7 +973,7 @@ int cmd_drone_init_direction(void)
if(state_flying==0)
return -1;
//Implemented socket communication
printf("Calibration...\n");
printf("Initialisation...\n");
speed=-0.5;
while(lock==1);
lock=1;
@ -928,12 +986,14 @@ int cmd_drone_init_direction(void)
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)
return -1;
else
{
speed_yaw=(1/180)*angle;
while(lock==1);
lock=1;
int *floatint_speed_pitch = (int32_t*) &speed_pitch;
@ -944,8 +1004,8 @@ int cmd_drone_init_direction(void)
(struct sockaddr *)&servcmd,sizeof(servcmd));
}
lock=0;
return 0;
lock=0;
return 0;
}
void drone_err(int err)
@ -969,7 +1029,7 @@ void drone_err(int err)
}
printf("%s",msg);
exit(0);
//drone_handler_sigint(SIGINT);
}
int drone_socket_create(void)
@ -991,9 +1051,9 @@ int drone_socket_close(void)
void* drone_socket_watchdog(void* NULL_value)
{
while(1)
while(run)
{
usleep(50000);
usleep(20000);
memset(sendline, 0, sizeof(sendline));
if(i==-1)
{
@ -1014,7 +1074,7 @@ void* drone_socket_watchdog(void* NULL_value)
else
strncat(sendline, LANDING, LENSTR);
strncat(sendline, "\r", LENSTR);
}
}
i++;
if(i>=101)
{
@ -1030,3 +1090,17 @@ void* drone_socket_watchdog(void* 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);
}
}

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

@ -22,9 +22,6 @@
/* 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
@ -62,6 +59,7 @@ void* thread_send(void*) ;
void* thread_control(void*) ;
void print_error(char*) ;
void print_version(void) ;
void drone_handler_sigint(const int);
typedef struct result result;
typedef struct gps gps;
/* Struct */
@ -107,3 +105,4 @@ int cmd_drone_init_direction(void); //Add socket
//Algo
float calcul_trajectory(void);
float calcul_speed(float);
int check_destination(struct point, struct point);