1193 lines
32 KiB
C
1193 lines
32 KiB
C
/*
|
|
* This file is part of the rtap localisation project.
|
|
*/
|
|
|
|
#include "owlps-drone.h"
|
|
|
|
#include <owlps-client.h>
|
|
#include <owlps-resultreader.h>
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <inttypes.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
#include <pthread.h>
|
|
|
|
#include <arpa/inet.h>
|
|
#include <net/if.h>
|
|
|
|
#include <time.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
#include <sys/types.h>
|
|
#include <sys/time.h>
|
|
#include <fcntl.h>
|
|
|
|
#include <signal.h>
|
|
#include <semaphore.h>
|
|
|
|
#include <assert.h>
|
|
|
|
|
|
/* Options */
|
|
|
|
gps datagps;
|
|
point_consigne array_cons[30];
|
|
|
|
int i_consigne=0;
|
|
int num_point=0;
|
|
|
|
struct timespec timestamp ;
|
|
struct {
|
|
char dest_ip[INET_ADDRSTRLEN] ; // Destination IP of the packets
|
|
uint_fast16_t dest_port ;
|
|
char iface[IFNAMSIZ + 1] ; // Source network interface
|
|
int_fast32_t delay ; // Time between two packet transmissions
|
|
uint_fast16_t nb_pkt ; // Number of packets to send
|
|
uint_fast16_t listening_port ;
|
|
// Calibration data:
|
|
owl_direction direction ;
|
|
float x ;
|
|
float y ;
|
|
float z ;
|
|
} options = {
|
|
"",
|
|
OWL_DEFAULT_REQUEST_PORT,
|
|
"",
|
|
-1,
|
|
0,
|
|
0,
|
|
0, 0, 0, 0
|
|
} ;
|
|
|
|
char *program_name = NULL ;
|
|
|
|
// True if the packet is a calibration request, false if it is a simple
|
|
// positioning request:
|
|
owl_bool is_calibration_request = owl_false ;
|
|
|
|
int socksendfd ; // Sending socket descriptor (send positioning packets)
|
|
int sockreceivefd ; // Receiving socket descriptor (Receive position)
|
|
struct sockaddr_in server ; // Server info
|
|
uint8_t *packet = NULL ; // Packet to send
|
|
uint_fast16_t packet_size ; // Packet size
|
|
|
|
struct point share_gps;
|
|
struct point share_gps_retro;
|
|
consigne* list_share_gps_cons = NULL;
|
|
struct point share_gps_cons;
|
|
struct point share_gps_origin;
|
|
struct point_relatif share_relatif;
|
|
struct point_relatif share_relatif_retro;
|
|
struct point_relatif share_relatif_cons;
|
|
|
|
int state_flying=0, i=-1, start=0, run=1;
|
|
sem_t lock;
|
|
/* Socket drone */
|
|
|
|
int sockcmdfd;
|
|
struct sockaddr_in servcmd;
|
|
char sendline[100];
|
|
char count[100];
|
|
|
|
//double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0;
|
|
//double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0;
|
|
double share_t_cons=-999.00;
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
sem_init(&lock, 0, 1);
|
|
FILE* log = NULL;
|
|
float angle, distance, speed_pitch, distance_total;
|
|
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));
|
|
|
|
//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 ;
|
|
pthread_t receive ;
|
|
create_socket() ;
|
|
pthread_create(&send, NULL, thread_send, NULL) ;
|
|
if (options.listening_port > 0)
|
|
pthread_create(&receive, NULL, receive_position, NULL) ;
|
|
}
|
|
else if(!strcmp(TYPE,"GPS"))
|
|
{
|
|
pthread_t gps ;
|
|
pthread_create(&gps, NULL, thread_gps, NULL) ;
|
|
}
|
|
else
|
|
{
|
|
printf("TYPE is not correct, change value of TYPE\n");
|
|
return -1;
|
|
}
|
|
|
|
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)
|
|
{
|
|
i_consigne=0;
|
|
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 File */
|
|
/* - */
|
|
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);
|
|
sleep(8);
|
|
while(i_consigne<=num_point)
|
|
{
|
|
share_gps_cons.lat = array_cons[i_consigne].lat;
|
|
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)) ;
|
|
|
|
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)) ;
|
|
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) ;
|
|
//printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying);
|
|
}
|
|
++i_consigne ;
|
|
}
|
|
fclose(log);
|
|
cmd_drone_landing();
|
|
}
|
|
sem_destroy(&lock);
|
|
close(socksendfd) ;
|
|
close(sockreceivefd) ;
|
|
|
|
return 0 ;
|
|
}
|
|
|
|
|
|
|
|
void parse_command_line(int argc, char **argv)
|
|
{
|
|
parse_main_options(argc, argv) ;
|
|
if(!strcmp(TYPE,"WIFI"))
|
|
{
|
|
check_destination_ip() ;
|
|
parse_calibration_data(argc, argv) ;
|
|
check_configuration() ;
|
|
|
|
|
|
#ifdef DEBUG
|
|
print_configuration() ;
|
|
#endif // DEBUG
|
|
}
|
|
}
|
|
|
|
|
|
void traficGPS(int gps) {
|
|
int offset=0, c=1, i=0;
|
|
char buffer[4096];
|
|
|
|
do
|
|
{
|
|
c=read(gps, buffer, 4096);
|
|
//printf("lecture intiale %d\n",c);
|
|
}
|
|
while (c>99);
|
|
do
|
|
{
|
|
c=read(gps, buffer+offset, 1);
|
|
offset=offset+c;
|
|
}
|
|
while (buffer[offset-c]!='$');
|
|
//buffer[offset]='\0';
|
|
//printf("phrase x%sx\n",buffer);
|
|
// Debut de phrase GPS sans le dollar
|
|
while (run) {
|
|
i=0;
|
|
offset=0;
|
|
do
|
|
{
|
|
c=read(gps, buffer+offset, 1);
|
|
offset=offset+c;
|
|
}
|
|
while (buffer[offset-c]!='$');
|
|
//buffer[offset]='\0';
|
|
//printf("phrase s x%sx\n",buffer);
|
|
if (strncmp(buffer+i,"GPRMC",5)==0)
|
|
{
|
|
char data[15][20];
|
|
int offset_line, length, ct_string, y;
|
|
int z;
|
|
|
|
ct_string = offset_line = y = z = 0 ;
|
|
length = strlen(buffer+i) ;
|
|
memset(data,0,200);
|
|
while(y<=length)
|
|
{
|
|
if((buffer)[y+i]==EOL && z==0) z=1;
|
|
else if((buffer)[y+i]==EOL && z==1)
|
|
{
|
|
length=y-1;
|
|
break;
|
|
}
|
|
++y ;
|
|
}
|
|
//printf("\nTaille finale : %d\n", length);
|
|
|
|
y=0;
|
|
while(y<=length)
|
|
{
|
|
if ((buffer)[y+i]==SEPARATOR)
|
|
{
|
|
|
|
if((y-offset_line)==0)
|
|
memcpy(data[ct_string], "\0", 1);
|
|
|
|
else
|
|
{
|
|
memcpy(data[ct_string], buffer + i + offset_line,
|
|
y - offset_line) ;
|
|
data[ct_string][y-offset_line]=0;
|
|
}
|
|
++ct_string ;
|
|
offset_line=y+1;
|
|
}
|
|
++y ;
|
|
}
|
|
|
|
if(!(ct_string==0 || offset_line==0))
|
|
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.orilat = data[4][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]);
|
|
datagps.date = atoi(data[9]);
|
|
datagps.decmag = atof(data[10]);
|
|
|
|
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
|
|
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
|
|
if(share_gps.lat!=datagps.latitude||share_gps.lon!=datagps.longitude)
|
|
{
|
|
share_gps_retro.lat=share_gps.lat;
|
|
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("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);
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
void* thread_send(void* NULL_value)
|
|
{
|
|
while(run)
|
|
{
|
|
make_packet() ;
|
|
send_request() ;
|
|
sleep(1) ;
|
|
}
|
|
pthread_exit(NULL_value);
|
|
}
|
|
|
|
|
|
// TCP Server OwlSIG
|
|
void* thread_control(void* NULL_value)
|
|
{
|
|
int sockfd, newsockfd, portno=8080;
|
|
socklen_t clilen;
|
|
char request[2048], response[2048], *c;
|
|
struct sockaddr_in serv_addr, cli_addr;
|
|
int n,i=4;
|
|
char *cmd, *sx, *sy, *sz, *st;
|
|
|
|
sockfd = socket(AF_INET, SOCK_STREAM, 0);
|
|
if (sockfd < 0)
|
|
perror("ERROR opening socket TCP");
|
|
bzero((char *) &serv_addr, sizeof(serv_addr));
|
|
serv_addr.sin_family = AF_INET;
|
|
serv_addr.sin_addr.s_addr = INADDR_ANY;
|
|
serv_addr.sin_port = htons(portno);
|
|
if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
|
|
perror("ERROR on binding");
|
|
listen(sockfd,1);
|
|
clilen = sizeof(cli_addr);
|
|
memset(request,0,1500);
|
|
cmd=malloc(256);
|
|
strcpy(cmd,"");
|
|
while (strcmp(cmd,"end")!=0) {
|
|
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
|
|
if (newsockfd < 0)
|
|
perror("ERROR on accept");
|
|
// while ( strcmp(request,"end")!=0) {
|
|
n = read(newsockfd,request,1500);
|
|
//printf("\ntaille %d\n", n);
|
|
fflush(NULL);
|
|
if (n < 0) perror("ERROR reading from socket");
|
|
//printf("%s", request);
|
|
// Add command processing
|
|
c=strtok(request,"=");
|
|
//printf("val 1 %s\n",c);
|
|
c=strtok(NULL,"=");
|
|
//printf("val 2 %s\n",c);
|
|
cmd=strtok(c,";");
|
|
//printf("val 3 %s\n",cmd);
|
|
sx=strtok(NULL,";");
|
|
sy=strtok(NULL,";");
|
|
sz=strtok(NULL,";");
|
|
st=strtok(NULL,";");
|
|
|
|
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) ;
|
|
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) ;
|
|
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;
|
|
++num_point ;
|
|
printf("[Consigne %d point défini]\n", num_point);
|
|
}
|
|
else if (strcmp(cmd, "SendSetpoint")==0) {
|
|
share_relatif_cons.x=atof(sx);
|
|
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) ;
|
|
}
|
|
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 if(strcmp(cmd, "SendEraseTrace")==0)
|
|
{
|
|
num_point=0;
|
|
sprintf(response,"%sSendEraseTrace;OK;",response);
|
|
}
|
|
else
|
|
strcat(response,"Not understand\n");
|
|
n = write(newsockfd,response,strlen(response));
|
|
if (n < 0) perror("ERROR writing to socket");
|
|
printf ("Write : %d %d\n", n, (int) strlen(response));
|
|
close(newsockfd);
|
|
|
|
// }
|
|
}
|
|
close(sockfd);
|
|
pthread_exit(NULL_value);
|
|
}
|
|
|
|
|
|
void parse_main_options(int argc, char **argv)
|
|
{
|
|
int opt ;
|
|
|
|
while ((opt = getopt(argc, argv, OPTIONS)) != -1)
|
|
{
|
|
switch (opt)
|
|
{
|
|
case 'd' :
|
|
strncpy(options.dest_ip, optarg, INET_ADDRSTRLEN) ;
|
|
break ;
|
|
case 'h' :
|
|
print_usage() ;
|
|
exit(0) ;
|
|
case 'i' :
|
|
strncpy(options.iface, optarg, IFNAMSIZ + 1) ;
|
|
break ;
|
|
case 'l' :
|
|
/* Facultative getopt options do not handle separated values
|
|
* (like -l <port>), so we have to test separately.
|
|
*/
|
|
if (optarg) // We got an option like -l<port>, it's OK
|
|
options.listening_port = strtoul(optarg, NULL, 0) ;
|
|
else // We got -l alone or -l <port>
|
|
{
|
|
/* If we are at the end of the string, or the next optind
|
|
* is an option, we have -l without a port number */
|
|
if (argv[optind] == NULL || argv[optind][0] == '-')
|
|
// Take the default value:
|
|
options.listening_port = OWL_DEFAULT_RESULT_PORT ;
|
|
else
|
|
{
|
|
// Take the optind value:
|
|
options.listening_port =
|
|
strtoul(argv[optind], NULL, 0) ;
|
|
++optind ;
|
|
}
|
|
}
|
|
break ;
|
|
case 'n' :
|
|
options.nb_pkt = strtoul(optarg, NULL, 0) ;
|
|
break ;
|
|
case 'p' :
|
|
options.dest_port = strtoul(optarg, NULL, 0) ;
|
|
break ;
|
|
case 't' :
|
|
options.delay = strtol(optarg, NULL, 0) ;
|
|
break ;
|
|
case 'V' :
|
|
print_version() ;
|
|
exit(0) ;
|
|
default :
|
|
print_usage() ;
|
|
exit(ERR_BAD_USAGE) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void check_destination_ip()
|
|
{
|
|
/* Check if we got a destination IP address */
|
|
if (options.dest_ip[0] == '\0')
|
|
{
|
|
fprintf(stderr, "Error! You must specify a destination IP address"
|
|
" (-d).\n") ;
|
|
print_usage() ;
|
|
exit(ERR_BAD_USAGE) ;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void parse_calibration_data(int argc, char **argv)
|
|
{
|
|
/* Parse remaining arguments (possible calibration data) */
|
|
if (argc - optind != 0)
|
|
{
|
|
if (argc - optind == 4)
|
|
{
|
|
is_calibration_request = owl_true ;
|
|
options.direction = strtoul(argv[optind++], NULL, 0) ;
|
|
options.x = strtod(argv[optind++], NULL) ;
|
|
options.y = strtod(argv[optind++], NULL) ;
|
|
options.z = strtod(argv[optind], NULL) ;
|
|
}
|
|
else // Bad number of arguments
|
|
{
|
|
print_usage() ;
|
|
exit(ERR_BAD_USAGE) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void check_configuration()
|
|
{
|
|
// Delay not specified (or bad delay):
|
|
if (options.delay < 0)
|
|
{
|
|
#ifdef DEBUG
|
|
fprintf(stderr,
|
|
"Warning! delay: failing back to default value.\n") ;
|
|
#endif // DEBUG
|
|
if (is_calibration_request)
|
|
options.delay = DEFAULT_DELAY_CALIB ;
|
|
else
|
|
options.delay = DEFAULT_DELAY_NORMAL ;
|
|
}
|
|
|
|
// Number of packet not specified (or bad number)
|
|
if (options.nb_pkt < 1)
|
|
{
|
|
#ifdef DEBUG
|
|
fprintf(stderr,
|
|
"Warning! nb_pkt: failing back to default value.\n") ;
|
|
#endif // DEBUG
|
|
if (is_calibration_request)
|
|
options.nb_pkt = DEFAULT_NBPKT_CALIB ;
|
|
else
|
|
options.nb_pkt = DEFAULT_NBPKT_NORMAL ;
|
|
}
|
|
|
|
// Calibration request but bad direction
|
|
if (is_calibration_request)
|
|
if (options.direction < OWL_DIRECTION_MIN ||
|
|
options.direction > OWL_DIRECTION_MAX)
|
|
{
|
|
fprintf(stderr, "Error! « %"PRIu8" » is not a valid"
|
|
" direction.\n", options.direction) ;
|
|
exit(ERR_BAD_USAGE) ;
|
|
}
|
|
|
|
// Check port numbers
|
|
if (options.dest_port < 1 || options.dest_port > 65535)
|
|
{
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Warning! Bad dest_port:"
|
|
" failing back to default value.\n") ;
|
|
options.dest_port = OWL_DEFAULT_REQUEST_PORT ;
|
|
#endif // DEBUG
|
|
}
|
|
if (options.listening_port > 65535)
|
|
{
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Warning! listening_port too high: ignored.\n") ;
|
|
options.listening_port = 0 ;
|
|
#endif // DEBUG
|
|
}
|
|
|
|
// We want to send a calibration request AND to be located, which is
|
|
// not allowed:
|
|
if (is_calibration_request && options.listening_port > 0)
|
|
{
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Warning! You cannot wait for a server answer when"
|
|
" you calibrate. Option -l ignored…\n") ;
|
|
#endif // DEBUG
|
|
options.listening_port = 0 ;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#ifdef DEBUG
|
|
void print_configuration()
|
|
{
|
|
fprintf(stderr, "Options:\n"
|
|
"\tDestination IP: %s\n"
|
|
"\tDestination port: %"PRIuFAST16"\n"
|
|
"\tInterface: %s\n"
|
|
"\tDelay: %"PRIuFAST32"\n"
|
|
"\tNumber of packets: %"PRIuFAST16"\n"
|
|
"\tListening port: %"PRIuFAST16"\n"
|
|
"\tDirection: %"PRIu8"\n"
|
|
"\tX: %f\n"
|
|
"\tY: %f\n"
|
|
"\tZ: %f\n"
|
|
,
|
|
options.dest_ip,
|
|
options.dest_port,
|
|
options.iface,
|
|
options.delay,
|
|
options.nb_pkt,
|
|
options.listening_port,
|
|
options.direction,
|
|
options.x,
|
|
options.y,
|
|
options.z
|
|
) ;
|
|
}
|
|
#endif // DEBUG
|
|
|
|
|
|
|
|
void create_socket()
|
|
{
|
|
socksendfd =
|
|
owl_create_trx_socket(options.dest_ip, options.dest_port,
|
|
&server, options.iface) ;
|
|
sockreceivefd =
|
|
owl_create_udp_listening_socket(options.listening_port) ;
|
|
}
|
|
|
|
|
|
|
|
/* Creates the packet to send. */
|
|
void make_packet()
|
|
{
|
|
uint_fast16_t offset ; // Index used to create the packet
|
|
owl_timestamp request_time ;
|
|
char request_time_str[OWL_TIMESTAMP_STRLEN] ;
|
|
|
|
// Get the current time and copy it as a string before to switch it to
|
|
// network endianess:
|
|
owl_timestamp_now(&request_time) ;
|
|
owl_timestamp_to_string(request_time_str, request_time) ;
|
|
request_time = owl_hton_timestamp(request_time) ;
|
|
|
|
if (is_calibration_request) // Calibration packet
|
|
{
|
|
printf("Preparing calibration request packet…\n") ;
|
|
|
|
offset = 0 ;
|
|
packet_size =
|
|
sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 ;
|
|
packet = malloc(packet_size) ;
|
|
|
|
memset(&packet[offset], OWL_REQUEST_CALIBRATION, 1) ; // Packet type
|
|
++offset ;
|
|
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
|
offset += sizeof(request_time) ;
|
|
packet[offset++] = options.direction ; // Direction
|
|
#ifdef DEBUG
|
|
printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
|
|
packet[offset - 1], options.x, options.y, options.z) ;
|
|
#endif // DEBUG
|
|
options.x = owl_htonf(options.x) ;
|
|
options.y = owl_htonf(options.y) ;
|
|
options.z = owl_htonf(options.z) ;
|
|
memcpy(&packet[offset], &options.x, sizeof(float)) ;
|
|
offset += sizeof(float) ;
|
|
memcpy(&packet[offset], &options.y, sizeof(float)) ;
|
|
offset += sizeof(float) ;
|
|
memcpy(&packet[offset], &options.z, sizeof(float)) ;
|
|
}
|
|
|
|
else // Standard packet
|
|
{
|
|
printf("Preparing request packet…\n") ;
|
|
packet_size = sizeof(uint8_t) + sizeof(owl_timestamp) ;
|
|
packet = malloc(packet_size) ;
|
|
memset(&packet[0], OWL_REQUEST_NORMAL, 1) ; // Packet type
|
|
memcpy(&packet[1], &request_time, sizeof(request_time)) ;
|
|
}
|
|
|
|
printf("Packet timestamp: %s\n", request_time_str) ;
|
|
}
|
|
|
|
|
|
|
|
void send_request()
|
|
{
|
|
owl_send_request(socksendfd, &server, packet, packet_size,
|
|
options.nb_pkt, options.delay) ;
|
|
}
|
|
|
|
|
|
|
|
void* receive_position(void* NULL_value)
|
|
{
|
|
// Position of the mobile as computed by the infrastructure:
|
|
owl_result *result ;
|
|
|
|
while(run)
|
|
{
|
|
result = owl_receive_position(sockreceivefd) ;
|
|
if (result)
|
|
{
|
|
owl_print_result(result) ;
|
|
owl_free_result(result) ;
|
|
}
|
|
usleep(100000) ;
|
|
}
|
|
|
|
pthread_exit(NULL_value) ;
|
|
}
|
|
|
|
|
|
|
|
void print_error(char* merror)
|
|
{
|
|
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"
|
|
"Localisation request:\n"
|
|
"\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
|
|
" [-n nb_packets] [-l [port]]\n"
|
|
"Calibration request:\n"
|
|
"\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
|
|
" [-n nb_packets] direction x y z\n"
|
|
"\n"
|
|
"Options:\n"
|
|
"\t-h\t\tPrint this help.\n"
|
|
"\t-V\t\tPrint version information.\n"
|
|
"\t-d dest_ip\tDestination IP address of the localisation request.\n"
|
|
"\t-p dest_port\tDestination port of the localisation request"
|
|
" (default: %d).\n"
|
|
"\t-t delay\tTime between each packet transmission (default: %d"
|
|
" µs for a normal request, %d µs for a calibration request).\n"
|
|
"\t-n nb_packets\tNumber of packet transmitted for the request"
|
|
" (default: %d for a normal request, %d for a calibration"
|
|
" request).\n"
|
|
"\t-i iface\tName of the network interface used to transmit the"
|
|
" request (e.g. \"eth2\"). If this option is absent, interface"
|
|
" is selected automatically. You must be root to use this"
|
|
" option.\n"
|
|
"\t-l [port]\tWait for the computed position and display it."
|
|
" Optional argument 'port' allows to specify the listening"
|
|
" port (default: %d).\n"
|
|
,
|
|
program_name,
|
|
program_name,
|
|
OWL_DEFAULT_REQUEST_PORT,
|
|
DEFAULT_DELAY_NORMAL,
|
|
DEFAULT_DELAY_CALIB,
|
|
DEFAULT_NBPKT_NORMAL,
|
|
DEFAULT_NBPKT_CALIB,
|
|
OWL_DEFAULT_RESULT_PORT
|
|
) ;
|
|
}
|
|
|
|
|
|
void print_version()
|
|
{
|
|
printf("This is OwlPS Client for AR.Drone, part of the Open Wireless"
|
|
" Positioning System project.\n"
|
|
"Version: %s.\n",
|
|
#ifdef OWLPS_VERSION
|
|
OWLPS_VERSION
|
|
#else // OWLPS_VERSION
|
|
"unknown version"
|
|
#endif // OWLPS_VERSION:w
|
|
) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Commands for drone *** */
|
|
|
|
|
|
void calcul_trajectory(float *distance, float *angle)
|
|
{
|
|
int a, b;
|
|
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)) ;
|
|
|
|
*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;
|
|
|
|
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*b>=0) (*angle) = -(*angle);
|
|
|
|
}
|
|
|
|
|
|
int check_destination(struct point drone, struct point target)
|
|
{
|
|
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");
|
|
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))
|
|
{
|
|
if(share_gps.lat!=0 && share_gps.lon!=0)
|
|
{
|
|
share_gps_origin.lat = share_gps.lat;
|
|
share_gps_origin.lon = share_gps.lon;
|
|
}
|
|
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) ;
|
|
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);
|
|
|
|
}
|
|
else return -1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
int cmd_drone_take_off()
|
|
{
|
|
printf("Take-off...\n");
|
|
state_flying=1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
int cmd_drone_landing()
|
|
{
|
|
printf("Landing....\n");
|
|
state_flying=0;
|
|
return 0;
|
|
}
|
|
|
|
|
|
int cmd_drone_init_direction()
|
|
{
|
|
//int boucle;
|
|
float speed;
|
|
assert(sizeof(float) == 4) ;
|
|
|
|
if(state_flying==0)
|
|
return -1;
|
|
//Implemented socket communication
|
|
printf("Initialisation...\n");
|
|
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)
|
|
{
|
|
int *floatint_speed = (int32_t*) &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,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, int angle)
|
|
{
|
|
float speed_yaw;
|
|
//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;
|
|
}
|
|
|
|
|
|
void drone_err(int err)
|
|
{
|
|
char msg[100];
|
|
|
|
switch(err)
|
|
{
|
|
case 1 :
|
|
strcpy(msg,
|
|
"Undetermined position or invalid localisation type,"
|
|
" canceled\n") ;
|
|
break;
|
|
case 2 :
|
|
strcpy(msg,"Taking-off failed, canceled\n");
|
|
break;
|
|
case 3 :
|
|
strcpy(msg,"Calibration failed, canceled\n");
|
|
break;
|
|
case 4 :
|
|
strcpy(msg,"Can not move drone, it's not flying, canceled\n");
|
|
break;
|
|
}
|
|
|
|
printf("%s",msg);
|
|
//drone_handler_sigint(SIGINT);
|
|
}
|
|
|
|
|
|
int drone_socket_create(void)
|
|
{
|
|
sockcmdfd=socket(AF_INET,SOCK_DGRAM,0);
|
|
|
|
bzero(&servcmd,sizeof(servcmd));
|
|
servcmd.sin_family = AF_INET;
|
|
servcmd.sin_addr.s_addr=inet_addr(IP);
|
|
servcmd.sin_port=htons(PORT);
|
|
|
|
return sockcmdfd;
|
|
}
|
|
|
|
|
|
int drone_socket_close(void)
|
|
{
|
|
return close(sockcmdfd);
|
|
}
|
|
|
|
|
|
void* drone_socket_watchdog(void* NULL_value)
|
|
{
|
|
struct timeval start, end;
|
|
|
|
while(run)
|
|
{
|
|
gettimeofday(&start, NULL);
|
|
usleep(8000);
|
|
memset(sendline, 0, sizeof(sendline));
|
|
if(i==-1)
|
|
{
|
|
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
|
|
{
|
|
sem_wait(&lock);
|
|
snprintf(count, 100, "%d", i);
|
|
strncat(sendline, ATREF, LENSTR);
|
|
strncat(sendline, count, LENSTR);
|
|
if(state_flying==0)
|
|
strncat(sendline, LANDING, LENSTR);
|
|
else if(state_flying==1)
|
|
strncat(sendline, TAKEOFF, LENSTR);
|
|
else
|
|
strncat(sendline, LANDING, LENSTR);
|
|
strncat(sendline, "\r", LENSTR);
|
|
}
|
|
++i ;
|
|
if(i>=101)
|
|
{
|
|
strncat(sendline, "AT*COMWDG=", LENSTR);
|
|
snprintf(count, 100, "%d", i);
|
|
strncat(sendline, count, LENSTR);
|
|
strncat(sendline, "\r", LENSTR);
|
|
i=1;
|
|
}
|
|
sendto(sockcmdfd,sendline,strlen(sendline),0,
|
|
(struct sockaddr *)&servcmd,sizeof(servcmd));
|
|
//printf("[WATCHDOG] %s\n", sendline);
|
|
sem_post(&lock);
|
|
gettimeofday(&end, NULL);
|
|
|
|
//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)
|
|
{
|
|
cmd_drone_landing();
|
|
sleep(2);
|
|
run=0;
|
|
close(socksendfd) ;
|
|
close(sockreceivefd) ;
|
|
drone_socket_close() ;
|
|
exit(0);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* *** 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* 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 ;
|
|
else
|
|
{
|
|
consigne* tmp=list;
|
|
while(tmp->next != NULL)
|
|
{
|
|
tmp = tmp->next;
|
|
}
|
|
tmp->next = new_consigne;
|
|
return list;
|
|
}
|
|
}
|