Diff: handle/mavcommands.h

Revision:

61:aa32e17f6801

Parent:

60:bf851bafc807

Child:

63:6e4afaa03d07

--- a/handle/mavcommands.h	Sat Apr 26 23:01:45 2014 +0000
+++ b/handle/mavcommands.h	Sun Apr 27 02:31:00 2014 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "mavcontrol.h"
 #include "adapt/usb.h"
+#include "packet.h"
 #include <algorithm>
 #include"handle/dataLocation.h"
 typedef struct MAV_REQUEST_LIST_S{
@@ -62,13 +63,15 @@
 } MAV_DATA_STREAM;
 
 typedef struct MAV_LOCDATA_S{
-    int32_t timestamp;
+    int32_t alt;//timestamp;
     int32_t lat;
     int32_t lon;
-    int32_t alt;
-    uint16_t x;
-    uint16_t y;
-    uint16_t z;
+    int32_t altB;
+    //int32_t ralt;
+    //int16_t x;
+    //int16_t y;
+    //int16_t z;
+    //uint16_t hdg;
 }MAV_LOCDATA;
 
 class MavCmd{
@@ -101,7 +104,27 @@
     void setup(){if(!initialized){setupCmds();initialized=true;}}
     void run(){
         setup();
-        for(int i=0;i<100;i++){readState=0;handleNextCmd();wait_ms(1);}
+        readState=0;
+        while(Mav::getSerial().readable()>0){char input=Mav::getSerial().getc();}
+        if(hasMoved()){
+            USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
+            // Issue count to init waypoint entry
+            Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+        }else{
+            USB::getSerial().printf("Grabbing Stream\n");
+            Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+        }
+        //wait_ms(10);
+        for(int i=0;i<1000;i++){
+            if(getPS().rx_ready_with_timeout(&Mav::getSerial(),0,1000)==1){
+            }else{
+                readState=0;
+            }
+            handleNextCmd();
+            wait_ms(1);
+            USB::getSerial().printf(".");
+        }
+        USB::getSerial().printf("\n");
     }
     static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
 };