2011-04-01 10:02:05 +02:00
/*
* This file is part of the rtap localisation project .
*/
# include "../libowlps-client/owlps-client.h"
2011-04-05 17:23:28 +02:00
# include "owlps-drone.h"
2011-04-04 16:41:52 +02:00
# include <stdio.h>
# include <stdlib.h>
# include <unistd.h>
# include <inttypes.h>
# include <string.h>
# 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>
# include <fcntl.h>
2011-05-03 17:25:59 +02:00
# include <assert.h>
2011-05-04 17:17:30 +02:00
# include <signal.h>
2011-04-14 10:58:59 +02:00
# define SEPARATOR ','
# define EOL '$'
2011-05-03 14:34:35 +02:00
# define TYPE "GPS"
2011-04-01 10:02:05 +02:00
/* Options */
2011-04-14 10:58:59 +02:00
gps datagps ;
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 = {
" " ,
LOC_REQUEST_DEFAULT_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:
2011-04-04 16:41:52 +02:00
owl_bool is_calibration_request = FALSE ;
2011-04-01 10:02:05 +02:00
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
2011-04-14 10:58:59 +02:00
// Shared variables for OwlSIG (Geo Information System)
2011-05-03 17:25:59 +02:00
//double share_x, share_y, share_z;
struct point share_gps ;
struct point share_gps_retro ;
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-04 17:17:30 +02:00
int state_flying = 0 , lock = 0 , i = - 1 , start = 0 , run = 1 ;
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-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-05 14:35:48 +02:00
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-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
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-04 17:17:30 +02:00
drone_socket_create ( ) ;
pthread_t drone_watchdog ;
pthread_create ( & drone_watchdog , NULL , drone_socket_watchdog , NULL ) ;
while ( start = = 0 ) usleep ( 200000 ) ;
2011-05-05 14:35:48 +02:00
speed_pitch = - 1 ;
2011-05-04 17:17:30 +02:00
if ( read_position ( TYPE ) < 0 )
drone_err ( 1 ) ;
2011-05-03 14:34:35 +02:00
2011-05-04 17:17:30 +02:00
if ( cmd_drone_take_off ( ) < 0 )
drone_err ( 2 ) ;
2011-05-03 14:34:35 +02:00
2011-05-04 17:17:30 +02:00
if ( cmd_drone_init_direction ( ) < 0 )
drone_err ( 3 ) ;
2011-05-05 14:35:48 +02:00
calcul_trajectory ( & angle , & distance ) ;
distance_total = distance ;
2011-05-04 17:17:30 +02:00
while ( ! check_destination ( share_gps , share_gps_cons ) )
{
if ( read_position ( TYPE ) < 0 )
drone_err ( 1 ) ;
2011-05-05 14:35:48 +02:00
calcul_trajectory ( & angle , & distance ) ;
if ( speed_pitch < - 1 ) speed_pitch = - 1 ;
else if ( speed_pitch > 1 ) speed_pitch = 1 ;
if ( speed_pitch < - ( distance / distance_total ) )
speed_pitch = speed_pitch ;
else
speed_pitch = - ( distance / distance_total ) ;
cmd_drone_move ( speed_pitch , angle ) ;
2011-05-04 17:17:30 +02:00
sleep ( 1 ) ;
}
2011-05-03 14:34:35 +02:00
2011-04-01 10:02:05 +02:00
( void ) close ( socksendfd ) ;
( void ) close ( sockreceivefd ) ;
2011-05-03 14:34:35 +02:00
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-05-03 13:55:57 +02:00
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 ] ! = ' $ ' ) ;
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-05-03 13:55:57 +02:00
do
{
c = read ( gps , buffer + offset , 1 ) ;
offset = offset + c ;
}
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 ;
}
y + + ;
}
printf ( " \n Taille 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 ;
2011-05-03 17:25:59 +02:00
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-03 13:55:57 +02:00
printf ( " lat %s lon %s \n " , data [ 3 ] , data [ 5 ] ) ;
2011-05-03 17:25:59 +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 13:55:57 +02:00
}
2011-04-14 10:58:59 +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-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-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 ) ;
printf ( " \n taille %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 )
2011-05-03 17:25:59 +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-05-03 17:25:59 +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 ) ;
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-05-03 17:25:59 +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 ;
cmd_drone_landing ( ) ;
sprintf ( response , " %sLanding;OK; " , response ) ;
}
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-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 ' :
/* Facultative getopt options does not handle separated
* values ( like - l < port > ) */
if ( optarg = = 0 )
{
/* 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 = MOBILE_DEFAULT_PORT ;
else
{
// Take the optind value:
options . listening_port =
strtoul ( argv [ optind ] , NULL , 0 ) ;
optind + + ;
}
}
else // We got an option like -l<port>, it's OK
options . listening_port = strtoul ( optarg , NULL , 0 ) ;
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 ;
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 )
{
is_calibration_request = 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 ;
}
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 " ) ;
options . dest_port = LOC_REQUEST_DEFAULT_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 " ) ;
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 =
owlclient_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 ;
char request_time_str [ OWL_TIMESTAMP_STR_LEN ] ;
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 ( )
{
owlclient_send_request ( socksendfd , & server , packet , packet_size ,
options . nb_pkt , options . delay ) ;
}
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-04-05 17:23:28 +02:00
char timestampXYZ [ 128 ] ;
2011-04-06 15:18:05 +02:00
char * data_cpy ;
2011-05-04 17:17:30 +02:00
while ( run )
2011-05-03 14:34:35 +02:00
{
recvfrom ( sockreceivefd , & timestampXYZ , 128 , 0 , NULL , NULL ) ;
data_cpy = strdup ( timestampXYZ ) ;
string2data ( data_cpy ) ;
usleep ( 100000 ) ;
}
pthread_exit ( NULL_value ) ;
2011-04-01 10:02:05 +02:00
}
void string2data ( char * string_data )
{
2011-04-06 15:18:05 +02:00
/*Découpage de la chaine de caractère reçu du serveur de positionnement
sous forme de addrMAC ; TypeRequete ; Timestamp1 . Timestamp2 ; Algo ; X ; Y ; Z
en variables séparées
*/
2011-04-05 17:23:28 +02:00
char * mac = NULL ;
2011-04-06 15:18:05 +02:00
char * ptr = NULL ;
2011-04-01 10:02:05 +02:00
char * delims = " ; " ;
2011-04-13 11:22:04 +02:00
int type_req = 0 ;
int count_algo = 0 ;
int count_print = 0 ;
2011-04-06 15:18:05 +02:00
int onetime = 0 ;
2011-06-21 11:32:09 +02:00
int error = 0 ;
2011-04-05 17:23:28 +02:00
2011-04-06 15:18:05 +02:00
while ( onetime < 1 )
{
2011-04-13 11:22:04 +02:00
2011-04-06 15:18:05 +02:00
//Lecture Adresse Mac
ptr = strtok ( string_data , delims ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
2011-04-06 15:18:05 +02:00
{
print_error ( " mac " ) ;
break ;
}
mac = ptr ;
//Lecture Type Request
ptr = strtok ( NULL , delims ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
2011-04-06 15:18:05 +02:00
{
print_error ( " request " ) ;
break ;
}
type_req = atoi ( ptr ) ;
2011-04-05 17:23:28 +02:00
2011-04-06 15:18:05 +02:00
//Lecture TimeStamp1
ptr = strtok ( NULL , " . " ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
2011-04-06 15:18:05 +02:00
{
print_error ( " timestamp " ) ;
break ;
}
sscanf ( ptr , " %ld " , & timestamp . tv_sec ) ;
2011-04-05 17:23:28 +02:00
2011-04-06 15:18:05 +02:00
//Lecture TimeStamp2
ptr = strtok ( NULL , " ; " ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
2011-04-06 15:18:05 +02:00
{
print_error ( " timestamp " ) ;
break ;
}
sscanf ( ptr , " %ld " , & timestamp . tv_nsec ) ;
onetime + + ;
}
2011-04-05 17:23:28 +02:00
result results [ 10 ] ;
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-04-06 15:18:05 +02:00
//Lecture de l'algorythme utilisé
2011-04-05 17:23:28 +02:00
ptr = strtok ( NULL , delims ) ;
2011-04-06 15:18:05 +02:00
if ( ptr = = NULL ) break ;
sscanf ( ptr , " %s " , results [ count_algo ] . algo ) ;
2011-06-21 11:32:09 +02:00
if ( ! strcmp ( results [ count_algo ] . algo , " Real " ) )
error = 1 ;
2011-04-06 15:18:05 +02:00
//Lecture du point X
2011-04-05 17:23:28 +02:00
ptr = strtok ( NULL , delims ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
{
if ( count_algo = = 0 )
{
print_error ( " trame " ) ;
break ;
}
else
{
print_error ( " algo " ) ;
count_algo - - ;
break ;
}
}
2011-04-06 15:18:05 +02:00
results [ count_algo ] . x = atof ( ptr ) ;
//Lecture du point Y
2011-04-05 17:23:28 +02:00
ptr = strtok ( NULL , delims ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
{
if ( count_algo = = 0 )
{
print_error ( " trame " ) ;
break ;
}
else
{
print_error ( " algo " ) ;
count_algo - - ;
break ;
}
}
2011-04-06 15:18:05 +02:00
results [ count_algo ] . y = atof ( ptr ) ;
//Lecture du point Z
2011-04-05 17:23:28 +02:00
ptr = strtok ( NULL , delims ) ;
2011-04-13 11:22:04 +02:00
if ( ptr = = NULL )
{
if ( count_algo = = 0 )
{
print_error ( " trame " ) ;
break ;
}
else
{
perror ( " algo " ) ;
count_algo - - ;
break ;
}
}
2011-04-06 15:18:05 +02:00
results [ count_algo ] . z = atof ( ptr ) ;
2011-06-21 11:32:09 +02:00
if ( error = = 1 )
{
//Lecture du message Error
ptr = strtok ( NULL , delims ) ;
if ( ptr = = NULL )
{
if ( count_algo = = 0 )
{
print_error ( " trame " ) ;
break ;
}
else
{
print_error ( " algo " ) ;
count_algo - - ;
break ;
}
}
results [ count_algo ] . check = ptr ;
//Lecture valeur Error
ptr = strtok ( NULL , delims ) ;
if ( ptr = = NULL )
{
if ( count_algo = = 0 )
{
print_error ( " trame " ) ;
break ;
}
else
{
perror ( " algo " ) ;
count_algo - - ;
break ;
}
}
results [ count_algo ] . err = atof ( ptr ) ;
2011-04-06 15:18:05 +02:00
2011-05-05 14:35:48 +02:00
2011-06-21 11:32:09 +02:00
count_algo + + ;
}
2011-04-06 15:18:05 +02:00
2011-06-21 11:32:09 +02:00
else
count_algo + + ;
2011-04-05 17:23:28 +02:00
}
2011-04-06 15:18:05 +02:00
for ( count_print = 0 ; count_print < count_algo ; count_print + + )
2011-04-05 17:23:28 +02:00
{
printf ( " \n ---------------- \n "
" Adresse Mac : %s \n "
" Type de requete : %d \n "
2011-04-06 15:18:05 +02:00
" Timestamp : %ld.%ld \n "
2011-04-05 17:23:28 +02:00
" Algo : %s \n "
" X : %f \n "
" Y : %f \n "
" Z : %f \n "
2011-04-06 15:18:05 +02:00
" ------------------ \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 ) ;
2011-04-05 17:23:28 +02:00
}
2011-04-01 10:02:05 +02:00
}
2011-04-06 15:18:05 +02:00
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 " ) ;
}
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 ,
LOC_REQUEST_DEFAULT_PORT ,
DEFAULT_DELAY_NORMAL ,
DEFAULT_DELAY_CALIB ,
DEFAULT_NBPKT_NORMAL ,
DEFAULT_NBPKT_CALIB ,
MOBILE_DEFAULT_PORT
) ;
}
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
/* Commands for drone */
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
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-05 14:35:48 +02:00
* 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 ;
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 */
/* ********** */
if ( ( a < 0 ) ! = ( b < 0 ) )
2011-05-05 14:35:48 +02:00
( * angle ) = ( * angle ) ;
else ( * angle ) = - ( * angle ) ;
2011-05-04 17:17:30 +02:00
2011-05-03 17:25:59 +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-05 14:35:48 +02:00
if ( oc_distance_between ( drone , target ) < 1.8 )
2011-05-04 17:17:30 +02:00
return 1 ;
else return 0 ;
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 " ) ;
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 ;
}
2011-05-05 14:35:48 +02:00
usleep ( 100000 ) ;
2011-05-03 17:25:59 +02:00
}
share_relatif = oc_convert ( share_gps_origin , share_gps ) ;
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 ( void )
{
printf ( " Take-off... \n " ) ;
state_flying = 1 ;
return 0 ;
}
int cmd_drone_landing ( void )
{
printf ( " Landing.... \n " ) ;
state_flying = 0 ;
return 0 ;
}
int cmd_drone_init_direction ( void )
{
int boucle ;
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 ;
while ( lock = = 1 ) ;
lock = 1 ;
for ( boucle = 0 ; boucle < 3 ; boucle + + )
{
int * floatint_speed = ( int32_t * ) & speed ;
sprintf ( sendline , " AT*PCMD=%d,0,0,%d,0,0 \r " , i , * floatint_speed ) ;
}
lock = 0 ;
return 1 ;
}
2011-05-04 17:17:30 +02:00
int cmd_drone_move ( float speed_pitch , float angle )
2011-05-03 17:25:59 +02:00
{
2011-05-04 17:17:30 +02:00
float speed_yaw ;
2011-05-03 17:25:59 +02:00
if ( state_flying = = 0 )
return - 1 ;
else
{
2011-05-04 17:17:30 +02:00
speed_yaw = ( 1 / 180 ) * angle ;
2011-05-03 17:25:59 +02:00
while ( lock = = 1 ) ;
lock = 1 ;
int * floatint_speed_pitch = ( int32_t * ) & speed_pitch ;
int * floatint_speed_yaw = ( int32_t * ) & speed_yaw ;
sprintf ( sendline , " AT*PCMD=%d,0,0,%d,0,%d \r " , i , * floatint_speed_pitch , * floatint_speed_yaw ) ;
i + + ;
sendto ( sockcmdfd , sendline , strlen ( sendline ) , 0 ,
( struct sockaddr * ) & servcmd , sizeof ( servcmd ) ) ;
}
2011-05-04 17:17:30 +02:00
lock = 0 ;
return 0 ;
2011-05-03 17:25:59 +02:00
}
void drone_err ( int err )
{
char msg [ 100 ] ;
switch ( err )
{
case 1 :
strcpy ( msg , " Position is not determined or type of localisation is invalid, 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 ) ;
2011-05-04 17:17:30 +02:00
//drone_handler_sigint(SIGINT);
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 ;
}
int drone_socket_close ( void )
{
return close ( sockcmdfd ) ;
}
void * drone_socket_watchdog ( void * NULL_value )
{
2011-05-04 17:17:30 +02:00
while ( run )
2011-05-03 17:25:59 +02:00
{
2011-05-04 17:17:30 +02:00
usleep ( 20000 ) ;
2011-05-03 17:25:59 +02:00
memset ( sendline , 0 , sizeof ( sendline ) ) ;
if ( i = = - 1 )
{
2011-05-05 14:35:48 +02:00
strncpy ( sendline , " AT*CONFIG=1, \" general:navdata_demo \" , \" TRUE \" \r AT*CONFIG=2, \" control:control_yaw \" , \" 2.5 \" \r AT*PMODE=3,2 \r AT*MISC=4,2,20,2000,3000 \r " , LENSTR ) ;
2011-05-03 17:25:59 +02:00
i = 4 ;
}
else
{
while ( lock = = 1 ) ;
lock = 1 ;
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-05-03 17:25:59 +02:00
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 ) ) ;
lock = 0 ;
}
pthread_exit ( NULL_value ) ;
}
2011-05-04 17:17:30 +02:00
void drone_handler_sigint ( const int num )
{
if ( num = = SIGINT )
{
( void ) cmd_drone_landing ( ) ;
sleep ( 2 ) ;
run = 0 ;
( void ) close ( socksendfd ) ;
( void ) close ( sockreceivefd ) ;
( void ) drone_socket_close ;
exit ( 0 ) ;
}
}