Diff: handle/handleGPS.cpp

Revision:

66:5d43988d100c

Parent:

64:d4818fb7813c

--- a/handle/handleGPS.cpp	Sun Apr 27 21:54:33 2014 +0000
+++ b/handle/handleGPS.cpp	Mon May 05 13:20:35 2014 +0000
@@ -28,7 +28,15 @@
     unsigned int num_points = pack.size;
     for(int i=0;i<num_points;i++){
         //USB::getSerial().printf("Adding Waypoint: %f, %f\r\n",points[i].lat,points[i].lon);
-        DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,6.0f));
+        if(i==num_points-1){
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,75.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,25.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,10.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,5.0f));
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,0.0f));
+        }else{
+            DH::Locs().add(LHType_targ,DataLocation(points[i].lat,points[i].lon,150.0f));
+        }
     }
     //USB::getSerial().printf("Waypoints size: %d\r\n",DH::Locs().getI(LHType_targ,LHIType_size));
     for(int i=0;i<DH::Locs().getI(LHType_targ,LHIType_size);i++){
@@ -52,7 +60,9 @@
     unsigned int sID=getPS().getSuperID();
     getPS().sendPacket(0,NULL,0,PT_EMPTY);
     getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
+    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
     getPS().sendPacket(sID,(char*)(&DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs))),sizeof(DataLocation));
+    DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs)).getLon() *= -1;
     getPS().sendPacket(sID,NULL,0,PT_END);
     wait_us(100000);
     getPS().closeConnection();
@@ -60,8 +70,8 @@
 }
 
 bool GPSHandle::if_image_location(){
-    double lon_thresh = 0.0005;
-    double lat_thresh = 0.00035;
+    double lon_thresh = 0.00030;
+    double lat_thresh = 0.00018;
     //USB::getSerial().printf("Checking if at waypoint\r\n");
     DataLocation current_loc = DH::Locs().getC(LHType_locs,DH::Locs().getI(LHType_locs));
     DataLocation next_waypoint = DH::Locs().getC(LHType_targ,DH::Locs().getI(LHType_targ));