Add owlps-ardrone
Owlps-ardrone is a client owlps modified for embedded system Ar.drone. The program send a request of positioning to the network and receive estimate position. In the future, the program control a direction of the Ar.Drone to target.
This commit is contained in:
Florian Taillard
Matteo Cypriani
parent
40fb159a91
commit
12b52d9365
|
|
@ -0,0 +1,98 @@
|
|||
# Répertoire d'installation
|
||||
PREFIX=/usr/local
|
||||
INSTALL_DIR= $(PREFIX)/bin
|
||||
INSTALL_LIB= $(PREFIX)/lib
|
||||
INSTALL_INC= $(PREFIX)/include
|
||||
INSTALL_MAN= $(PREFIX)/share/man
|
||||
|
||||
# Compilateur
|
||||
COLORGCC := $(shell which colorgcc >/dev/null 2>&1 ; echo $$?)
|
||||
ifeq ($(COLORGCC), 0)
|
||||
CC = colorgcc
|
||||
endif
|
||||
|
||||
# Cross_compil
|
||||
#CC = /arm/bin/arm-none-linux-gnueabi-gcc
|
||||
|
||||
# Commandes d'installation et de désinstallation
|
||||
RM=rm -fv
|
||||
CP=cp -v
|
||||
|
||||
# Cible
|
||||
TARGET=owlps-client
|
||||
HEADER=
|
||||
|
||||
# Flags
|
||||
CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -O -I.
|
||||
DEPFLAGS = -MMD
|
||||
XCFLAGS = $(CFLAGS) $(DEPFLAGS) $(WARN) $(HEADERS)
|
||||
PICFLAG = -fPIC
|
||||
LIBS = -L../../libowlps -lowlps -L../libowlps-client -lowlps-client
|
||||
SEMISTATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
STATIC_LIBS = -liw -lm -lrt
|
||||
|
||||
|
||||
## Cibles de compilation standard ##
|
||||
|
||||
.PHONY : all dynamic static install uninstall clean purge help
|
||||
|
||||
dynamic : $(TARGET)
|
||||
static : $(TARGET).static
|
||||
semistatic: $(TARGET).semistatic
|
||||
all : dynamic static
|
||||
|
||||
% : %.o
|
||||
$(CC) $(LDFLAGS) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LIBS)
|
||||
%.o : %.c $(HEADER)
|
||||
$(CC) $(XCFLAGS) -c $<
|
||||
|
||||
# Compilation du programme
|
||||
$(TARGET) : $(TARGET).o $(HEADER)
|
||||
|
||||
$(TARGET).semistatic: $(TARGET).c $(HEADER)
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(SEMISTATIC_LIBS)
|
||||
|
||||
$(TARGET).static : $(TARGET).c $(HEADER)
|
||||
$(CC) $(LDFLAGS) -static $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LIBS) $(STATIC_LIBS)
|
||||
|
||||
|
||||
## Installation / désinstallation ##
|
||||
|
||||
install : $(TARGET)
|
||||
@$(CP) $(TARGET) $(INSTALL_DIR)
|
||||
@cd $(INSTALL_DIR) ; chown root:root $(TARGET) ; chmod 755 $(TARGET)
|
||||
|
||||
install-static : $(TARGET).static
|
||||
@$(CP) $(TARGET).static $(INSTALL_DIR)
|
||||
@cd $(INSTALL_DIR) ; chown root:root $(TARGET).static ; chmod 755 $(TARGET).static
|
||||
|
||||
uninstall :
|
||||
@$(RM) $(INSTALL_DIR)/{$(TARGET),$(TARGET).static}
|
||||
|
||||
|
||||
## Nettoyage ##
|
||||
|
||||
clean :
|
||||
@$(RM) -fv *~ *.o *.d
|
||||
|
||||
purge : clean
|
||||
@$(RM) -fv $(TARGET) $(TARGET).static
|
||||
|
||||
|
||||
## Aide ##
|
||||
|
||||
help :
|
||||
@echo -e "Bibliothèques nécessaires à la compilation :\n\
|
||||
libowlps1.0 (fournie)\n\
|
||||
\n\
|
||||
Cibles possibles :\n\
|
||||
$(TARGET) (cible par défaut) : Compile le programme $(TARGET).\n\
|
||||
$(TARGET).static : Compile le programme $(TARGET).static \
|
||||
(version sans lien dynamique).\n\
|
||||
install : Installe le programme $(TARGET).\n\
|
||||
uninstall : Désinstalle le programme $(TARGET).\n\
|
||||
clean : Supprime les fichiers temporaires.\n\
|
||||
purge : Supprime le résultat de la compilation.\n\
|
||||
\n\
|
||||
Note : l'installation se fait dans l'arborescence $(PREFIX). \
|
||||
Modifiez la variable PREFIX du Makefile pour changer ce comportement."
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
# Emplacement de la suite de cross-compilation
|
||||
TOOLCHAIN_PREFIX = $$HOME/openwrt/atheros/8.09
|
||||
TOOLCHAIN_BIN = $(TOOLCHAIN_PREFIX)/staging_dir/toolchain-mips_gcc4.1.2/bin
|
||||
TOOLCHAIN_USR = $(TOOLCHAIN_PREFIX)/staging_dir/mips/usr
|
||||
TOOLCHAIN_USR_2 = $(TOOLCHAIN_PREFIX)/build_dir/mips/OpenWrt-SDK-atheros-for-Linux-i686/staging_dir/toolchain-mips_gcc4.1.2
|
||||
|
||||
# Répertoire d'installation
|
||||
PREFIX=/usr/local
|
||||
INSTALL_DIR= $(PREFIX)/bin
|
||||
INSTALL_LIB= $(PREFIX)/lib
|
||||
INSTALL_INC= $(PREFIX)/include
|
||||
INSTALL_MAN= $(PREFIX)/share/man
|
||||
|
||||
# Compilateur
|
||||
CC = $(TOOLCHAIN_BIN)/mips-linux-gcc
|
||||
|
||||
# Commandes d'installation et de désinstallation
|
||||
RM=rm -fv
|
||||
CP=cp -v
|
||||
|
||||
# Cible
|
||||
TARGET=owlps-client
|
||||
HEADER=
|
||||
|
||||
# Flags
|
||||
CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -I.
|
||||
DEPFLAGS = -MMD
|
||||
XCFLAGS = $(CFLAGS) $(DEPFLAGS) $(WARN) $(HEADERS)
|
||||
PICFLAG = -fPIC
|
||||
LOWLPS = -L../../libowlps -lowlps
|
||||
LOWLPSA = ../../libowlps/libowlps.a
|
||||
LOWLPSCLIENTA = ../libowlps-client/libowlps-client.a
|
||||
LIBS = $(LOWLPSCLIENTA) -liw -lm
|
||||
DYNAMIC_LIBS = $(LOWLPS) $(LIBS)
|
||||
SEMISTATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
STATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
LATHEROS = -L$(TOOLCHAIN_USR)/lib -L$(TOOLCHAIN_USR_2)/lib
|
||||
IATHEROS = -I$(TOOLCHAIN_USR)/include -I$(TOOLCHAIN_USR_2)/include
|
||||
LDFLAGS = $(IATHEROS) $(LATHEROS)
|
||||
|
||||
|
||||
## Cibles de compilation standard ##
|
||||
|
||||
.PHONY: all dynamic semistatic static install uninstall clean purge help
|
||||
|
||||
dynamic: $(TARGET)
|
||||
semistatic: $(TARGET).semistatic
|
||||
static: $(TARGET).static
|
||||
all: dynamic semistatic static
|
||||
|
||||
# Cancel implicit make rule
|
||||
%: %.c
|
||||
|
||||
%: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(DYNAMIC_LIBS)
|
||||
|
||||
%.semistatic: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(SEMISTATIC_LIBS)
|
||||
|
||||
%.static: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(STATIC_LIBS) \
|
||||
-static
|
||||
|
||||
%.o: %.c $(HEADER)
|
||||
$(CC) $(XCFLAGS) $(IATHEROS) $(OWLPSFLAGS) -c $<
|
||||
|
||||
|
||||
## Installation / désinstallation ##
|
||||
|
||||
install : $(TARGET)
|
||||
@$(CP) $(TARGET) $(INSTALL_DIR)
|
||||
@cd $(INSTALL_DIR) ; chown root:root $(TARGET) ; chmod 755 $(TARGET)
|
||||
|
||||
uninstall :
|
||||
@$(RM) $(INSTALL_DIR)/$(TARGET)
|
||||
|
||||
|
||||
## Nettoyage ##
|
||||
|
||||
clean :
|
||||
@$(RM) -fv *~ *.o *.d
|
||||
|
||||
purge : clean
|
||||
@$(RM) -fv $(TARGET) $(TARGET).static $(TARGET).semistatic
|
||||
|
||||
|
||||
## Aide ##
|
||||
|
||||
help :
|
||||
@echo -e "Bibliothèques nécessaires à la compilation :\n\
|
||||
libowlps1.0 (fournie)\n\
|
||||
\n\
|
||||
Cibles possibles :\n\
|
||||
$(TARGET) (cible par défaut) : Compile le programme $(TARGET).\n\
|
||||
$(TARGET).static : Compile le programme $(TARGET).static \
|
||||
(version sans lien dynamique).\n\
|
||||
$(TARGET).semistatic : Compile le programme $(TARGET).semistatic \
|
||||
(version sans lien dynamique vers libowlps).\n\
|
||||
install : Installe le programme $(TARGET).\n\
|
||||
uninstall : Désinstalle le programme $(TARGET).\n\
|
||||
clean : Supprime les fichiers temporaires.\n\
|
||||
purge : Supprime le résultat de la compilation.\n\
|
||||
\n\
|
||||
Note : l'installation se fait dans l'arborescence $(PREFIX). \
|
||||
Modifiez la variable PREFIX du Makefile pour changer ce comportement."
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
# Emplacement de la suite de cross-compilation
|
||||
TOOLCHAIN_PREFIX = $$HOME/openwrt/atheros/10.03
|
||||
TOOLCHAIN_BIN = $(TOOLCHAIN_PREFIX)/staging_dir/toolchain-mips_gcc-4.3.3+cs_uClibc-0.9.30.1/usr/bin
|
||||
TOOLCHAIN_USR = $(TOOLCHAIN_PREFIX)/staging_dir/toolchain-mips_gcc-4.3.3+cs_uClibc-0.9.30.1/usr
|
||||
TOOLCHAIN_USR_2 = $(TOOLCHAIN_PREFIX)/staging_dir/target-mips_uClibc-0.9.30.1/usr
|
||||
|
||||
# Répertoire d'installation
|
||||
PREFIX=/usr/local
|
||||
INSTALL_DIR= $(PREFIX)/bin
|
||||
INSTALL_LIB= $(PREFIX)/lib
|
||||
INSTALL_INC= $(PREFIX)/include
|
||||
INSTALL_MAN= $(PREFIX)/share/man
|
||||
|
||||
# Compilateur
|
||||
CC = $(TOOLCHAIN_BIN)/mips-openwrt-linux-gcc
|
||||
|
||||
# Commandes d'installation et de désinstallation
|
||||
RM=rm -fv
|
||||
CP=cp -v
|
||||
|
||||
# Cible
|
||||
TARGET=owlps-client
|
||||
HEADER=
|
||||
|
||||
# Flags
|
||||
CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -I.
|
||||
DEPFLAGS = -MMD
|
||||
XCFLAGS = $(CFLAGS) $(DEPFLAGS) $(WARN) $(HEADERS)
|
||||
PICFLAG = -fPIC
|
||||
LOWLPS = -L../../libowlps -lowlps
|
||||
LOWLPSA = ../../libowlps/libowlps.a
|
||||
LOWLPSCLIENTA = ../libowlps-client/libowlps-client.a
|
||||
LIBS = $(LOWLPSCLIENTA) -liw -lm
|
||||
DYNAMIC_LIBS = $(LOWLPS) $(LIBS)
|
||||
SEMISTATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
STATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
LATHEROS = -L$(TOOLCHAIN_USR)/lib -L$(TOOLCHAIN_USR_2)/lib
|
||||
IATHEROS = -I$(TOOLCHAIN_USR)/include -I$(TOOLCHAIN_USR_2)/include
|
||||
LDFLAGS = $(IATHEROS) $(LATHEROS)
|
||||
|
||||
|
||||
## Cibles de compilation standard ##
|
||||
|
||||
.PHONY: all dynamic semistatic static install uninstall clean purge help
|
||||
|
||||
dynamic: $(TARGET)
|
||||
semistatic: $(TARGET).semistatic
|
||||
static: $(TARGET).static
|
||||
all: dynamic semistatic static
|
||||
|
||||
# Cancel implicit make rule
|
||||
%: %.c
|
||||
|
||||
%: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(DYNAMIC_LIBS)
|
||||
|
||||
%.semistatic: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(SEMISTATIC_LIBS)
|
||||
|
||||
%.static: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(STATIC_LIBS) \
|
||||
-static
|
||||
|
||||
%.o: %.c $(HEADER)
|
||||
$(CC) $(XCFLAGS) $(IATHEROS) $(OWLPSFLAGS) -c $<
|
||||
|
||||
|
||||
## Installation / désinstallation ##
|
||||
|
||||
install : $(TARGET)
|
||||
@$(CP) $(TARGET) $(INSTALL_DIR)
|
||||
@cd $(INSTALL_DIR) ; chown root:root $(TARGET) ; chmod 755 $(TARGET)
|
||||
|
||||
uninstall :
|
||||
@$(RM) $(INSTALL_DIR)/$(TARGET)
|
||||
|
||||
|
||||
## Nettoyage ##
|
||||
|
||||
clean :
|
||||
@$(RM) -fv *~ *.o *.d
|
||||
|
||||
purge : clean
|
||||
@$(RM) -fv $(TARGET) $(TARGET).static $(TARGET).semistatic
|
||||
|
||||
|
||||
## Aide ##
|
||||
|
||||
help :
|
||||
@echo -e "Bibliothèques nécessaires à la compilation :\n\
|
||||
libowlps1.0 (fournie)\n\
|
||||
\n\
|
||||
Cibles possibles :\n\
|
||||
$(TARGET) (cible par défaut) : Compile le programme $(TARGET).\n\
|
||||
$(TARGET).static : Compile le programme $(TARGET).static \
|
||||
(version sans lien dynamique).\n\
|
||||
$(TARGET).semistatic : Compile le programme $(TARGET).semistatic \
|
||||
(version sans lien dynamique vers libowlps).\n\
|
||||
install : Installe le programme $(TARGET).\n\
|
||||
uninstall : Désinstalle le programme $(TARGET).\n\
|
||||
clean : Supprime les fichiers temporaires.\n\
|
||||
purge : Supprime le résultat de la compilation.\n\
|
||||
\n\
|
||||
Note : l'installation se fait dans l'arborescence $(PREFIX). \
|
||||
Modifiez la variable PREFIX du Makefile pour changer ce comportement."
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
# Emplacement de la suite de cross-compilation
|
||||
TOOLCHAIN_PREFIX = /arm
|
||||
TOOLCHAIN_BIN = $(TOOLCHAIN_PREFIX)/bin
|
||||
TOOLCHAIN_USR = $(TOOLCHAIN_PREFIX)/arm-none-linux-gnueabi
|
||||
TOOLCHAIN_USR_2 = $(TOOLCHAIN_PREFIX)/arm-none-linux-gnueabi/libc
|
||||
|
||||
# Compilateur
|
||||
CC = $(TOOLCHAIN_BIN)/arm-none-linux-gnueabi-gcc
|
||||
|
||||
# Répertoire d'installation
|
||||
PREFIX=/usr/local
|
||||
INSTALL_DIR= $(PREFIX)/bin
|
||||
INSTALL_LIB= $(PREFIX)/lib
|
||||
INSTALL_INC= $(PREFIX)/include
|
||||
INSTALL_MAN= $(PREFIX)/share/man
|
||||
|
||||
# Commandes d'installation et de désinstallation
|
||||
RM=rm -fv
|
||||
CP=cp -v
|
||||
|
||||
# Cible
|
||||
TARGET=owlps-
|
||||
HEADER=
|
||||
|
||||
# Flags
|
||||
CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -I.
|
||||
DEPFLAGS = -MMD
|
||||
XCFLAGS = $(CFLAGS) $(DEPFLAGS) $(WARN) $(HEADERS)
|
||||
PICFLAG = -fPIC
|
||||
LOWLPS = -L../../libowlps -lowlps
|
||||
LOWLPSA = ../../libowlps/libowlps.a
|
||||
LOWLPSCLIENTA = ../libowlps-client/libowlps-client.a
|
||||
LIBS = $(LOWLPSCLIENTA) -liw -lm -lrt
|
||||
DYNAMIC_LIBS = $(LOWLPS) $(LIBS)
|
||||
SEMISTATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
STATIC_LIBS = $(LOWLPSA) $(LIBS)
|
||||
LATHEROS = -L$(TOOLCHAIN_USR)/lib -L$(TOOLCHAIN_USR_2)/lib
|
||||
IATHEROS = -I$(TOOLCHAIN_USR)/include -I$(TOOLCHAIN_USR_2)/include
|
||||
LDFLAGS = $(IATHEROS) $(LATHEROS)
|
||||
|
||||
|
||||
## Cibles de compilation standard ##
|
||||
|
||||
.PHONY: all semistatic static install uninstall clean purge help
|
||||
|
||||
semistatic: $(TARGET)-drone
|
||||
static: $(TARGET).static
|
||||
all: semistatic static
|
||||
|
||||
# Cancel implicit make rule
|
||||
%: %.c
|
||||
|
||||
%: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(DYNAMIC_LIBS)
|
||||
|
||||
%-ardrone: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(SEMISTATIC_LIBS)
|
||||
|
||||
%.static: %.o
|
||||
$(CC) $(STRIPFLAGS) $(XCFLAGS) -o $@ $^ $(LDFLAGS) $(STATIC_LIBS) \
|
||||
-static
|
||||
|
||||
%.o: %.c $(HEADER)
|
||||
$(CC) $(XCFLAGS) $(IATHEROS) $(OWLPSFLAGS) -c $<
|
||||
|
||||
|
||||
## Installation / désinstallation ##
|
||||
|
||||
install : $(TARGET)
|
||||
@$(CP) $(TARGET) $(INSTALL_DIR)
|
||||
@cd $(INSTALL_DIR) ; chown root:root $(TARGET) ; chmod 755 $(TARGET)
|
||||
|
||||
uninstall :
|
||||
@$(RM) $(INSTALL_DIR)/$(TARGET)
|
||||
|
||||
|
||||
## Nettoyage ##
|
||||
|
||||
clean :
|
||||
@$(RM) -fv *~ *.o *.d
|
||||
|
||||
purge : clean
|
||||
@$(RM) -fv $(TARGET) $(TARGET).static $(TARGET)-ardrone
|
||||
|
||||
|
||||
## Aide ##
|
||||
|
||||
help :
|
||||
@echo -e "Bibliothèques nécessaires à la compilation :\n\
|
||||
libowlps1.0 (fournie)\n\
|
||||
\n\
|
||||
Cibles possibles :\n\
|
||||
$(TARGET) (cible par défaut) : Compile le programme $(TARGET).\n\
|
||||
$(TARGET).static : Compile le programme $(TARGET).static \
|
||||
(version sans lien dynamique).\n\
|
||||
$(TARGET).semistatic : Compile le programme $(TARGET).semistatic \
|
||||
(version sans lien dynamique vers libowlps).\n\
|
||||
install : Installe le programme $(TARGET).\n\
|
||||
uninstall : Désinstalle le programme $(TARGET).\n\
|
||||
clean : Supprime les fichiers temporaires.\n\
|
||||
purge : Supprime le résultat de la compilation.\n\
|
||||
\n\
|
||||
Note : l'installation se fait dans l'arborescence $(PREFIX). \
|
||||
Modifiez la variable PREFIX du Makefile pour changer ce comportement."
|
||||
|
|
@ -0,0 +1,482 @@
|
|||
/*
|
||||
* This file is part of the rtap localisation project.
|
||||
*/
|
||||
|
||||
#include "../libowlps-client/owlps-client.h"
|
||||
#include <time.h>
|
||||
#include <pthread.h>
|
||||
#define DEBUG
|
||||
|
||||
/* Error codes */
|
||||
#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments)
|
||||
|
||||
/* 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
|
||||
|
||||
/* Delay between two packet transmissions (in microseconds) */
|
||||
#define DEFAULT_DELAY_CALIB 50000 // Calibration request
|
||||
#define DEFAULT_DELAY_NORMAL 25000 // Localisation request
|
||||
|
||||
/* Program arguments (getopt string) */
|
||||
#define OPTIONS "d:hi:l::n:p:t:"
|
||||
|
||||
|
||||
/* Function headers */
|
||||
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
|
||||
void create_socket(void) ;
|
||||
void make_packet(void) ;
|
||||
void send_request(void) ;
|
||||
void receive_position(void) ;
|
||||
void print_usage(void) ;
|
||||
void send_labview(char*) ;
|
||||
void string2data(char*) ;
|
||||
void* thread_send(void*) ;
|
||||
|
||||
/* Options */
|
||||
struct timespec timestamp ;
|
||||
struct {
|
||||
char dest_ip[16] ; // Destination IP of the packets
|
||||
uint_fast16_t dest_port ;
|
||||
char iface[IFNAMSIZ + 1] ; // Source network interface
|
||||
int_fast32_t delay ; // Time between two packet transmissions
|
||||
uint_fast16_t nb_pkt ; // Number of packets to send
|
||||
uint_fast16_t listening_port ;
|
||||
// Calibration data:
|
||||
DIRECTION direction ;
|
||||
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:
|
||||
BOOL is_calibration_request = FALSE ;
|
||||
|
||||
int socksendfd ; // Sending socket descriptor (send positioning packets)
|
||||
int sockreceivefd ; // Receiving socket descriptor (Receive position)
|
||||
int socksendlvfd ; // Sending labview socket descriptor (Send position to labview)
|
||||
struct sockaddr_in server ; // Server info
|
||||
uint8_t *packet = NULL ; // Packet to send
|
||||
uint_fast16_t packet_size ; // Packet size
|
||||
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
program_name = argv[0] ;
|
||||
parse_command_line(argc, argv) ;
|
||||
|
||||
create_socket() ;
|
||||
pthread_t send ;
|
||||
pthread_create(&send, NULL, thread_send, NULL) ;
|
||||
while(1)
|
||||
{
|
||||
if (options.listening_port > 0)
|
||||
receive_position() ;
|
||||
}
|
||||
(void) close(socksendfd) ;
|
||||
(void) close(sockreceivefd) ;
|
||||
//(void) close(socksendlvfd) ;
|
||||
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* thread_send(void* data)
|
||||
{
|
||||
while(1)
|
||||
{
|
||||
make_packet() ;
|
||||
send_request() ;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void parse_main_options(int argc, char **argv)
|
||||
{
|
||||
int opt ;
|
||||
|
||||
while ((opt = getopt(argc, argv, OPTIONS)) != -1)
|
||||
{
|
||||
switch (opt)
|
||||
{
|
||||
case 'd' :
|
||||
strncpy(options.dest_ip, optarg, 16) ;
|
||||
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 ;
|
||||
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 ;
|
||||
}
|
||||
|
||||
// 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") ;
|
||||
#endif // DEBUG
|
||||
options.listening_port = 0 ;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
void print_configuration()
|
||||
{
|
||||
fprintf(stderr, "Options:\n"
|
||||
"\tDestination IP: %s\n"
|
||||
"\tDestination port: %"PRIuFAST16"\n"
|
||||
"\tInterface: %s\n"
|
||||
"\tDelay: %"PRIuFAST32"\n"
|
||||
"\tNumber of packets: %"PRIuFAST16"\n"
|
||||
"\tListening port: %"PRIuFAST16"\n"
|
||||
"\tDirection: %d\n"
|
||||
"\tX: %f\n"
|
||||
"\tY: %f\n"
|
||||
"\tZ: %f\n"
|
||||
,
|
||||
options.dest_ip,
|
||||
options.dest_port,
|
||||
options.iface,
|
||||
options.delay,
|
||||
options.nb_pkt,
|
||||
options.listening_port,
|
||||
options.direction,
|
||||
options.x,
|
||||
options.y,
|
||||
options.z
|
||||
) ;
|
||||
}
|
||||
#endif // DEBUG
|
||||
|
||||
|
||||
|
||||
void create_socket()
|
||||
{
|
||||
char *ip_labview = "192.168.3.2" ;
|
||||
int port_labview = 50000 ;
|
||||
socksendfd =
|
||||
owlclient_create_trx_socket(options.dest_ip, options.dest_port,
|
||||
&server, options.iface) ;
|
||||
|
||||
//socksendlvfd =
|
||||
//owlclient_create_trx_socket(ip_labview, port_labview,
|
||||
// &server, options.iface) ;
|
||||
|
||||
sockreceivefd =
|
||||
owl_create_udp_listening_socket(options.listening_port) ;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* Creates the packet to send. */
|
||||
void make_packet()
|
||||
{
|
||||
uint_fast16_t offset ; // Index used to create the packet
|
||||
TIMESTAMP request_time ;
|
||||
char request_time_str[TIMESTAMP_STR_LEN] ;
|
||||
|
||||
// Get the current time and copy it as a string before to switch it to
|
||||
// network endianess:
|
||||
owl_timestamp_now(&request_time) ;
|
||||
owl_timestamp_to_string(request_time_str, request_time) ;
|
||||
request_time = owl_hton_timestamp(request_time) ;
|
||||
|
||||
if (is_calibration_request) // Calibration packet
|
||||
{
|
||||
printf("Preparing calibration request packet…\n") ;
|
||||
|
||||
offset = 0 ;
|
||||
packet_size =
|
||||
sizeof(uint8_t) * 2 + sizeof(TIMESTAMP) + sizeof(float) * 3 ;
|
||||
packet = malloc(packet_size) ;
|
||||
|
||||
memset(&packet[offset], PACKET_TYPE_CALIBRATION, 1) ; // Packet type
|
||||
++offset ;
|
||||
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
||||
offset += sizeof(request_time) ;
|
||||
packet[offset++] = options.direction ; // Direction
|
||||
#ifdef DEBUG
|
||||
printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
|
||||
packet[offset - 1], options.x, options.y, options.z) ;
|
||||
#endif // DEBUG
|
||||
memcpy(&packet[offset], &options.x, sizeof(float)) ;
|
||||
offset += sizeof(float) ;
|
||||
memcpy(&packet[offset], &options.y, sizeof(float)) ;
|
||||
offset += sizeof(float) ;
|
||||
memcpy(&packet[offset], &options.z, sizeof(float)) ;
|
||||
}
|
||||
|
||||
else // Standard packet
|
||||
{
|
||||
printf("Preparing request packet…\n") ;
|
||||
packet_size = sizeof(uint8_t) + sizeof(TIMESTAMP) ;
|
||||
packet = malloc(packet_size) ;
|
||||
memset(&packet[0], PACKET_TYPE_NORMAL, 1) ; // Packet type
|
||||
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) ;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void receive_position()
|
||||
{
|
||||
// Position of the mobile as computed by the infrastructure:
|
||||
char timestampXYZ[50] ;
|
||||
recvfrom(sockreceivefd, ×tampXYZ, 50, 0, NULL, NULL) ;
|
||||
//printf("%s", timestampXYZ) ;
|
||||
send_labview(timestampXYZ) ;
|
||||
string2data(timestampXYZ) ;
|
||||
}
|
||||
|
||||
void string2data(char* string_data)
|
||||
{
|
||||
float x, y, z = 0.0 ;
|
||||
int i = 0 ;
|
||||
char *ptr= NULL ;
|
||||
char *delims = ";" ;
|
||||
|
||||
ptr = strtok(string_data, ".") ;
|
||||
sscanf(ptr, "%ld", ×tamp.tv_sec) ;
|
||||
ptr = strtok(NULL, ";") ;
|
||||
sscanf(ptr, "%ld", ×tamp.tv_nsec) ;
|
||||
//printf("\nsec : %ld nsec : %ld \n", timestamp.tv_sec, timestamp.tv_nsec) ;
|
||||
while(i<3)
|
||||
{
|
||||
switch(i)
|
||||
{
|
||||
case 0:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &x) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &y) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &z) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
default:
|
||||
i++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
printf("\nReceive position.......\n\tTimestamp : %ld.%ld \n\tX : %f \n\tY : %f \n\tZ : %f\n", timestamp.tv_sec, timestamp.tv_nsec, x, y , z) ;
|
||||
}
|
||||
|
||||
void send_labview(char* string_data)
|
||||
{
|
||||
//owlclient_send_request(socksendlvfd, &server, string_data, strlen(string_data),
|
||||
//1, 0) ;
|
||||
//printf("Send to labView : %s", string_data) ;
|
||||
}
|
||||
|
||||
void print_usage()
|
||||
{
|
||||
printf("Usage:\n"
|
||||
"Localisation request:\n"
|
||||
"\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
|
||||
" [-n nb_packets] [-l [port]]\n"
|
||||
"Calibration request:\n"
|
||||
"\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]"
|
||||
" [-n nb_packets] direction x y z\n"
|
||||
"\n"
|
||||
"Options:\n"
|
||||
"\t-h\t\tPrint this help.\n"
|
||||
"\t-d dest_ip\tDestination IP address of the localisation request.\n"
|
||||
"\t-p dest_port\tDestination port of the localisation request"
|
||||
" (default: %d).\n"
|
||||
"\t-t delay\tTime between each packet transmission (default: %d"
|
||||
" µs for a normal request, %d µs for a calibration request).\n"
|
||||
"\t-n nb_packets\tNumber of packet transmitted for the request"
|
||||
" (default: %d for a normal request, %d for a calibration"
|
||||
" request).\n"
|
||||
"\t-i iface\tName of the network interface used to transmit the"
|
||||
" request (e.g. \"eth2\"). If this option is absent, interface"
|
||||
" is selected automatically. You must be root to use this"
|
||||
" option.\n"
|
||||
"\t-l [port]\tWait for the computed position and display it."
|
||||
" Optional argument 'port' allows to specify the listening"
|
||||
" port (default: %d).\n"
|
||||
,
|
||||
program_name,
|
||||
program_name,
|
||||
LOC_REQUEST_DEFAULT_PORT,
|
||||
DEFAULT_DELAY_NORMAL,
|
||||
DEFAULT_DELAY_CALIB,
|
||||
DEFAULT_NBPKT_NORMAL,
|
||||
DEFAULT_NBPKT_CALIB,
|
||||
MOBILE_DEFAULT_PORT
|
||||
) ;
|
||||
}
|
||||
Loading…
Reference in New Issue