Diff: handle/mavcommands.cpp

Revision:

66:5d43988d100c

Parent:

64:d4818fb7813c

--- a/handle/mavcommands.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/mavcommands.cpp	Mon May 05 13:20:35 2014 +0000
@@ -10,8 +10,8 @@
 static void fixLatLonAlt(double& latitude,double& longitude,double& altitude){
     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);
+    //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;
@@ -30,7 +30,7 @@
     USB::getSerial().printf("HM Lat %f\n",nLat);
     USB::getSerial().printf("HM Lon %f\n",nLon);
     USB::getSerial().printf("HM Alt %f\n",nAlt);
-    if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
+    //if(moveValid&&cLat==nLat&&cLon==nLon&&cAlt==nAlt)return false;
     moveValid=false;
     cLat=nLat;
     cLon=nLon;
@@ -46,7 +46,7 @@
     // Issue disarm motors
     issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
     // Issue a mission count
-    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
+    issueCount.targSys=1;issueCount.targComp=1;issueCount.count=5;
     // Issue a mission item
     issueItem.targSys=1;issueItem.targComp=1;issueItem.lat=5.0f;issueItem.lon=6.0f;issueItem.alt=7.0f;issueItem.cmd=16;issueItem.seq=0;issueItem.current=0;issueItem.frame=0;issueItem.confirm=0;
     // Issue a take off item
@@ -116,6 +116,7 @@
         }
         // Check for mavlink message request
         if(myCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
+            hasMoved();
             USB::getSerial().printf("Issue Item %f %f %f\n",cLat,cLon,cAlt);
             // Set sequence to requested sequence
             issueItem.seq=myCmd[6];
@@ -149,11 +150,19 @@
             //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=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);double alt=(double)*((int32_t*)&myCmd[10+8]);
+            //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=(double)*((int32_t*)&myCmd[10]);double lon=(double)*((int32_t*)&myCmd[10+4]);
+            double alt=(double)*((int32_t*)&myCmd[10-4]);
             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));
+            if(calibAlt==0){
+                calibAlt=alt;
+            }
+            USB::getSerial().printf("Got lat %f\n",lat);USB::getSerial().printf("Got lon %f\n",lon);USB::getSerial().printf("Got alt %f\n",alt-calibAlt);USB::getSerial().printf("Got aalt %f\n",alt);
+            USB::getSerial().printf("Got heading %f\n",compassHandle::getCompassHand().heading);
+            DH::Locs().add(LHType_locs,DataLocation(lat,lon,alt-calibAlt,0,compassHandle::getCompassHand().heading));
+            USB::getSerial().printf("USING NEW WAYPOINT!!!\n");
+            // Issue count to init waypoint entry
+            Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
         }
         // Check for waypoint count
         if(myCmd[5]==44){