Diff: handle/mavcommands.cpp

Revision:

60:bf851bafc807

Parent:

59:972b4eb0df94

Child:

61:aa32e17f6801

--- a/handle/mavcommands.cpp	Sat Apr 26 21:12:09 2014 +0000
+++ b/handle/mavcommands.cpp	Sat Apr 26 23:01:45 2014 +0000
@@ -1,7 +1,7 @@
 #include "mavcommands.h"
 
 // Maximium number of charictors to read without giving up the processor
-#define MAX_READ_LOOP 256
+#define MAX_READ_LOOP 2560
 
 #define ENABLE_MOTORS false
 
@@ -11,6 +11,8 @@
     latitude/=10000000.0f;
     longitude/=10000000.0f;
     altitude/=10000000.0f;
+    USB::getSerial().printf("Got2 lat %f\n",latitude);USB::getSerial().printf("Got lon2 %f\n",longitude);USB::getSerial().printf("Got alt2 %f\n",altitude);
+    return;
     int degrees;
     double minutes;
     degrees = (int)(latitude/100);
@@ -62,6 +64,7 @@
 char* MavCmd::getNextCmd(){
     for(int readcnt=0;readcnt<MAX_READ_LOOP;readcnt++){
         if(Mav::getSerial().readable()<=0){
+            //wait_ms(10);
             // Nothing to run, wait until next time
             continue;//return NULL;
         }else{
@@ -107,8 +110,10 @@
                 USB::getSerial().printf("USING NEW WAYPOINT!!!\n",myCmd[1],myCmd[5]);
                 // Issue count to init waypoint entry
                 Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
+                wait_ms(1);
             }else{
                 Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_DATA_STREAM,(char*)&issueStreamReq,sizeof(MAV_DATA_STREAM));
+                wait_ms(1);
             }
             //else{
             // Start sending location data
@@ -148,8 +153,9 @@
         if(myCmd[5]==33){
             for(int i=0;i<18;i++){USB::getSerial().printf("Got loc %d/18 %d\n",i,myCmd[6+i]);}
             MAV_LOCDATA* input=(MAV_LOCDATA*)&myCmd[6];
+            
             USB::getSerial().printf("Got lat %d\n",input->lat);USB::getSerial().printf("Got lon %d\n",input->lon);USB::getSerial().printf("Got alt %d\n",input->alt);
-            double lat=input->lat;double lon=input->lon;double alt=input->alt;
+            double lat=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);double alt=(double)*((int32_t*)&myCmd[10+8]);
             fixLatLonAlt(lat,lon,alt);
             USB::getSerial().printf("Got lat fixed %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt);
             DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt));