Revision 60:bf851bafc807, committed 2014-04-26

Comitter:

krobertson

Date:

Sat Apr 26 23:01:45 2014 +0000

Parent:

59:972b4eb0df94

Child:

61:aa32e17f6801

Commit message:

gps working

Changed in this revision

--- 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));

--- a/handle/mavcommands.h	Sat Apr 26 21:12:09 2014 +0000
+++ b/handle/mavcommands.h	Sat Apr 26 23:01:45 2014 +0000
@@ -62,9 +62,10 @@
 } MAV_DATA_STREAM;
 
 typedef struct MAV_LOCDATA_S{
-    uint32_t alt;
-    uint32_t lat;
-    uint32_t lon;
+    int32_t timestamp;
+    int32_t lat;
+    int32_t lon;
+    int32_t alt;
     uint16_t x;
     uint16_t y;
     uint16_t z;
@@ -100,7 +101,7 @@
     void setup(){if(!initialized){setupCmds();initialized=true;}}
     void run(){
         setup();
-        for(int i=0;i<100;i++){handleNextCmd();wait_ms(10);}
+        for(int i=0;i<100;i++){readState=0;handleNextCmd();wait_ms(1);}
     }
     static MavCmd& get(){if(mavcmd==NULL){mavcmd=new MavCmd();}return *mavcmd;}
 };

--- a/main.cpp	Sat Apr 26 21:12:09 2014 +0000
+++ b/main.cpp	Sat Apr 26 23:01:45 2014 +0000
@@ -116,10 +116,10 @@
     //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
     USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
     wait(DELAYBOOT);    
-    while(true){
-        MavCmd::get().run();
-        wait(1);
-    }
+    //while(true){
+//        MavCmd::get().run();
+//        wait(1);
+//    }
 
     USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
     USB::getSerial().printf("Check GPS\n");