[ARDrone] Update owlps-drone to new format
Receive and processing a new format of message string. Add owlps-drone.h for prototype.
This commit is contained in:
parent
b33aadd8ad
commit
9eb3e02b2c
|
@ -3,7 +3,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "../libowlps-client/owlps-client.h"
|
#include "../libowlps-client/owlps-client.h"
|
||||||
|
#include "owlps-drone.h"
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
@ -407,42 +407,95 @@ void send_request()
|
||||||
void receive_position()
|
void receive_position()
|
||||||
{
|
{
|
||||||
// Position of the mobile as computed by the infrastructure:
|
// Position of the mobile as computed by the infrastructure:
|
||||||
char timestampXYZ[50] ;
|
char timestampXYZ[128] ;
|
||||||
recvfrom(sockreceivefd, ×tampXYZ, 50, 0, NULL, NULL) ;
|
recvfrom(sockreceivefd, ×tampXYZ, 128, 0, NULL, NULL) ;
|
||||||
//printf("%s", timestampXYZ) ;
|
//printf("%s", timestampXYZ) ;
|
||||||
send_labview(timestampXYZ) ;
|
//send_labview(timestampXYZ) ;
|
||||||
string2data(timestampXYZ) ;
|
string2data(timestampXYZ) ;
|
||||||
}
|
}
|
||||||
|
|
||||||
void string2data(char* string_data)
|
void string2data(char* string_data)
|
||||||
{
|
{
|
||||||
float x, y, z = 0.0 ;
|
char *mac = NULL ;
|
||||||
int i = 0 ;
|
int type_req ;
|
||||||
|
int i ;
|
||||||
|
int y ;
|
||||||
char *ptr= NULL ;
|
char *ptr= NULL ;
|
||||||
char *delims = ";" ;
|
char *delims = ";" ;
|
||||||
|
|
||||||
ptr = strtok(string_data, ".") ;
|
//Lecture Adresse Mac
|
||||||
|
ptr = strtok(string_data, delims) ;
|
||||||
|
mac = ptr ;
|
||||||
|
|
||||||
|
//Lecture Type Request
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
type_req = atoi(ptr) ;
|
||||||
|
|
||||||
|
//Lecture TimeStamp1
|
||||||
|
ptr = strtok(NULL, ".") ;
|
||||||
sscanf(ptr, "%ld", ×tamp.tv_sec) ;
|
sscanf(ptr, "%ld", ×tamp.tv_sec) ;
|
||||||
|
|
||||||
|
//Lecture TimeStamp2
|
||||||
ptr = strtok(NULL, ";") ;
|
ptr = strtok(NULL, ";") ;
|
||||||
sscanf(ptr, "%ld", ×tamp.tv_nsec) ;
|
sscanf(ptr, "%ld", ×tamp.tv_nsec) ;
|
||||||
|
|
||||||
|
result results[10];
|
||||||
|
i=0;
|
||||||
|
while(1)
|
||||||
|
{
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
if(ptr==NULL) break;
|
||||||
|
sscanf(ptr, "%s", results[i].algo) ;
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
if(ptr==NULL) break;
|
||||||
|
results[i].x = atof(ptr) ;
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
if(ptr==NULL) break;
|
||||||
|
results[i].y = atof(ptr) ;
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
if(ptr==NULL) break;
|
||||||
|
results[i].z = atof(ptr) ;
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
for(y=0;y<i;y++)
|
||||||
|
{
|
||||||
|
printf("\n----------------\n"
|
||||||
|
" Adresse Mac : %s\n"
|
||||||
|
" Type de requete : %d\n"
|
||||||
|
" Timestamp : %ld . %ld\n"
|
||||||
|
" Algo : %s\n"
|
||||||
|
" X : %f\n "
|
||||||
|
" Y : %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);
|
||||||
|
}
|
||||||
|
|
||||||
//printf("\nsec : %ld nsec : %ld \n", timestamp.tv_sec, timestamp.tv_nsec) ;
|
//printf("\nsec : %ld nsec : %ld \n", timestamp.tv_sec, timestamp.tv_nsec) ;
|
||||||
while(i<3)
|
/*
|
||||||
|
while(1)
|
||||||
{
|
{
|
||||||
switch(i)
|
switch(i)
|
||||||
{
|
{
|
||||||
|
|
||||||
case 0:
|
case 0:
|
||||||
|
ptr = strtok(NULL, delims) ;
|
||||||
|
sscanf(ptr, "%s", &algo) ;
|
||||||
|
i++ ;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
sscanf(ptr, "%f", &x) ;
|
sscanf(ptr, "%f", &x) ;
|
||||||
i++;
|
i++;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 1:
|
case 2:
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
sscanf(ptr, "%f", &y) ;
|
sscanf(ptr, "%f", &y) ;
|
||||||
i++;
|
i++;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 3:
|
||||||
ptr = strtok(NULL, delims) ;
|
ptr = strtok(NULL, delims) ;
|
||||||
sscanf(ptr, "%f", &z) ;
|
sscanf(ptr, "%f", &z) ;
|
||||||
i++;
|
i++;
|
||||||
|
@ -453,7 +506,8 @@ void string2data(char* string_data)
|
||||||
break;
|
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) ;
|
*/
|
||||||
|
// 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)
|
||||||
|
|
|
@ -0,0 +1,11 @@
|
||||||
|
#include <stdio.h>
|
||||||
|
#define ALGO_STRLEN 15
|
||||||
|
|
||||||
|
typedef struct result result;
|
||||||
|
struct result
|
||||||
|
{
|
||||||
|
char algo[ALGO_STRLEN];
|
||||||
|
float x;
|
||||||
|
float y;
|
||||||
|
float z;
|
||||||
|
};
|
Loading…
Reference in New Issue