From 9eb3e02b2c3234868e6ef6ead0853ae5ba7f3a6c Mon Sep 17 00:00:00 2001
From: Florian Taillard <floriantaillard@free.fr>
Date: Tue, 5 Apr 2011 17:23:28 +0200
Subject: [PATCH] [ARDrone] Update owlps-drone to new format

Receive and processing a new format of message string.
Add owlps-drone.h for prototype.
---
 owlps-ardrone/owlps-drone.c | 76 +++++++++++++++++++++++++++++++------
 owlps-ardrone/owlps-drone.h | 11 ++++++
 2 files changed, 76 insertions(+), 11 deletions(-)
 create mode 100644 owlps-ardrone/owlps-drone.h

diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c
index cf018a2..4d4b482 100644
--- a/owlps-ardrone/owlps-drone.c
+++ b/owlps-ardrone/owlps-drone.c
@@ -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, &timestampXYZ, 50, 0, NULL, NULL) ;
+  char timestampXYZ[128] ;
+  recvfrom(sockreceivefd, &timestampXYZ, 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", &timestamp.tv_sec) ;
+
+  //Lecture TimeStamp2
   ptr = strtok(NULL, ";") ;
   sscanf(ptr, "%ld", &timestamp.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)
diff --git a/owlps-ardrone/owlps-drone.h b/owlps-ardrone/owlps-drone.h
new file mode 100644
index 0000000..42a6e13
--- /dev/null
+++ b/owlps-ardrone/owlps-drone.h
@@ -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;
+};