[ARDrone] A bit of beautifying/cleaning
Long lines, trailing white spaces, unused variables, etc. I left most of the commented code, but it certainly should be deleted someday.
This commit is contained in:
Matteo Cypriani
parent
c079ee14ca
commit
5284120632
|
|
@ -70,14 +70,10 @@ owl_bool is_calibration_request = FALSE ;
|
|||
|
||||
int socksendfd ; // Sending socket descriptor (send positioning packets)
|
||||
int sockreceivefd ; // Receiving socket descriptor (Receive position)
|
||||
int socksendlvfd ; // Sending labview socket descriptor (Send position to labview)
|
||||
struct sockaddr_in server ; // Server info
|
||||
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;
|
||||
|
||||
struct point share_gps;
|
||||
struct point share_gps_retro;
|
||||
consigne* list_share_gps_cons = NULL;
|
||||
|
|
@ -161,7 +157,10 @@ int main(int argc, char *argv[])
|
|||
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");
|
||||
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)
|
||||
{
|
||||
i_consigne=0;
|
||||
|
|
@ -175,10 +174,10 @@ int main(int argc, char *argv[])
|
|||
/* - */
|
||||
printf("[GPS] GPS fix OK!\n");
|
||||
speed_pitch=-1;
|
||||
|
||||
|
||||
if(read_position(TYPE)<0)
|
||||
drone_err(1);
|
||||
|
||||
|
||||
if(cmd_drone_take_off()<0)
|
||||
drone_err(2);
|
||||
while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00);
|
||||
|
|
@ -189,26 +188,45 @@ int main(int argc, char *argv[])
|
|||
share_gps_cons.lon = array_cons[i_consigne].lon;
|
||||
share_gps_cons.alt = array_cons[i_consigne].alt;
|
||||
|
||||
distance_total=fabs(oc_distance_between(share_gps, share_gps_cons));
|
||||
|
||||
distance_total =
|
||||
fabs(oc_distance_between(share_gps, share_gps_cons)) ;
|
||||
|
||||
while(start==1&&!check_destination(share_gps, share_gps_cons))
|
||||
{
|
||||
if(read_position(TYPE)<0)
|
||||
drone_err(1);
|
||||
|
||||
|
||||
//calcul_trajectory(&angle, &distance_old);
|
||||
angle = calcul_angle();
|
||||
distance = fabs(oc_distance_between(share_gps, share_gps_cons));
|
||||
distance =
|
||||
fabs(oc_distance_between(share_gps, share_gps_cons)) ;
|
||||
if(speed_pitch<-(distance/distance_total))
|
||||
speed_pitch=-(distance/distance_total)-0.2;
|
||||
|
||||
if(speed_pitch < -1) speed_pitch = -1;
|
||||
else if(speed_pitch > 1) speed_pitch = 1;
|
||||
|
||||
|
||||
cmd_drone_move((speed_pitch), (angle));
|
||||
usleep(10000);
|
||||
printf("Vitesse : %f Angle : %f\n", speed_pitch, angle);
|
||||
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, angle, share_relatif.x, share_relatif.y, share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);
|
||||
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,
|
||||
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);
|
||||
}
|
||||
i_consigne++;
|
||||
|
|
@ -217,9 +235,9 @@ int main(int argc, char *argv[])
|
|||
cmd_drone_landing();
|
||||
}
|
||||
sem_destroy(&lock);
|
||||
(void) close(socksendfd) ;
|
||||
(void) close(sockreceivefd) ;
|
||||
|
||||
close(socksendfd) ;
|
||||
close(sockreceivefd) ;
|
||||
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
|
|
@ -245,18 +263,18 @@ void parse_command_line(int argc, char **argv)
|
|||
void traficGPS(int gps) {
|
||||
int offset=0, c=1, i=0;
|
||||
char buffer[4096];
|
||||
|
||||
do
|
||||
|
||||
do
|
||||
{
|
||||
c=read(gps, buffer, 4096);
|
||||
//printf("lecture intiale %d\n",c);
|
||||
}
|
||||
}
|
||||
while (c>99);
|
||||
do
|
||||
do
|
||||
{
|
||||
c=read(gps, buffer+offset, 1);
|
||||
offset=offset+c;
|
||||
}
|
||||
}
|
||||
while (buffer[offset-c]!='$');
|
||||
//buffer[offset]='\0';
|
||||
//printf("phrase x%sx\n",buffer);
|
||||
|
|
@ -264,11 +282,11 @@ void traficGPS(int gps) {
|
|||
while (run) {
|
||||
i=0;
|
||||
offset=0;
|
||||
do
|
||||
do
|
||||
{
|
||||
c=read(gps, buffer+offset, 1);
|
||||
offset=offset+c;
|
||||
}
|
||||
}
|
||||
while (buffer[offset-c]!='$');
|
||||
//buffer[offset]='\0';
|
||||
//printf("phrase s x%sx\n",buffer);
|
||||
|
|
@ -304,7 +322,8 @@ void traficGPS(int gps) {
|
|||
|
||||
else
|
||||
{
|
||||
memcpy(data[ct_string], buffer+i+offset_line, y-offset_line);
|
||||
memcpy(data[ct_string], buffer + i + offset_line,
|
||||
y - offset_line) ;
|
||||
data[ct_string][y-offset_line]=0;
|
||||
}
|
||||
ct_string++;
|
||||
|
|
@ -314,14 +333,19 @@ void traficGPS(int gps) {
|
|||
}
|
||||
|
||||
if(!(ct_string==0 || offset_line==0))
|
||||
memcpy(data[ct_string], buffer+i+offset_line, length-offset_line);
|
||||
memcpy(data[ct_string], buffer + i + offset_line,
|
||||
length - offset_line) ;
|
||||
data[ct_string][length-offset_line]=0;
|
||||
|
||||
datagps.time = atof(data[1]);
|
||||
datagps.state = data[2][0];
|
||||
datagps.latitude = atof(strndup(data[3],2))+atof(strndup(data[3]+2,strlen(data[3])-2))/60.0 ;
|
||||
datagps.latitude =
|
||||
atof(strndup(data[3], 2)) +
|
||||
atof(strndup(data[3] + 2, strlen(data[3]) - 2)) / 60.0 ;
|
||||
datagps.orilat = data[4][0];
|
||||
datagps.longitude = atof(strndup(data[5],3))+atof(strndup(data[5]+3,strlen(data[5])-3))/60.0 ;
|
||||
datagps.longitude =
|
||||
atof(strndup(data[5], 3)) +
|
||||
atof(strndup(data[5] + 3, strlen(data[5]) - 3)) / 60.0 ;
|
||||
datagps.orilon = data[6][0];
|
||||
datagps.speed = atof(data[7])*1.852;
|
||||
datagps.cap = atof(data[8]);
|
||||
|
|
@ -348,6 +372,7 @@ void traficGPS(int gps) {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void* thread_gps(void* NULL_value)
|
||||
{
|
||||
char buffer[1024];
|
||||
|
|
@ -358,6 +383,7 @@ void* thread_gps(void* NULL_value)
|
|||
pthread_exit(NULL_value);
|
||||
}
|
||||
|
||||
|
||||
void* thread_send(void* NULL_value)
|
||||
{
|
||||
while(run)
|
||||
|
|
@ -369,6 +395,7 @@ void* thread_send(void* NULL_value)
|
|||
pthread_exit(NULL_value);
|
||||
}
|
||||
|
||||
|
||||
// TCP Server OwlSIG
|
||||
void* thread_control(void* NULL_value)
|
||||
{
|
||||
|
|
@ -415,18 +442,30 @@ void* thread_control(void* NULL_value)
|
|||
sz=strtok(NULL,";");
|
||||
st=strtok(NULL,";");
|
||||
|
||||
strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: Apache/ProXad [Aug 5 2010 16:17:11]\nAccess-Control-Allow-Origin: *\nX-Powered-By: PHP/4.4.3-dev\nConnection: close\nContent-Type: text/html\n\n");
|
||||
strcpy(response,
|
||||
"HTTP/1.1 200 OK\n"
|
||||
"Date: Mon, 18 Apr 2011 19:51:52 GMT\n"
|
||||
"Server: Apache/ProXad [Aug 5 2010 16:17:11]\n"
|
||||
"Access-Control-Allow-Origin: *\n"
|
||||
"X-Powered-By: PHP/4.4.3-dev\n"
|
||||
"Connection: close\n"
|
||||
"Content-Type: text/html\n"
|
||||
"\n") ;
|
||||
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_gps.lat,share_gps.lon,share_gps.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_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_gps_cons.lat,share_gps_cons.lon,share_gps_cons.alt,share_t_cons);
|
||||
printf("[Consigne] Latitude %f Lontitude %f défini!\n",share_gps_cons.lat, share_gps_cons.lon);
|
||||
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) ;
|
||||
printf("[Consigne] Latitude %f Lontitude %f défini!\n",
|
||||
share_gps_cons.lat, share_gps_cons.lon) ;
|
||||
array_cons[num_point].lat = share_gps_cons.lat;
|
||||
array_cons[num_point].lon = share_gps_cons.lon;
|
||||
array_cons[num_point].alt = share_gps_cons.alt;
|
||||
|
|
@ -438,7 +477,9 @@ void* thread_control(void* NULL_value)
|
|||
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_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)
|
||||
{
|
||||
|
|
@ -469,6 +510,7 @@ void* thread_control(void* NULL_value)
|
|||
pthread_exit(NULL_value);
|
||||
}
|
||||
|
||||
|
||||
void parse_main_options(int argc, char **argv)
|
||||
{
|
||||
int opt ;
|
||||
|
|
@ -755,6 +797,7 @@ void* receive_position(void* NULL_value)
|
|||
pthread_exit(NULL_value);
|
||||
}
|
||||
|
||||
|
||||
void string2data(char* string_data)
|
||||
{
|
||||
/*Découpage de la chaine de caractère reçu du serveur de positionnement
|
||||
|
|
@ -923,27 +966,38 @@ void string2data(char* string_data)
|
|||
count_algo++;
|
||||
}
|
||||
|
||||
for(count_print=0;count_print<count_algo;count_print++)
|
||||
{
|
||||
printf("\n----------------\n"
|
||||
" Adresse Mac : %s\n"
|
||||
" Type de requete : %d\n"
|
||||
" Timestamp : %ld.%ld\n"
|
||||
" Algo : %s\n"
|
||||
" X : %f\n "
|
||||
" Y : %f\n"
|
||||
" Z : %f\n"
|
||||
"------------------\n", mac, type_req, timestamp.tv_sec, timestamp.tv_nsec, results[count_print].algo, results[count_print].x, results[count_print].y, results[count_print].z);
|
||||
}
|
||||
for (count_print = 0 ; count_print < count_algo ; ++count_print)
|
||||
printf("\n----------------\n"
|
||||
" Adresse Mac : %s\n"
|
||||
" Type de requete : %d\n"
|
||||
" Timestamp : %ld.%ld\n"
|
||||
" Algo : %s\n"
|
||||
" X : %f\n "
|
||||
" Y : %f\n"
|
||||
" Z : %f\n"
|
||||
"------------------\n",
|
||||
mac,
|
||||
type_req,
|
||||
timestamp.tv_sec,
|
||||
timestamp.tv_nsec,
|
||||
results[count_print].algo,
|
||||
results[count_print].x,
|
||||
results[count_print].y,
|
||||
results[count_print].z) ;
|
||||
}
|
||||
|
||||
|
||||
void print_error(char* merror)
|
||||
{
|
||||
if(!strcmp(merror,"trame")) printf("Impossible à lire la trame : Abandon") ;
|
||||
else if (!strcmp(merror,"algo")) printf("Impossible à lire les coordonnées de l'algo : Abandon") ;
|
||||
else printf("Erreur inconnu : Abandon") ;
|
||||
if (! strcmp(merror,"trame"))
|
||||
printf("Impossible de lire la trame : Abandon") ;
|
||||
else if (! strcmp(merror,"algo"))
|
||||
printf("Impossible de lire les coordonnées de l'algo : Abandon") ;
|
||||
else
|
||||
printf("Erreur inconnue : Abandon") ;
|
||||
}
|
||||
|
||||
|
||||
void print_usage()
|
||||
{
|
||||
printf("Usage:\n"
|
||||
|
|
@ -999,8 +1053,8 @@ void print_version()
|
|||
}
|
||||
|
||||
|
||||
/* Commands for drone */
|
||||
|
||||
/* *** Commands for drone *** */
|
||||
|
||||
|
||||
void calcul_trajectory(float *distance, float *angle)
|
||||
|
|
@ -1009,11 +1063,15 @@ void calcul_trajectory(float *distance, float *angle)
|
|||
float dist_av, dist_ar, dep ;
|
||||
(*distance) = 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));
|
||||
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 = acos((pow(dist_ar, 2) + pow(dep, 2) - pow(dist_av, 2)) /
|
||||
(2 * dist_ar * dep)) ;
|
||||
*angle = (*angle)*180/M_PI;
|
||||
*distance = dist_av;
|
||||
|
||||
|
|
@ -1032,6 +1090,7 @@ void calcul_trajectory(float *distance, float *angle)
|
|||
|
||||
}
|
||||
|
||||
|
||||
int check_destination(struct point drone, struct point target)
|
||||
{
|
||||
if(oc_distance_between(drone, target)<3)
|
||||
|
|
@ -1039,6 +1098,7 @@ int check_destination(struct point drone, struct point target)
|
|||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
float calcul_angle()
|
||||
{
|
||||
float captarget = oc_cap(share_gps, share_gps_cons);
|
||||
|
|
@ -1050,13 +1110,15 @@ float calcul_angle()
|
|||
return ((int)(360 - share_gps.cap + (180 - captarget))%360)-180;
|
||||
}
|
||||
|
||||
|
||||
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))
|
||||
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)
|
||||
{
|
||||
|
|
@ -1068,7 +1130,8 @@ int read_position(char *type)
|
|||
|
||||
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);
|
||||
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);
|
||||
|
|
@ -1078,20 +1141,24 @@ int read_position(char *type)
|
|||
return 0;
|
||||
}
|
||||
|
||||
int cmd_drone_take_off(void)
|
||||
|
||||
int cmd_drone_take_off()
|
||||
{
|
||||
printf("Take-off...\n");
|
||||
state_flying=1;
|
||||
return 0;
|
||||
}
|
||||
int cmd_drone_landing(void)
|
||||
|
||||
|
||||
int cmd_drone_landing()
|
||||
{
|
||||
printf("Landing....\n");
|
||||
state_flying=0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int cmd_drone_init_direction(void)
|
||||
|
||||
int cmd_drone_init_direction()
|
||||
{
|
||||
//int boucle;
|
||||
float speed;
|
||||
|
|
@ -1104,7 +1171,7 @@ int cmd_drone_init_direction(void)
|
|||
speed=-0.5;
|
||||
sem_wait(&lock);
|
||||
//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 ;
|
||||
sprintf(sendline,"AT*PCMD=%d,1,0,%d,0,0\r", i, *floatint_speed);
|
||||
|
|
@ -1119,6 +1186,7 @@ int cmd_drone_init_direction(void)
|
|||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int cmd_drone_move(float speed_pitch, int angle)
|
||||
{
|
||||
float speed_yaw;
|
||||
|
|
@ -1130,7 +1198,8 @@ int cmd_drone_move(float speed_pitch, int 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,1,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) ;
|
||||
//printf("%s\n", sendline);
|
||||
i++;
|
||||
sendto(sockcmdfd,sendline,strlen(sendline),0,
|
||||
|
|
@ -1141,6 +1210,7 @@ int cmd_drone_move(float speed_pitch, int angle)
|
|||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void drone_err(int err)
|
||||
{
|
||||
char msg[100];
|
||||
|
|
@ -1148,7 +1218,9 @@ void drone_err(int err)
|
|||
switch(err)
|
||||
{
|
||||
case 1 :
|
||||
strcpy(msg,"Position is not determined or type of localisation is invalid, canceled\n");
|
||||
strcpy(msg,
|
||||
"Undetermined position or invalid localisation type,"
|
||||
" canceled\n") ;
|
||||
break;
|
||||
case 2 :
|
||||
strcpy(msg,"Taking-off failed, canceled\n");
|
||||
|
|
@ -1165,6 +1237,7 @@ void drone_err(int err)
|
|||
//drone_handler_sigint(SIGINT);
|
||||
}
|
||||
|
||||
|
||||
int drone_socket_create(void)
|
||||
{
|
||||
sockcmdfd=socket(AF_INET,SOCK_DGRAM,0);
|
||||
|
|
@ -1177,16 +1250,17 @@ int drone_socket_create(void)
|
|||
return sockcmdfd;
|
||||
}
|
||||
|
||||
|
||||
int drone_socket_close(void)
|
||||
{
|
||||
return close(sockcmdfd);
|
||||
}
|
||||
|
||||
|
||||
void* drone_socket_watchdog(void* NULL_value)
|
||||
{
|
||||
struct timeval start, end;
|
||||
long max, calc;
|
||||
max = 0;
|
||||
|
||||
while(run)
|
||||
{
|
||||
gettimeofday(&start, NULL);
|
||||
|
|
@ -1194,7 +1268,12 @@ void* drone_socket_watchdog(void* NULL_value)
|
|||
memset(sendline, 0, sizeof(sendline));
|
||||
if(i==-1)
|
||||
{
|
||||
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);
|
||||
strncpy(sendline,
|
||||
"AT*CONFIG=1,\"control:outdoor\",\"TRUE\"\r"
|
||||
"AT*CONFIG=2,\"general:navdata_demo\",\"TRUE\"\r"
|
||||
"AT*CONFIG=3,\"control:control_yaw\",\"2.5\"\r"
|
||||
"AT*PMODE=4,2\rAT*MISC=5,2,20,2000,3000\r",
|
||||
LENSTR) ;
|
||||
i=6;
|
||||
}
|
||||
else
|
||||
|
|
@ -1226,56 +1305,63 @@ void* drone_socket_watchdog(void* NULL_value)
|
|||
sem_post(&lock);
|
||||
gettimeofday(&end, NULL);
|
||||
|
||||
calc = ((end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec));
|
||||
//long 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);
|
||||
}
|
||||
|
||||
|
||||
void drone_handler_sigint(const int num)
|
||||
{
|
||||
if(num==SIGINT)
|
||||
{
|
||||
(void)cmd_drone_landing();
|
||||
cmd_drone_landing();
|
||||
sleep(2);
|
||||
run=0;
|
||||
(void) close(socksendfd) ;
|
||||
(void) close(sockreceivefd) ;
|
||||
(void) drone_socket_close ;
|
||||
close(socksendfd) ;
|
||||
close(sockreceivefd) ;
|
||||
drone_socket_close() ;
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
/* Liste chaine */
|
||||
|
||||
consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude)
|
||||
|
||||
/* *** Linked list functions *** */
|
||||
|
||||
|
||||
consigne* list_add_front(consigne* list, double latitude,
|
||||
double longitude, double altitude)
|
||||
{
|
||||
consigne* new_consigne = malloc(sizeof(consigne));
|
||||
|
||||
|
||||
new_consigne->lat = latitude;
|
||||
new_consigne->lon = longitude;
|
||||
new_consigne->alt = altitude;
|
||||
|
||||
new_consigne->next = list;
|
||||
|
||||
|
||||
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));
|
||||
|
||||
|
||||
new_consigne->lat = lat;
|
||||
new_consigne->lon = lon;
|
||||
new_consigne->alt = alt;
|
||||
|
||||
|
||||
new_consigne->next = NULL;
|
||||
|
||||
|
||||
if(list == NULL)
|
||||
{
|
||||
return new_consigne;
|
||||
}
|
||||
return new_consigne ;
|
||||
else
|
||||
{
|
||||
consigne* tmp=list;
|
||||
|
|
|
|||
Loading…
Reference in New Issue