[ARDrone] Update owlps-drone to multi-algo
Create a array for receive multi-algo. Move all prototypes to .h. Add error message and ignore corrupted trame. Rename variables i->count_algo , y->count_print.
This commit is contained in:
parent
9eb3e02b2c
commit
9f77c6c91b
|
@ -16,40 +16,6 @@
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <pthread.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 */
|
/* Options */
|
||||||
struct timespec timestamp ;
|
struct timespec timestamp ;
|
||||||
|
@ -105,7 +71,7 @@ int main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
(void) close(socksendfd) ;
|
(void) close(socksendfd) ;
|
||||||
(void) close(sockreceivefd) ;
|
(void) close(sockreceivefd) ;
|
||||||
//(void) close(socksendlvfd) ;
|
//(void) close(socksendlvfd) ; //Use for close labview socket ; for next use
|
||||||
return 0 ;
|
return 0 ;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -408,106 +374,150 @@ void receive_position()
|
||||||
{
|
{
|
||||||
// Position of the mobile as computed by the infrastructure:
|
// Position of the mobile as computed by the infrastructure:
|
||||||
char timestampXYZ[128] ;
|
char timestampXYZ[128] ;
|
||||||
|
char *data_cpy ;
|
||||||
recvfrom(sockreceivefd, ×tampXYZ, 128, 0, NULL, NULL) ;
|
recvfrom(sockreceivefd, ×tampXYZ, 128, 0, NULL, NULL) ;
|
||||||
//printf("%s", timestampXYZ) ;
|
//send_labview(timestampXYZ) ; // Use for future
|
||||||
//send_labview(timestampXYZ) ;
|
data_cpy = strdup(timestampXYZ) ;
|
||||||
string2data(timestampXYZ) ;
|
string2data(data_cpy) ;
|
||||||
}
|
}
|
||||||
|
|
||||||
void string2data(char* string_data)
|
void string2data(char* string_data)
|
||||||
{
|
{
|
||||||
|
/*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
|
||||||
|
*/
|
||||||
|
|
||||||
char *mac = NULL ;
|
char *mac = NULL ;
|
||||||
int type_req ;
|
char *ptr = NULL ;
|
||||||
int i ;
|
|
||||||
int y ;
|
|
||||||
char *ptr= NULL ;
|
|
||||||
char *delims = ";" ;
|
char *delims = ";" ;
|
||||||
|
|
||||||
//Lecture Adresse Mac
|
int type_req = 0 ;
|
||||||
ptr = strtok(string_data, delims) ;
|
int count_algo= 0 ;
|
||||||
mac = ptr ;
|
int count_print = 0 ;
|
||||||
|
int onetime = 0 ;
|
||||||
//Lecture Type Request
|
|
||||||
ptr = strtok(NULL, delims) ;
|
|
||||||
type_req = atoi(ptr) ;
|
|
||||||
|
|
||||||
//Lecture TimeStamp1
|
while(onetime<1)
|
||||||
ptr = strtok(NULL, ".") ;
|
{
|
||||||
sscanf(ptr, "%ld", ×tamp.tv_sec) ;
|
|
||||||
|
//Lecture Adresse Mac
|
||||||
|
ptr = strtok(string_data, delims) ;
|
||||||
|
if(ptr==NULL)
|
||||||
|
{
|
||||||
|
print_error("mac") ;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
mac = ptr ;
|
||||||
|
|
||||||
//Lecture TimeStamp2
|
//Lecture Type Request
|
||||||
ptr = strtok(NULL, ";") ;
|
ptr = strtok(NULL, delims) ;
|
||||||
sscanf(ptr, "%ld", ×tamp.tv_nsec) ;
|
if (ptr==NULL)
|
||||||
|
{
|
||||||
|
print_error("request") ;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
type_req = atoi(ptr) ;
|
||||||
|
|
||||||
|
//Lecture TimeStamp1
|
||||||
|
ptr = strtok(NULL, ".") ;
|
||||||
|
if (ptr==NULL)
|
||||||
|
{
|
||||||
|
print_error("timestamp") ;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
sscanf(ptr, "%ld", ×tamp.tv_sec) ;
|
||||||
|
|
||||||
|
//Lecture TimeStamp2
|
||||||
|
ptr = strtok(NULL, ";") ;
|
||||||
|
if (ptr==NULL)
|
||||||
|
{
|
||||||
|
print_error("timestamp") ;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
sscanf(ptr, "%ld", ×tamp.tv_nsec) ;
|
||||||
|
onetime++;
|
||||||
|
}
|
||||||
result results[10];
|
result results[10];
|
||||||
i=0;
|
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
|
//Lecture de l'algorythme utilisé
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
if(ptr==NULL) break;
|
if(ptr==NULL) break ;
|
||||||
sscanf(ptr, "%s", results[i].algo) ;
|
sscanf(ptr, "%s", results[count_algo].algo) ;
|
||||||
|
|
||||||
|
//Lecture du point X
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
if(ptr==NULL) break;
|
if(ptr==NULL)
|
||||||
results[i].x = atof(ptr) ;
|
{
|
||||||
|
if(count_algo==0)
|
||||||
|
{
|
||||||
|
print_error ("trame");
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
print_error ("algo");
|
||||||
|
count_algo-- ;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
results[count_algo].x = atof(ptr) ;
|
||||||
|
|
||||||
|
//Lecture du point Y
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
if(ptr==NULL) break;
|
if(ptr==NULL)
|
||||||
results[i].y = atof(ptr) ;
|
{
|
||||||
|
if(count_algo==0)
|
||||||
|
{
|
||||||
|
print_error ("trame");
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
print_error ("algo");
|
||||||
|
count_algo--;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
results[count_algo].y = atof(ptr) ;
|
||||||
|
|
||||||
|
//Lecture du point Z
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
if(ptr==NULL) break;
|
if(ptr==NULL)
|
||||||
results[i].z = atof(ptr) ;
|
{
|
||||||
i++;
|
if(count_algo==0)
|
||||||
|
{
|
||||||
|
print_error ("trame");
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
perror ("algo");
|
||||||
|
count_algo--;
|
||||||
|
break ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
results[count_algo].z = atof(ptr) ;
|
||||||
|
|
||||||
|
count_algo++;
|
||||||
|
|
||||||
}
|
}
|
||||||
for(y=0;y<i;y++)
|
|
||||||
|
for(count_print=0;count_print<count_algo;count_print++)
|
||||||
{
|
{
|
||||||
printf("\n----------------\n"
|
printf("\n----------------\n"
|
||||||
" Adresse Mac : %s\n"
|
" Adresse Mac : %s\n"
|
||||||
" Type de requete : %d\n"
|
" Type de requete : %d\n"
|
||||||
" Timestamp : %ld . %ld\n"
|
" Timestamp : %ld.%ld\n"
|
||||||
" Algo : %s\n"
|
" Algo : %s\n"
|
||||||
" X : %f\n "
|
" X : %f\n "
|
||||||
" Y : %f\n"
|
" Y : %f\n"
|
||||||
" Z : %f\n"
|
" Z : %f\n"
|
||||||
"------------------\n", mac, type_req, timestamp.tv_sec, timestamp.tv_nsec, results[y].algo, results[y].x, results[y].y, results[y].z);
|
"------------------\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);
|
||||||
}
|
}
|
||||||
|
|
||||||
//printf("\nsec : %ld nsec : %ld \n", timestamp.tv_sec, timestamp.tv_nsec) ;
|
|
||||||
/*
|
|
||||||
while(1)
|
|
||||||
{
|
|
||||||
switch(i)
|
|
||||||
{
|
|
||||||
|
|
||||||
case 0:
|
|
||||||
ptr = strtok(NULL, delims) ;
|
|
||||||
sscanf(ptr, "%s", &algo) ;
|
|
||||||
i++ ;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 1:
|
|
||||||
ptr = strtok(NULL, delims) ;
|
|
||||||
sscanf(ptr, "%f", &x) ;
|
|
||||||
i++;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 2:
|
|
||||||
ptr = strtok(NULL, delims) ;
|
|
||||||
sscanf(ptr, "%f", &y) ;
|
|
||||||
i++;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 3:
|
|
||||||
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)
|
void send_labview(char* string_data)
|
||||||
|
@ -517,6 +527,13 @@ void send_labview(char* string_data)
|
||||||
//printf("Send to labView : %s", string_data) ;
|
//printf("Send to labView : %s", string_data) ;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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") ;
|
||||||
|
}
|
||||||
|
|
||||||
void print_usage()
|
void print_usage()
|
||||||
{
|
{
|
||||||
printf("Usage:\n"
|
printf("Usage:\n"
|
||||||
|
|
|
@ -1,7 +1,46 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#define ALGO_STRLEN 15
|
|
||||||
|
|
||||||
|
#define DEBUG
|
||||||
|
|
||||||
|
/* Lenght of string algo */
|
||||||
|
#define ALGO_STRLEN 15
|
||||||
|
/* 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*) ;
|
||||||
|
void print_error(char*) ;
|
||||||
typedef struct result result;
|
typedef struct result result;
|
||||||
|
|
||||||
|
/* Struct */
|
||||||
|
|
||||||
struct result
|
struct result
|
||||||
{
|
{
|
||||||
char algo[ALGO_STRLEN];
|
char algo[ALGO_STRLEN];
|
||||||
|
|
Loading…
Reference in New Issue