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

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

@ -110,13 +110,13 @@ 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_move(float, int);
int cmd_drone_init_direction(void); //Add socket
//Algo
void calcul_trajectory(float*, float*);
float calcul_speed(float);
int check_destination(struct point, struct point);
float calcul_angle(void);
//Liste chaine
consigne* list_add_front(consigne*, double, double, double);
consigne* list_add_back(consigne*, double, double, double);