[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 "owlps-drone.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
@ -407,42 +407,95 @@ void send_request()
|
|||
void receive_position()
|
||||
{
|
||||
// Position of the mobile as computed by the infrastructure:
|
||||
char timestampXYZ[50] ;
|
||||
recvfrom(sockreceivefd, ×tampXYZ, 50, 0, NULL, NULL) ;
|
||||
char timestampXYZ[128] ;
|
||||
recvfrom(sockreceivefd, ×tampXYZ, 128, 0, NULL, NULL) ;
|
||||
//printf("%s", timestampXYZ) ;
|
||||
send_labview(timestampXYZ) ;
|
||||
//send_labview(timestampXYZ) ;
|
||||
string2data(timestampXYZ) ;
|
||||
}
|
||||
|
||||
void string2data(char* string_data)
|
||||
{
|
||||
float x, y, z = 0.0 ;
|
||||
int i = 0 ;
|
||||
char *mac = NULL ;
|
||||
int type_req ;
|
||||
int i ;
|
||||
int y ;
|
||||
char *ptr= NULL ;
|
||||
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) ;
|
||||
|
||||
//Lecture TimeStamp2
|
||||
ptr = strtok(NULL, ";") ;
|
||||
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) ;
|
||||
while(i<3)
|
||||
/*
|
||||
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 1:
|
||||
case 2:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &y) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
case 3:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &z) ;
|
||||
i++;
|
||||
|
@ -453,7 +506,8 @@ void string2data(char* string_data)
|
|||
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)
|
||||
|
|
|
@ -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