2011-04-01 10:02:05 +02:00
/*
* This file is part of the rtap localisation project .
*/
2011-04-05 17:23:28 +02:00
# include "owlps-drone.h"
2011-04-04 16:41:52 +02:00
2011-08-18 10:00:45 +02:00
# include <owlps-client.h>
2011-12-01 17:53:37 +01:00
# include <owlps-resultreader.h>
2011-08-18 10:00:45 +02:00
2011-04-04 16:41:52 +02:00
# include <stdio.h>
# include <stdlib.h>
# include <unistd.h>
# include <inttypes.h>
# include <string.h>
2011-08-18 10:00:45 +02:00
# include <math.h>
# include <pthread.h>
2011-04-04 16:41:52 +02:00
2011-08-18 10:00:45 +02:00
# include <arpa/inet.h>
2011-04-04 16:41:52 +02:00
# include <net/if.h>
2011-04-01 10:02:05 +02:00
# include <time.h>
2011-04-04 16:41:52 +02:00
2011-04-14 10:58:59 +02:00
# include <sys/ioctl.h>
# include <sys/types.h>
2011-05-31 14:29:14 +02:00
# include <sys/time.h>
2011-04-14 10:58:59 +02:00
# include <fcntl.h>
2011-05-04 17:17:30 +02:00
# include <signal.h>
2011-05-18 14:28:54 +02:00
# include <semaphore.h>
2011-08-18 10:00:45 +02:00
# include <assert.h>
2011-05-03 14:34:35 +02:00
2011-04-01 10:02:05 +02:00
/* Options */
2011-04-14 10:58:59 +02:00
gps datagps ;
2011-06-14 14:04:50 +02:00
point_consigne array_cons [ 30 ] ;
int i_consigne = 0 ;
int num_point = 0 ;
2011-04-14 10:58:59 +02:00
2011-04-01 10:02:05 +02:00
struct timespec timestamp ;
struct {
2011-04-04 16:41:52 +02:00
char dest_ip [ INET_ADDRSTRLEN ] ; // Destination IP of the packets
2011-04-01 10:02:05 +02:00
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:
2011-04-04 16:41:52 +02:00
owl_direction direction ;
2011-04-01 10:02:05 +02:00
float x ;
float y ;
float z ;
} options = {
" " ,
2011-12-01 16:29:12 +01:00
OWL_DEFAULT_REQUEST_PORT ,
2011-04-01 10:02:05 +02:00
" " ,
- 1 ,
0 ,
0 ,
0 , 0 , 0 , 0
} ;
char * program_name = NULL ;
2011-12-01 16:29:12 +01:00
// True if the packet is a calibration request, false if it is a simple
2011-04-01 10:02:05 +02:00
// positioning request:
2011-12-01 16:29:12 +01:00
owl_bool is_calibration_request = owl_false ;
2011-04-01 10:02:05 +02:00
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
2011-05-03 17:25:59 +02:00
struct point share_gps ;
struct point share_gps_retro ;
2011-05-18 14:28:54 +02:00
consigne * list_share_gps_cons = NULL ;
2011-05-03 17:25:59 +02:00
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 ;
2011-05-18 14:28:54 +02:00
int state_flying = 0 , i = - 1 , start = 0 , run = 1 ;
sem_t lock ;
2011-05-03 17:25:59 +02:00
/* 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 ;
2011-04-01 10:02:05 +02:00
int main ( int argc , char * argv [ ] )
{
2011-05-18 14:28:54 +02:00
sem_init ( & lock , 0 , 1 ) ;
2011-05-09 13:54:34 +02:00
FILE * log = NULL ;
2011-05-05 14:35:48 +02:00
float angle , distance , speed_pitch , distance_total ;
2011-05-04 17:17:30 +02:00
struct sigaction action ;
2011-04-01 10:02:05 +02:00
program_name = argv [ 0 ] ;
parse_command_line ( argc , argv ) ;
2011-05-04 17:17:30 +02:00
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 ) ) ;
2011-05-31 14:29:14 +02:00
//Set unknow value gps
2011-05-18 14:28:54 +02:00
share_gps_retro . lat = - 999.00 ;
share_gps_retro . lon = - 999.00 ;
2011-05-31 14:29:14 +02:00
share_gps_cons . lat = - 999.00 ;
share_gps_cons . lon = - 999.00 ;
2011-04-14 10:58:59 +02:00
// Thread communication TCP OwlSIG
pthread_t control ;
pthread_create ( & control , NULL , thread_control , NULL ) ;
2011-05-03 14:34:35 +02:00
2011-05-31 14:29:14 +02:00
//Launch thread or wifi depend of TYPE
2011-05-03 14:34:35 +02:00
if ( ! strcmp ( TYPE , " WIFI " ) )
2011-04-13 11:22:04 +02:00
{
2011-05-03 14:34:35 +02:00
pthread_t send ;
pthread_t receive ;
create_socket ( ) ;
pthread_create ( & send , NULL , thread_send , NULL ) ;
2011-04-01 10:02:05 +02:00
if ( options . listening_port > 0 )
2011-05-03 14:34:35 +02:00
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 ;
2011-04-01 10:02:05 +02:00
}
2011-05-03 14:34:35 +02:00
2011-05-31 14:29:14 +02:00
drone_socket_create ( ) ;
pthread_t drone_watchdog ;
pthread_create ( & drone_watchdog , NULL , drone_socket_watchdog , NULL ) ;
log = fopen ( " vol.log " , " w " ) ;
2011-08-18 10:51:50 +02:00
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 " ) ;
2011-05-09 13:54:34 +02:00
while ( run )
{
2011-06-14 14:04:50 +02:00
i_consigne = 0 ;
2011-05-09 13:54:34 +02:00
while ( start = = 0 ) usleep ( 200000 ) ;
2011-05-31 14:29:14 +02:00
//Wait GPS Signal
2011-05-18 14:28:54 +02:00
if ( share_gps . lat = = 0 | | share_gps . lon = = 0 )
printf ( " [GPS] GPS not fix! Wait... \n " ) ;
2011-05-09 13:54:34 +02:00
while ( share_gps . lat = = 0 | | share_gps . lon = = 0 ) ;
2011-05-31 14:29:14 +02:00
/* Log File */
/* - */
2011-05-09 13:54:34 +02:00
printf ( " [GPS] GPS fix OK! \n " ) ;
speed_pitch = - 1 ;
2011-08-18 10:51:50 +02:00
2011-05-09 13:54:34 +02:00
if ( read_position ( TYPE ) < 0 )
drone_err ( 1 ) ;
2011-08-18 10:51:50 +02:00
2011-05-09 13:54:34 +02:00
if ( cmd_drone_take_off ( ) < 0 )
drone_err ( 2 ) ;
2011-05-31 14:29:14 +02:00
while ( share_gps_cons . lat = = - 999.00 | | share_gps_cons . lon = = - 999.00 ) ;
sleep ( 8 ) ;
2011-06-14 14:04:50 +02:00
while ( i_consigne < = num_point )
2011-05-09 13:54:34 +02:00
{
2011-06-14 14:04:50 +02:00
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 ;
2011-08-18 10:51:50 +02:00
distance_total =
fabs ( oc_distance_between ( share_gps , share_gps_cons ) ) ;
2011-06-14 14:04:50 +02:00
while ( start = = 1 & & ! check_destination ( share_gps , share_gps_cons ) )
{
if ( read_position ( TYPE ) < 0 )
drone_err ( 1 ) ;
2011-08-18 10:51:50 +02:00
2011-06-14 14:04:50 +02:00
//calcul_trajectory(&angle, &distance_old);
angle = calcul_angle ( ) ;
2011-08-18 10:51:50 +02:00
distance =
fabs ( oc_distance_between ( share_gps , share_gps_cons ) ) ;
2011-06-14 14:04:50 +02:00
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 ;
2011-08-18 10:51:50 +02:00
2011-06-14 14:04:50 +02:00
cmd_drone_move ( ( speed_pitch ) , ( angle ) ) ;
usleep ( 10000 ) ;
printf ( " Vitesse : %f Angle : %f \n " , speed_pitch , angle ) ;
2011-08-18 10:51:50 +02:00
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 ) ;
2011-06-14 14:04:50 +02:00
//printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying);
}
2011-08-18 11:50:55 +02:00
+ + i_consigne ;
2011-05-09 13:54:34 +02:00
}
fclose ( log ) ;
cmd_drone_landing ( ) ;
2011-05-04 17:17:30 +02:00
}
2011-05-18 14:28:54 +02:00
sem_destroy ( & lock ) ;
2011-08-18 10:51:50 +02:00
close ( socksendfd ) ;
close ( sockreceivefd ) ;
2011-04-01 10:02:05 +02:00
return 0 ;
}
void parse_command_line ( int argc , char * * argv )
{
parse_main_options ( argc , argv ) ;
2011-05-03 14:34:35 +02:00
if ( ! strcmp ( TYPE , " WIFI " ) )
{
check_destination_ip ( ) ;
parse_calibration_data ( argc , argv ) ;
check_configuration ( ) ;
2011-04-01 10:02:05 +02:00
# ifdef DEBUG
2011-05-03 14:34:35 +02:00
print_configuration ( ) ;
2011-04-01 10:02:05 +02:00
# endif // DEBUG
2011-05-03 14:34:35 +02:00
}
2011-04-01 10:02:05 +02:00
}
2011-04-14 10:58:59 +02:00
2011-05-03 14:34:35 +02:00
void traficGPS ( int gps ) {
int offset = 0 , c = 1 , i = 0 ;
char buffer [ 4096 ] ;
2011-08-18 10:51:50 +02:00
do
2011-05-03 13:55:57 +02:00
{
c = read ( gps , buffer , 4096 ) ;
2011-05-31 14:29:14 +02:00
//printf("lecture intiale %d\n",c);
2011-08-18 10:51:50 +02:00
}
2011-05-03 13:55:57 +02:00
while ( c > 99 ) ;
2011-08-18 10:51:50 +02:00
do
2011-05-03 13:55:57 +02:00
{
c = read ( gps , buffer + offset , 1 ) ;
offset = offset + c ;
2011-08-18 10:51:50 +02:00
}
2011-05-03 13:55:57 +02:00
while ( buffer [ offset - c ] ! = ' $ ' ) ;
2011-04-14 10:58:59 +02:00
//buffer[offset]='\0';
//printf("phrase x%sx\n",buffer);
// Debut de phrase GPS sans le dollar
2011-05-04 17:17:30 +02:00
while ( run ) {
2011-04-14 10:58:59 +02:00
i = 0 ;
offset = 0 ;
2011-08-18 10:51:50 +02:00
do
2011-05-03 13:55:57 +02:00
{
c = read ( gps , buffer + offset , 1 ) ;
offset = offset + c ;
2011-08-18 10:51:50 +02:00
}
2011-05-03 13:55:57 +02:00
while ( buffer [ offset - c ] ! = ' $ ' ) ;
2011-04-14 10:58:59 +02:00
//buffer[offset]='\0';
//printf("phrase s x%sx\n",buffer);
if ( strncmp ( buffer + i , " GPRMC " , 5 ) = = 0 )
2011-05-03 13:55:57 +02:00
{
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 ;
}
2011-08-18 11:50:55 +02:00
+ + y ;
2011-05-03 13:55:57 +02:00
}
2011-05-31 14:29:14 +02:00
//printf("\nTaille finale : %d\n", length);
2011-05-03 13:55:57 +02:00
y = 0 ;
while ( y < = length )
{
if ( ( buffer ) [ y + i ] = = SEPARATOR )
{
if ( ( y - offset_line ) = = 0 )
memcpy ( data [ ct_string ] , " \0 " , 1 ) ;
else
{
2011-08-18 10:51:50 +02:00
memcpy ( data [ ct_string ] , buffer + i + offset_line ,
y - offset_line ) ;
2011-05-03 13:55:57 +02:00
data [ ct_string ] [ y - offset_line ] = 0 ;
}
2011-08-18 11:50:55 +02:00
+ + ct_string ;
2011-05-03 13:55:57 +02:00
offset_line = y + 1 ;
}
2011-08-18 11:50:55 +02:00
+ + y ;
2011-05-03 13:55:57 +02:00
}
if ( ! ( ct_string = = 0 | | offset_line = = 0 ) )
2011-08-18 10:51:50 +02:00
memcpy ( data [ ct_string ] , buffer + i + offset_line ,
length - offset_line ) ;
2011-05-03 13:55:57 +02:00
data [ ct_string ] [ length - offset_line ] = 0 ;
datagps . time = atof ( data [ 1 ] ) ;
datagps . state = data [ 2 ] [ 0 ] ;
2011-08-18 10:51:50 +02:00
datagps . latitude =
atof ( strndup ( data [ 3 ] , 2 ) ) +
atof ( strndup ( data [ 3 ] + 2 , strlen ( data [ 3 ] ) - 2 ) ) / 60.0 ;
2011-05-03 13:55:57 +02:00
datagps . orilat = data [ 4 ] [ 0 ] ;
2011-08-18 10:51:50 +02:00
datagps . longitude =
atof ( strndup ( data [ 5 ] , 3 ) ) +
atof ( strndup ( data [ 5 ] + 3 , strlen ( data [ 5 ] ) - 3 ) ) / 60.0 ;
2011-05-03 13:55:57 +02:00
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 ;
2011-05-18 14:28:54 +02:00
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 ;
2011-05-31 14:29:14 +02:00
share_gps . cap = datagps . cap ;
2011-05-18 14:28:54 +02:00
}
2011-05-09 13:54:34 +02:00
share_gps . date = datagps . date ;
share_gps . time = datagps . time ;
2011-05-31 14:29:14 +02:00
//printf("lat %s lon %s\n",data[3], data[5]);
2011-05-03 17:25:59 +02:00
2011-05-09 13:54:34 +02:00
//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);
2011-05-03 17:25:59 +02:00
2011-05-03 13:55:57 +02:00
}
2011-04-14 10:58:59 +02:00
}
}
2011-08-18 10:51:50 +02:00
2011-05-03 14:34:35 +02:00
void * thread_gps ( void * NULL_value )
2011-04-14 10:58:59 +02:00
{
char buffer [ 1024 ] ;
int gps_fd = open ( " /dev/ttyPA0 " , O_RDONLY ) ;
read ( gps_fd , buffer , 80 ) ;
traficGPS ( gps_fd ) ;
2011-05-03 14:34:35 +02:00
pthread_exit ( NULL_value ) ;
2011-04-14 10:58:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-05-03 14:34:35 +02:00
void * thread_send ( void * NULL_value )
2011-04-01 10:02:05 +02:00
{
2011-05-04 17:17:30 +02:00
while ( run )
2011-04-01 10:02:05 +02:00
{
make_packet ( ) ;
send_request ( ) ;
2011-05-03 13:55:57 +02:00
sleep ( 1 ) ;
2011-04-01 10:02:05 +02:00
}
2011-05-03 14:34:35 +02:00
pthread_exit ( NULL_value ) ;
2011-04-01 10:02:05 +02:00
}
2011-08-18 10:51:50 +02:00
2011-04-14 10:58:59 +02:00
// TCP Server OwlSIG
2011-05-03 14:34:35 +02:00
void * thread_control ( void * NULL_value )
2011-04-14 10:58:59 +02:00
{
int sockfd , newsockfd , portno = 8080 ;
socklen_t clilen ;
2011-05-03 13:55:57 +02:00
char request [ 2048 ] , response [ 2048 ] , * c ;
2011-04-14 10:58:59 +02:00
struct sockaddr_in serv_addr , cli_addr ;
2011-05-03 13:55:57 +02:00
int n , i = 4 ;
char * cmd , * sx , * sy , * sz , * st ;
2011-04-14 10:58:59 +02:00
sockfd = socket ( AF_INET , SOCK_STREAM , 0 ) ;
if ( sockfd < 0 )
2011-05-03 13:55:57 +02:00
perror ( " ERROR opening socket TCP " ) ;
2011-04-14 10:58:59 +02:00
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 )
2011-05-03 13:55:57 +02:00
perror ( " ERROR on binding " ) ;
2011-04-14 10:58:59 +02:00
listen ( sockfd , 1 ) ;
clilen = sizeof ( cli_addr ) ;
2011-05-03 13:55:57 +02:00
memset ( request , 0 , 1500 ) ;
cmd = malloc ( 256 ) ;
strcpy ( cmd , " " ) ;
2011-05-04 17:17:30 +02:00
while ( strcmp ( cmd , " end " ) ! = 0 ) {
2011-05-03 13:55:57 +02:00
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 ) ;
2011-05-09 13:54:34 +02:00
//printf("\ntaille %d\n", n);
2011-05-03 13:55:57 +02:00
fflush ( NULL ) ;
if ( n < 0 ) perror ( " ERROR reading from socket " ) ;
2011-05-09 13:54:34 +02:00
//printf("%s", request);
2011-05-03 13:55:57 +02:00
// Add command processing
c = strtok ( request , " = " ) ;
2011-05-09 13:54:34 +02:00
//printf("val 1 %s\n",c);
2011-05-03 13:55:57 +02:00
c = strtok ( NULL , " = " ) ;
2011-05-09 13:54:34 +02:00
//printf("val 2 %s\n",c);
2011-05-03 13:55:57 +02:00
cmd = strtok ( c , " ; " ) ;
2011-05-09 13:54:34 +02:00
//printf("val 3 %s\n",cmd);
2011-05-03 13:55:57 +02:00
sx = strtok ( NULL , " ; " ) ;
sy = strtok ( NULL , " ; " ) ;
sz = strtok ( NULL , " ; " ) ;
st = strtok ( NULL , " ; " ) ;
2011-08-18 10:51:50 +02:00
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 " ) ;
2011-05-03 13:55:57 +02:00
if ( strcmp ( cmd , " ReadPosition " ) = = 0 )
sprintf ( response , " %sPosition;OK;%d.0;5.0;6.1 \n " , response , i + + ) ;
else if ( strcmp ( cmd , " ReadGeoPosition " ) = = 0 )
2011-08-18 10:51:50 +02:00
sprintf ( response , " %sGeoPosition;OK;%f;%f;%f \n " ,
response , share_gps . lat , share_gps . lon , share_gps . alt ) ;
2011-05-03 13:55:57 +02:00
else if ( strcmp ( cmd , " SendGeoSetpoint " ) = = 0 ) {
2011-05-03 17:25:59 +02:00
share_gps_cons . lat = atof ( sx ) ;
share_gps_cons . lon = atof ( sy ) ;
share_gps_cons . alt = atof ( sz ) ;
2011-05-03 13:55:57 +02:00
share_t_cons = atof ( st ) ;
2011-08-18 10:51:50 +02:00
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 ) ;
2011-06-14 14:04:50 +02:00
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 ;
2011-08-18 11:50:55 +02:00
+ + num_point ;
2011-06-14 14:04:50 +02:00
printf ( " [Consigne %d point défini] \n " , num_point ) ;
2011-05-03 13:55:57 +02:00
}
else if ( strcmp ( cmd , " SendSetpoint " ) = = 0 ) {
2011-05-03 17:25:59 +02:00
share_relatif_cons . x = atof ( sx ) ;
share_relatif_cons . y = atof ( sy ) ;
share_relatif_cons . z = atof ( sz ) ;
2011-05-03 13:55:57 +02:00
share_t_cons = atof ( st ) ;
2011-08-18 10:51:50 +02:00
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 ) ;
2011-04-14 10:58:59 +02:00
}
2011-05-04 17:17:30 +02:00
else if ( strcmp ( cmd , " SendStart " ) = = 0 )
{
start = 1 ;
sprintf ( response , " %sStarted;OK; " , response ) ;
}
else if ( strcmp ( cmd , " SendLanding " ) = = 0 )
{
start = 0 ;
2011-05-18 14:28:54 +02:00
cmd_drone_landing ( ) ;
2011-05-04 17:17:30 +02:00
sprintf ( response , " %sLanding;OK; " , response ) ;
}
2011-06-14 14:04:50 +02:00
else if ( strcmp ( cmd , " SendEraseTrace " ) = = 0 )
2011-05-31 14:29:14 +02:00
{
num_point = 0 ;
2011-06-14 14:04:50 +02:00
sprintf ( response , " %sSendEraseTrace;OK; " , response ) ;
2011-05-31 14:29:14 +02:00
}
2011-05-03 13:55:57 +02:00
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 ) ;
2011-05-03 14:34:35 +02:00
pthread_exit ( NULL_value ) ;
2011-04-14 10:58:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-04-01 10:02:05 +02:00
void parse_main_options ( int argc , char * * argv )
{
int opt ;
while ( ( opt = getopt ( argc , argv , OPTIONS ) ) ! = - 1 )
{
switch ( opt )
{
case ' d ' :
2011-04-04 16:41:52 +02:00
strncpy ( options . dest_ip , optarg , INET_ADDRSTRLEN ) ;
2011-04-01 10:02:05 +02:00
break ;
case ' h ' :
print_usage ( ) ;
exit ( 0 ) ;
case ' i ' :
strncpy ( options . iface , optarg , IFNAMSIZ + 1 ) ;
break ;
case ' l ' :
2011-07-25 14:21:27 +02:00
/* 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>
2011-04-01 10:02:05 +02:00
{
/* 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:
2011-12-01 16:29:12 +01:00
options . listening_port = OWL_DEFAULT_RESULT_PORT ;
2011-04-01 10:02:05 +02:00
else
{
// Take the optind value:
options . listening_port =
strtoul ( argv [ optind ] , NULL , 0 ) ;
2011-08-18 11:50:55 +02:00
+ + optind ;
2011-04-01 10:02:05 +02:00
}
2011-08-18 10:43:38 +02:00
}
break ;
2011-04-01 10:02:05 +02:00
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 ;
2011-04-13 11:12:08 +02:00
case ' V ' :
print_version ( ) ;
exit ( 0 ) ;
2011-04-01 10:02:05 +02:00
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 )
{
2011-12-01 16:29:12 +01:00
is_calibration_request = owl_true ;
2011-04-01 10:02:05 +02:00
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 ;
}
2011-04-13 11:12:08 +02:00
2011-04-04 16:41:52 +02:00
// 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 ) ;
}
2011-04-01 10:02:05 +02:00
// 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 " ) ;
2011-12-01 16:29:12 +01:00
options . dest_port = OWL_DEFAULT_REQUEST_PORT ;
2011-04-01 10:02:05 +02:00
# 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 " ) ;
2011-04-13 11:22:04 +02:00
# endif // DEBUG
2011-04-01 10:02:05 +02:00
options . listening_port = 0 ;
}
}
# ifdef DEBUG
void print_configuration ( )
{
fprintf ( stderr , " Options: \n "
" \t Destination IP: %s \n "
" \t Destination port: % " PRIuFAST16 " \n "
" \t Interface: %s \n "
" \t Delay: % " PRIuFAST32 " \n "
" \t Number of packets: % " PRIuFAST16 " \n "
" \t Listening port: % " PRIuFAST16 " \n "
2011-04-13 11:12:08 +02:00
" \t Direction: % " PRIu8 " \n "
2011-04-01 10:02:05 +02:00
" \t X: %f \n "
" \t Y: %f \n "
" \t Z: %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 =
2011-12-01 16:29:12 +01:00
owl_create_trx_socket ( options . dest_ip , options . dest_port ,
& server , options . iface ) ;
2011-04-13 11:22:04 +02:00
sockreceivefd =
2011-04-01 10:02:05 +02:00
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
2011-04-04 16:41:52 +02:00
owl_timestamp request_time ;
2011-12-01 16:29:12 +01:00
char request_time_str [ OWL_TIMESTAMP_STRLEN ] ;
2011-04-01 10:02:05 +02:00
// 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 =
2011-04-04 16:41:52 +02:00
sizeof ( uint8_t ) * 2 + sizeof ( owl_timestamp ) + sizeof ( float ) * 3 ;
2011-04-01 10:02:05 +02:00
packet = malloc ( packet_size ) ;
2011-04-04 16:41:52 +02:00
memset ( & packet [ offset ] , OWL_REQUEST_CALIBRATION , 1 ) ; // Packet type
2011-04-01 10:02:05 +02:00
+ + 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
2011-04-13 11:12:08 +02:00
options . x = owl_htonf ( options . x ) ;
options . y = owl_htonf ( options . y ) ;
options . z = owl_htonf ( options . z ) ;
2011-04-01 10:02:05 +02:00
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 " ) ;
2011-04-04 16:41:52 +02:00
packet_size = sizeof ( uint8_t ) + sizeof ( owl_timestamp ) ;
2011-04-01 10:02:05 +02:00
packet = malloc ( packet_size ) ;
2011-04-04 16:41:52 +02:00
memset ( & packet [ 0 ] , OWL_REQUEST_NORMAL , 1 ) ; // Packet type
2011-04-01 10:02:05 +02:00
memcpy ( & packet [ 1 ] , & request_time , sizeof ( request_time ) ) ;
}
printf ( " Packet timestamp: %s \n " , request_time_str ) ;
}
void send_request ( )
{
2011-12-01 16:29:12 +01:00
owl_send_request ( socksendfd , & server , packet , packet_size ,
options . nb_pkt , options . delay ) ;
2011-04-01 10:02:05 +02:00
}
2011-05-03 14:34:35 +02:00
void * receive_position ( void * NULL_value )
2011-04-01 10:02:05 +02:00
{
// Position of the mobile as computed by the infrastructure:
2011-12-01 17:53:37 +01:00
owl_result * result ;
2011-04-06 15:18:05 +02:00
2011-05-04 17:17:30 +02:00
while ( run )
2011-04-05 17:23:28 +02:00
{
2011-12-01 17:53:37 +01:00
result = owl_receive_position ( sockreceivefd ) ;
if ( result )
2011-04-13 11:22:04 +02:00
{
2011-12-01 17:53:37 +01:00
owl_print_result ( result ) ;
owl_free_result ( result ) ;
2011-04-13 11:22:04 +02:00
}
2011-12-01 17:53:37 +01:00
usleep ( 100000 ) ;
2011-04-05 17:23:28 +02:00
}
2011-04-06 15:18:05 +02:00
2011-12-01 17:53:37 +01:00
pthread_exit ( NULL_value ) ;
2011-04-01 10:02:05 +02:00
}
2011-08-18 10:51:50 +02:00
2011-12-01 17:53:37 +01:00
2011-04-06 15:18:05 +02:00
void print_error ( char * merror )
{
2011-08-18 10:51:50 +02:00
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 " ) ;
2011-04-06 15:18:05 +02:00
}
2011-08-18 10:51:50 +02:00
2011-04-01 10:02:05 +02:00
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 \t Print this help. \n "
2011-04-13 11:12:08 +02:00
" \t -V \t \t Print version information. \n "
2011-04-01 10:02:05 +02:00
" \t -d dest_ip \t Destination IP address of the localisation request. \n "
" \t -p dest_port \t Destination port of the localisation request "
" (default: %d). \n "
" \t -t delay \t Time between each packet transmission (default: %d "
" µs for a normal request, %d µs for a calibration request). \n "
" \t -n nb_packets \t Number of packet transmitted for the request "
" (default: %d for a normal request, %d for a calibration "
" request). \n "
" \t -i iface \t Name 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] \t Wait for the computed position and display it. "
" Optional argument 'port' allows to specify the listening "
" port (default: %d). \n "
,
program_name ,
program_name ,
2011-12-01 16:29:12 +01:00
OWL_DEFAULT_REQUEST_PORT ,
2011-04-01 10:02:05 +02:00
DEFAULT_DELAY_NORMAL ,
DEFAULT_DELAY_CALIB ,
DEFAULT_NBPKT_NORMAL ,
DEFAULT_NBPKT_CALIB ,
2011-12-01 16:29:12 +01:00
OWL_DEFAULT_RESULT_PORT
2011-04-01 10:02:05 +02:00
) ;
}
2011-04-13 11:12:08 +02:00
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
) ;
}
2011-05-03 17:25:59 +02:00
2011-08-18 10:51:50 +02:00
/* *** Commands for drone *** */
2011-05-03 17:25:59 +02:00
2011-05-05 14:35:48 +02:00
void calcul_trajectory ( float * distance , float * angle )
2011-05-03 17:25:59 +02:00
{
2011-05-04 17:17:30 +02:00
int a , b ;
2011-05-03 17:25:59 +02:00
float dist_av , dist_ar , dep ;
2011-05-05 14:35:48 +02:00
( * distance ) = dist_av = dist_ar = dep = ( * angle ) = 0 ;
2011-05-03 17:25:59 +02:00
2011-08-18 10:51:50 +02:00
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 ) ) ;
2011-05-03 17:25:59 +02:00
2011-08-18 10:51:50 +02:00
* angle = acos ( ( pow ( dist_ar , 2 ) + pow ( dep , 2 ) - pow ( dist_av , 2 ) ) /
( 2 * dist_ar * dep ) ) ;
2011-05-05 14:35:48 +02:00
* angle = ( * angle ) * 180 / M_PI ;
* distance = dist_av ;
2011-05-03 17:25:59 +02:00
2011-05-04 17:17:30 +02:00
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 */
/* ********** */
2011-05-31 14:29:14 +02:00
if ( a * b > = 0 ) ( * angle ) = - ( * angle ) ;
2011-05-04 17:17:30 +02:00
2011-05-03 17:25:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-05-04 17:17:30 +02:00
int check_destination ( struct point drone , struct point target )
2011-05-03 17:25:59 +02:00
{
2011-05-31 14:29:14 +02:00
if ( oc_distance_between ( drone , target ) < 3 )
2011-05-04 17:17:30 +02:00
return 1 ;
else return 0 ;
2011-05-03 17:25:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
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 ;
}
2011-08-18 10:51:50 +02:00
2011-05-03 17:25:59 +02:00
int read_position ( char * type )
{
if ( ! strcmp ( type , " WIFI " ) ) printf ( " Localisation by WIFI \n " ) ;
else if ( ! strcmp ( type , " GPS " ) )
{
printf ( " Localisation by GPS \n " ) ;
2011-08-18 10:51:50 +02:00
while ( ( share_gps . lat = = 0 | | share_gps . lon = = 0 ) & &
( share_gps_origin . lat = = 0 | | share_gps_origin . lon = = 0 ) )
2011-05-03 17:25:59 +02:00
{
if ( share_gps . lat ! = 0 & & share_gps . lon ! = 0 )
{
share_gps_origin . lat = share_gps . lat ;
share_gps_origin . lon = share_gps . lon ;
}
2011-05-05 14:35:48 +02:00
usleep ( 100000 ) ;
2011-05-03 17:25:59 +02:00
}
2011-05-31 14:29:14 +02:00
2011-05-03 17:25:59 +02:00
share_relatif = oc_convert ( share_gps_origin , share_gps ) ;
2011-05-18 14:28:54 +02:00
if ( share_gps_retro . lat ! = - 999.00 & & share_gps_retro . lon ! = - 999.00 )
2011-08-18 10:51:50 +02:00
share_relatif_retro =
oc_convert ( share_gps_origin , share_gps_retro ) ;
2011-05-03 17:25:59 +02:00
share_relatif_cons = oc_convert ( share_gps_origin , share_gps_cons ) ;
2011-05-09 13:54:34 +02:00
//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);
2011-05-03 17:25:59 +02:00
}
else return - 1 ;
return 0 ;
}
2011-08-18 10:51:50 +02:00
int cmd_drone_take_off ( )
2011-05-03 17:25:59 +02:00
{
printf ( " Take-off... \n " ) ;
state_flying = 1 ;
return 0 ;
}
2011-08-18 10:51:50 +02:00
int cmd_drone_landing ( )
2011-05-03 17:25:59 +02:00
{
printf ( " Landing.... \n " ) ;
state_flying = 0 ;
return 0 ;
}
2011-08-18 10:51:50 +02:00
int cmd_drone_init_direction ( )
2011-05-03 17:25:59 +02:00
{
2011-05-18 14:28:54 +02:00
//int boucle;
2011-05-03 17:25:59 +02:00
float speed ;
assert ( sizeof ( float ) = = 4 ) ;
if ( state_flying = = 0 )
return - 1 ;
//Implemented socket communication
2011-05-04 17:17:30 +02:00
printf ( " Initialisation... \n " ) ;
2011-05-03 17:25:59 +02:00
speed = - 0.5 ;
2011-05-18 14:28:54 +02:00
sem_wait ( & lock ) ;
//for(boucle=0;boucle<3;boucle++)
2011-08-18 10:51:50 +02:00
while ( share_gps_retro . lat = = - 999.00 | | share_gps_retro . lon = = - 999.00 )
2011-05-31 14:29:14 +02:00
{
2011-05-03 17:25:59 +02:00
int * floatint_speed = ( int32_t * ) & speed ;
2011-05-31 14:29:14 +02:00
sprintf ( sendline , " AT*PCMD=%d,1,0,%d,0,0 \r " , i , * floatint_speed ) ;
2011-05-09 13:54:34 +02:00
sendto ( sockcmdfd , sendline , strlen ( sendline ) , 0 ,
( struct sockaddr * ) & servcmd , sizeof ( servcmd ) ) ;
usleep ( 100000 ) ;
2011-05-03 17:25:59 +02:00
}
2011-05-31 14:29:14 +02:00
sprintf ( sendline , " AT*PCMD=%d,1,0,0,0,0 \r " , i ) ;
2011-05-09 13:54:34 +02:00
sendto ( sockcmdfd , sendline , strlen ( sendline ) , 0 ,
( struct sockaddr * ) & servcmd , sizeof ( servcmd ) ) ;
2011-05-18 14:28:54 +02:00
sem_post ( & lock ) ;
2011-05-03 17:25:59 +02:00
return 1 ;
}
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
int cmd_drone_move ( float speed_pitch , int angle )
2011-05-03 17:25:59 +02:00
{
2011-05-04 17:17:30 +02:00
float speed_yaw ;
2011-05-31 14:29:14 +02:00
//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 ;
2011-08-18 10:51:50 +02:00
sprintf ( sendline , " AT*PCMD=%d,1,0,%d,0,%d \r " ,
i , * floatint_speed_pitch , * floatint_speed_yaw ) ;
2011-05-31 14:29:14 +02:00
//printf("%s\n", sendline);
2011-08-18 11:50:55 +02:00
+ + i ;
2011-05-31 14:29:14 +02:00
sendto ( sockcmdfd , sendline , strlen ( sendline ) , 0 ,
( struct sockaddr * ) & servcmd , sizeof ( servcmd ) ) ;
//printf("[ATPCMD] pitch : %f yaw : %f\n", speed_pitch, speed_yaw);
//}
2011-05-18 14:28:54 +02:00
sem_post ( & lock ) ;
2011-05-04 17:17:30 +02:00
return 0 ;
2011-05-03 17:25:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-05-03 17:25:59 +02:00
void drone_err ( int err )
{
char msg [ 100 ] ;
switch ( err )
{
case 1 :
2011-08-18 10:51:50 +02:00
strcpy ( msg ,
" Undetermined position or invalid localisation type, "
" canceled \n " ) ;
2011-05-03 17:25:59 +02:00
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 ) ;
2011-05-04 17:17:30 +02:00
//drone_handler_sigint(SIGINT);
2011-05-03 17:25:59 +02:00
}
2011-08-18 10:51:50 +02:00
2011-05-03 17:25:59 +02:00
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 ;
}
2011-08-18 10:51:50 +02:00
2011-05-03 17:25:59 +02:00
int drone_socket_close ( void )
{
return close ( sockcmdfd ) ;
}
2011-08-18 10:51:50 +02:00
2011-05-03 17:25:59 +02:00
void * drone_socket_watchdog ( void * NULL_value )
{
2011-05-31 14:29:14 +02:00
struct timeval start , end ;
2011-08-18 10:51:50 +02:00
2011-05-04 17:17:30 +02:00
while ( run )
2011-05-03 17:25:59 +02:00
{
2011-05-31 14:29:14 +02:00
gettimeofday ( & start , NULL ) ;
usleep ( 8000 ) ;
2011-05-03 17:25:59 +02:00
memset ( sendline , 0 , sizeof ( sendline ) ) ;
if ( i = = - 1 )
{
2011-08-18 10:51:50 +02:00
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 \r AT*MISC=5,2,20,2000,3000 \r " ,
LENSTR ) ;
2011-05-31 14:29:14 +02:00
i = 6 ;
2011-05-03 17:25:59 +02:00
}
else
{
2011-05-18 14:28:54 +02:00
sem_wait ( & lock ) ;
2011-05-03 17:25:59 +02:00
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 ) ;
2011-05-04 17:17:30 +02:00
}
2011-08-18 11:50:55 +02:00
+ + i ;
2011-05-03 17:25:59 +02:00
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 ) ) ;
2011-05-31 14:29:14 +02:00
//printf("[WATCHDOG] %s\n", sendline);
2011-05-18 14:28:54 +02:00
sem_post ( & lock ) ;
2011-05-31 14:29:14 +02:00
gettimeofday ( & end , NULL ) ;
2011-08-18 10:51:50 +02:00
//long calc =
// (end.tv_sec * 1000000 + end.tv_usec) -
// (start.tv_sec * 1000000 + start.tv_usec) ;
2011-05-31 14:29:14 +02:00
//printf("[WATCHDOG] Time usec : %ld\n", calc);
2011-05-03 17:25:59 +02:00
}
pthread_exit ( NULL_value ) ;
}
2011-05-04 17:17:30 +02:00
2011-08-18 10:51:50 +02:00
2011-05-04 17:17:30 +02:00
void drone_handler_sigint ( const int num )
{
if ( num = = SIGINT )
{
2011-08-18 10:51:50 +02:00
cmd_drone_landing ( ) ;
2011-05-04 17:17:30 +02:00
sleep ( 2 ) ;
run = 0 ;
2011-08-18 10:51:50 +02:00
close ( socksendfd ) ;
close ( sockreceivefd ) ;
drone_socket_close ( ) ;
2011-05-04 17:17:30 +02:00
exit ( 0 ) ;
}
}
2011-05-18 14:28:54 +02:00
2011-08-18 10:51:50 +02:00
/* *** Linked list functions *** */
consigne * list_add_front ( consigne * list , double latitude ,
double longitude , double altitude )
2011-05-18 14:28:54 +02:00
{
2011-05-31 14:29:14 +02:00
consigne * new_consigne = malloc ( sizeof ( consigne ) ) ;
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
new_consigne - > lat = latitude ;
new_consigne - > lon = longitude ;
new_consigne - > alt = altitude ;
2011-05-18 14:28:54 +02:00
2011-05-31 14:29:14 +02:00
new_consigne - > next = list ;
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
return new_consigne ;
2011-05-18 14:28:54 +02:00
}
2011-08-18 10:51:50 +02:00
consigne * list_add_back ( consigne * list , double lat ,
double lon , double alt )
2011-05-18 14:28:54 +02:00
{
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
consigne * new_consigne = malloc ( sizeof ( consigne ) ) ;
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
new_consigne - > lat = lat ;
new_consigne - > lon = lon ;
new_consigne - > alt = alt ;
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
new_consigne - > next = NULL ;
2011-08-18 10:51:50 +02:00
2011-05-31 14:29:14 +02:00
if ( list = = NULL )
2011-08-18 10:51:50 +02:00
return new_consigne ;
2011-05-31 14:29:14 +02:00
else
2011-05-18 14:28:54 +02:00
{
2011-05-31 14:29:14 +02:00
consigne * tmp = list ;
while ( tmp - > next ! = NULL )
2011-05-18 14:28:54 +02:00
{
2011-05-31 14:29:14 +02:00
tmp = tmp - > next ;
2011-05-18 14:28:54 +02:00
}
2011-05-31 14:29:14 +02:00
tmp - > next = new_consigne ;
return list ;
2011-05-18 14:28:54 +02:00
}
}