2008-02-14 08:54:51 +01:00
|
|
|
/*
|
|
|
|
* This file is part of the rtap localisation project.
|
|
|
|
*/
|
|
|
|
|
2010-07-29 11:04:30 +02:00
|
|
|
#include "../libowlps-client/owlps-client.h"
|
2008-02-14 08:54:51 +01:00
|
|
|
|
2008-03-14 11:31:04 +01:00
|
|
|
#define DEBUG
|
2008-02-14 08:54:51 +01:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Error codes */
|
2010-07-29 11:04:30 +02:00
|
|
|
#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments)
|
2008-02-14 08:54:51 +01:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Number of packets to send */
|
|
|
|
#define DEFAULT_NBPKT_CALIB 20 // 20 packets when calibrating
|
|
|
|
#define DEFAULT_NBPKT_NORMAL 10 // 10 packets when requesting the position
|
2008-06-17 15:23:03 +02:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Delay between two packet transmissions (in microseconds) */
|
|
|
|
#define DEFAULT_DELAY_CALIB 50000 // Calibration request
|
|
|
|
#define DEFAULT_DELAY_NORMAL 25000 // Localisation request
|
2008-02-16 10:58:12 +01:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Program arguments (getopt string) */
|
2010-07-13 17:02:48 +02:00
|
|
|
#define OPTIONS "d:hi:l::n:p:t:"
|
2009-07-02 20:48:32 +02:00
|
|
|
|
2010-07-13 16:59:45 +02:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Function headers */
|
2010-07-13 16:59:45 +02:00
|
|
|
void parse_command_line(int argc, char **argv) ;
|
|
|
|
void parse_main_options(int argc, char **argv) ;
|
|
|
|
void check_destination_ip(void) ;
|
|
|
|
void parse_calibration_data(int argc, char **argv) ;
|
|
|
|
void check_configuration(void) ;
|
|
|
|
#ifdef DEBUG
|
|
|
|
void print_configuration(void) ;
|
|
|
|
#endif // DEBUG
|
2010-07-13 18:20:35 +02:00
|
|
|
void create_socket(void) ;
|
|
|
|
void make_packet(void) ;
|
|
|
|
void send_request(void) ;
|
|
|
|
void receive_position(void) ;
|
2010-07-13 16:59:45 +02:00
|
|
|
void print_usage(void) ;
|
|
|
|
|
|
|
|
|
|
|
|
/* Options */
|
|
|
|
struct {
|
|
|
|
char dest_ip[16] ; // Destination IP of the packets
|
|
|
|
long dest_port ;
|
|
|
|
char iface[IFNAMSIZ + 1] ; // Source network interface
|
|
|
|
long delay ; // Time between two packet transmissions
|
|
|
|
short nb_pkt ; // Number of packets to send
|
|
|
|
long listening_port ;
|
|
|
|
// Calibration data:
|
|
|
|
DIRECTION direction ;
|
|
|
|
float x ;
|
|
|
|
float y ;
|
|
|
|
float z ;
|
|
|
|
} options = {
|
|
|
|
"",
|
|
|
|
LOC_REQUEST_DEFAULT_PORT,
|
|
|
|
"",
|
|
|
|
-1,
|
|
|
|
-1,
|
|
|
|
-1,
|
|
|
|
0, 0, 0, 0
|
|
|
|
} ;
|
|
|
|
|
|
|
|
char *program_name = NULL ;
|
|
|
|
|
|
|
|
// TRUE if the packet is a calibration request, FALSE if it is a simple
|
|
|
|
// positioning request:
|
|
|
|
BOOL is_calibration_request = FALSE ;
|
2008-02-14 08:54:51 +01:00
|
|
|
|
2010-07-13 18:20:35 +02:00
|
|
|
int sockfd ; // Sending socket descriptor
|
|
|
|
struct sockaddr_in server ; // Server info
|
2010-08-03 12:13:09 +02:00
|
|
|
char *packet = NULL ; // Packet to send
|
|
|
|
int packet_size ; // Packet size
|
2010-07-13 18:20:35 +02:00
|
|
|
|
2008-02-14 08:54:51 +01:00
|
|
|
|
|
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
|
|
{
|
2010-07-13 16:59:45 +02:00
|
|
|
program_name = argv[0] ;
|
|
|
|
parse_command_line(argc, argv) ;
|
|
|
|
|
2010-07-13 18:20:35 +02:00
|
|
|
create_socket() ;
|
|
|
|
make_packet() ;
|
|
|
|
send_request() ;
|
2010-07-13 16:59:45 +02:00
|
|
|
|
|
|
|
(void) close(sockfd) ;
|
|
|
|
|
|
|
|
if (options.listening_port != -1)
|
2010-07-13 18:20:35 +02:00
|
|
|
receive_position() ;
|
2010-07-13 16:59:45 +02:00
|
|
|
|
|
|
|
return 0 ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void parse_command_line(int argc, char **argv)
|
|
|
|
{
|
|
|
|
parse_main_options(argc, argv) ;
|
|
|
|
check_destination_ip() ;
|
|
|
|
parse_calibration_data(argc, argv) ;
|
|
|
|
check_configuration() ;
|
|
|
|
|
|
|
|
#ifdef DEBUG
|
|
|
|
print_configuration() ;
|
|
|
|
#endif // DEBUG
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void parse_main_options(int argc, char **argv)
|
|
|
|
{
|
|
|
|
int opt ;
|
2009-04-14 14:29:21 +02:00
|
|
|
|
|
|
|
while ((opt = getopt(argc, argv, OPTIONS)) != -1)
|
|
|
|
{
|
|
|
|
switch (opt)
|
2011-02-24 14:54:31 +01:00
|
|
|
{
|
|
|
|
case 'd' :
|
|
|
|
strncpy(options.dest_ip, optarg, 16) ;
|
|
|
|
break ;
|
2010-07-13 17:02:48 +02:00
|
|
|
case 'h' :
|
|
|
|
print_usage() ;
|
|
|
|
exit(0) ;
|
2011-02-24 14:54:31 +01:00
|
|
|
case 'i' :
|
|
|
|
strncpy(options.iface, optarg, IFNAMSIZ + 1) ;
|
|
|
|
break ;
|
|
|
|
case 'l' :
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Facultative getopt options does not handle separated
|
|
|
|
* values (like -l <port>) */
|
2011-02-24 14:54:31 +01:00
|
|
|
if (optarg == 0)
|
|
|
|
{
|
2010-07-13 16:32:16 +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 */
|
2011-02-24 14:54:31 +01:00
|
|
|
if (argv[optind] == NULL || argv[optind][0] == '-')
|
2010-07-13 16:32:16 +02:00
|
|
|
// Take the default value:
|
2011-02-24 14:54:31 +01:00
|
|
|
options.listening_port = MOBILE_DEFAULT_PORT ;
|
|
|
|
else
|
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
// Take the optind value:
|
2011-02-24 14:54:31 +01:00
|
|
|
options.listening_port = strtol(argv[optind], NULL, 0) ;
|
|
|
|
optind++ ;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else // We got an option like -l<port>, it's OK
|
|
|
|
options.listening_port = strtol(optarg, NULL, 0) ;
|
|
|
|
break ;
|
|
|
|
case 'n' :
|
|
|
|
options.nb_pkt = strtol(optarg, NULL, 0) ;
|
|
|
|
break ;
|
|
|
|
case 'p' :
|
|
|
|
options.dest_port = strtol(optarg, NULL, 0) ;
|
|
|
|
break ;
|
|
|
|
case 't' :
|
|
|
|
options.delay = strtol(optarg, NULL, 0) ;
|
|
|
|
break ;
|
|
|
|
default :
|
|
|
|
print_usage() ;
|
|
|
|
exit(ERR_BAD_USAGE) ;
|
|
|
|
}
|
2009-04-14 14:29:21 +02:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
}
|
|
|
|
|
2008-06-17 15:23:03 +02:00
|
|
|
|
2010-07-13 16:59:45 +02:00
|
|
|
|
|
|
|
void check_destination_ip()
|
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Check if we got a destination IP address */
|
|
|
|
if (options.dest_ip[0] == '\0')
|
2009-04-14 14:29:21 +02:00
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
fprintf(stderr, "Error! You must specify a destination IP address"
|
|
|
|
" (-d).\n") ;
|
2010-07-13 16:59:45 +02:00
|
|
|
print_usage() ;
|
|
|
|
exit(ERR_BAD_USAGE) ;
|
2009-04-14 14:29:21 +02:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2008-02-16 10:58:12 +01:00
|
|
|
|
2010-07-13 16:59:45 +02:00
|
|
|
void parse_calibration_data(int argc, char **argv)
|
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
/* Parse remaining arguments (possible calibration data) */
|
2009-04-14 14:29:21 +02:00
|
|
|
if (argc - optind != 0)
|
2008-02-16 10:58:12 +01:00
|
|
|
{
|
2009-04-14 14:29:21 +02:00
|
|
|
if (argc - optind == 4)
|
2011-02-24 14:54:31 +01:00
|
|
|
{
|
|
|
|
is_calibration_request = TRUE ;
|
|
|
|
options.direction = strtol(argv[optind++], NULL, 0) ;
|
|
|
|
options.x = strtod(argv[optind++], NULL) ;
|
|
|
|
options.y = strtod(argv[optind++], NULL) ;
|
|
|
|
options.z = strtod(argv[optind], NULL) ;
|
|
|
|
}
|
2010-07-13 16:32:16 +02:00
|
|
|
else // Bad number of arguments
|
2011-02-24 14:54:31 +01:00
|
|
|
{
|
|
|
|
print_usage() ;
|
|
|
|
exit(ERR_BAD_USAGE) ;
|
|
|
|
}
|
2008-02-16 10:58:12 +01:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2008-02-16 10:58:12 +01:00
|
|
|
|
2010-07-13 16:59:45 +02:00
|
|
|
void check_configuration()
|
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
// Delay not specified (or bad delay):
|
|
|
|
if (options.delay < 0)
|
2008-02-16 10:58:12 +01:00
|
|
|
{
|
2009-04-15 14:15:49 +02:00
|
|
|
#ifdef DEBUG
|
2010-07-13 16:32:16 +02:00
|
|
|
fprintf(stderr,
|
|
|
|
"Warning! delay: failing back to default value.\n") ;
|
2009-04-15 14:15:49 +02:00
|
|
|
#endif // DEBUG
|
2009-04-14 14:29:21 +02:00
|
|
|
if (is_calibration_request)
|
2011-02-24 14:54:31 +01:00
|
|
|
options.delay = DEFAULT_DELAY_CALIB ;
|
2009-04-14 14:29:21 +02:00
|
|
|
else
|
2011-02-24 14:54:31 +01:00
|
|
|
options.delay = DEFAULT_DELAY_NORMAL ;
|
2009-04-14 14:29:21 +02:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
// Number of packet not specified (or bad number)
|
|
|
|
if (options.nb_pkt < 1)
|
2009-04-14 14:29:21 +02:00
|
|
|
{
|
2009-04-15 14:15:49 +02:00
|
|
|
#ifdef DEBUG
|
2010-07-13 16:32:16 +02:00
|
|
|
fprintf(stderr,
|
|
|
|
"Warning! nb_pkt: failing back to default value.\n") ;
|
2009-04-15 14:15:49 +02:00
|
|
|
#endif // DEBUG
|
2009-04-14 14:29:21 +02:00
|
|
|
if (is_calibration_request)
|
2011-02-24 14:54:31 +01:00
|
|
|
options.nb_pkt = DEFAULT_NBPKT_CALIB ;
|
2009-04-14 14:29:21 +02:00
|
|
|
else
|
2011-02-24 14:54:31 +01:00
|
|
|
options.nb_pkt = DEFAULT_NBPKT_NORMAL ;
|
2008-02-16 10:58:12 +01:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
|
2010-07-13 16:32:16 +02:00
|
|
|
// We want to send a calibration request AND to be located, which is
|
|
|
|
// not allowed:
|
2009-07-02 20:48:32 +02:00
|
|
|
if (is_calibration_request && options.listening_port != -1)
|
|
|
|
{
|
|
|
|
#ifdef DEBUG
|
2010-07-13 16:32:16 +02:00
|
|
|
fprintf(stderr, "Warning! You cannot wait for a server answer when"
|
|
|
|
" you calibrate. Option -l ignored…\n") ;
|
2009-07-02 20:48:32 +02:00
|
|
|
#endif // DEBUG
|
|
|
|
options.listening_port = -1 ;
|
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2008-02-16 10:58:12 +01:00
|
|
|
|
2009-04-14 14:29:21 +02:00
|
|
|
#ifdef DEBUG
|
2010-07-13 16:59:45 +02:00
|
|
|
void print_configuration()
|
|
|
|
{
|
2010-07-13 16:32:16 +02:00
|
|
|
fprintf(stderr, "Options:\n"
|
|
|
|
"\tDestination IP: %s\n"
|
|
|
|
"\tDestination port: %ld\n"
|
|
|
|
"\tInterface: %s\n"
|
|
|
|
"\tDelay: %ld\n"
|
|
|
|
"\tNumber of packets: %d\n"
|
|
|
|
"\tListening port: %ld\n"
|
|
|
|
"\tDirection: %d\n"
|
|
|
|
"\tX: %f\n"
|
|
|
|
"\tY: %f\n"
|
|
|
|
"\tZ: %f\n"
|
|
|
|
,
|
2011-02-24 14:54:31 +01:00
|
|
|
options.dest_ip,
|
|
|
|
options.dest_port,
|
|
|
|
options.iface,
|
|
|
|
options.delay,
|
|
|
|
options.nb_pkt,
|
|
|
|
options.listening_port,
|
|
|
|
options.direction,
|
|
|
|
options.x,
|
|
|
|
options.y,
|
|
|
|
options.z
|
|
|
|
) ;
|
2008-02-14 08:54:51 +01:00
|
|
|
}
|
2010-07-13 16:59:45 +02:00
|
|
|
#endif // DEBUG
|
2010-07-13 16:32:16 +02:00
|
|
|
|
|
|
|
|
|
|
|
|
2010-10-25 15:04:57 +02:00
|
|
|
void create_socket()
|
2010-07-29 11:04:30 +02:00
|
|
|
{
|
|
|
|
sockfd =
|
|
|
|
owlps_create_socket_to_aggregator(options.dest_ip, options.dest_port,
|
|
|
|
&server, options.iface) ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2010-07-13 18:20:35 +02:00
|
|
|
/* Creates the packet to send. */
|
|
|
|
void make_packet()
|
|
|
|
{
|
2010-08-03 12:13:09 +02:00
|
|
|
int offset ; // Index used to create the packet
|
2010-07-13 18:20:35 +02:00
|
|
|
struct timeval request_time ;
|
|
|
|
|
|
|
|
gettimeofday(&request_time, NULL) ;
|
|
|
|
|
|
|
|
if (is_calibration_request) // Calibration packet
|
|
|
|
{
|
|
|
|
printf("Calibration time: %llu\n", timeval_to_ms(request_time)) ;
|
|
|
|
|
2010-08-03 12:13:09 +02:00
|
|
|
offset = 0 ;
|
2010-10-25 13:06:55 +02:00
|
|
|
packet_size =
|
2010-07-13 18:20:35 +02:00
|
|
|
sizeof(char) * 2 + sizeof(struct timeval) + sizeof(float) * 3 ;
|
2010-10-25 13:06:55 +02:00
|
|
|
packet = malloc(packet_size) ;
|
2010-07-13 18:20:35 +02:00
|
|
|
|
2010-08-03 12:13:09 +02:00
|
|
|
packet[offset++] = PACKET_TYPE_CALIBRATION ; // Packet type
|
|
|
|
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
|
|
|
offset += sizeof(request_time) ;
|
|
|
|
packet[offset++] = options.direction ; // Direction
|
2010-07-13 18:20:35 +02:00
|
|
|
#ifdef DEBUG
|
|
|
|
printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
|
2010-08-03 12:13:09 +02:00
|
|
|
packet[offset - 1], options.x, options.y, options.z) ;
|
2010-07-13 18:20:35 +02:00
|
|
|
#endif // DEBUG
|
2010-08-03 12:13:09 +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)) ;
|
2010-07-13 18:20:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
else // Standard packet
|
|
|
|
{
|
|
|
|
printf("Request time: %llu\n", timeval_to_ms(request_time)) ;
|
2010-10-25 13:06:55 +02:00
|
|
|
packet_size = sizeof(char) + sizeof(struct timeval) ;
|
|
|
|
packet = malloc(packet_size) ;
|
2010-08-03 12:13:09 +02:00
|
|
|
packet[0] = PACKET_TYPE_NORMAL ; // Packet type
|
|
|
|
memcpy(&packet[1], &request_time, sizeof(request_time)) ;
|
2010-07-13 18:20:35 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2010-10-25 15:04:57 +02:00
|
|
|
void send_request()
|
2010-07-13 18:20:35 +02:00
|
|
|
{
|
2010-08-03 12:13:09 +02:00
|
|
|
owlps_send_request(sockfd, &server, packet, packet_size,
|
2010-07-29 11:04:30 +02:00
|
|
|
options.nb_pkt, options.delay) ;
|
2010-07-13 18:20:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void receive_position()
|
|
|
|
{
|
|
|
|
// Position of the mobile as computed by the infrastructure:
|
|
|
|
float x, y, z ;
|
|
|
|
|
|
|
|
sockfd = create_udp_listening_socket(options.listening_port) ;
|
|
|
|
recvfrom(sockfd, &x, sizeof(float), 0, NULL, NULL) ;
|
|
|
|
recvfrom(sockfd, &y, sizeof(float), 0, NULL, NULL) ;
|
|
|
|
recvfrom(sockfd, &z, sizeof(float), 0, NULL, NULL) ;
|
|
|
|
(void) close(sockfd) ;
|
|
|
|
|
|
|
|
printf("Computed position: (%.4f;%.4f;%.4f)\n", x, y, z) ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2010-07-13 16:59:45 +02:00
|
|
|
void print_usage()
|
2010-07-13 16:32:16 +02:00
|
|
|
{
|
|
|
|
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"
|
2010-10-25 13:06:55 +02:00
|
|
|
"\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
|
|
|
|
" [-n nb_packets] direction x y z\n"
|
2010-07-13 16:32:16 +02:00
|
|
|
"\n"
|
|
|
|
"Options:\n"
|
2010-07-13 17:02:48 +02:00
|
|
|
"\t-h\t\tPrint this help.\n"
|
2010-07-13 16:32:16 +02:00
|
|
|
"\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"
|
2010-07-29 10:56:50 +02:00
|
|
|
"\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"
|
2010-07-13 16:32:16 +02:00
|
|
|
"\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"
|
|
|
|
,
|
2010-07-13 16:59:45 +02:00
|
|
|
program_name,
|
|
|
|
program_name,
|
2010-07-13 16:32:16 +02:00
|
|
|
LOC_REQUEST_DEFAULT_PORT,
|
2010-07-29 10:56:50 +02:00
|
|
|
DEFAULT_DELAY_NORMAL,
|
|
|
|
DEFAULT_DELAY_CALIB,
|
|
|
|
DEFAULT_NBPKT_NORMAL,
|
|
|
|
DEFAULT_NBPKT_CALIB,
|
2010-07-13 16:32:16 +02:00
|
|
|
MOBILE_DEFAULT_PORT
|
|
|
|
) ;
|
|
|
|
}
|