Diff: Get.cpp

Revision:

32:263d39ea6d3e

Parent:

29:95b0320bf779

Child:

34:610d315c1bab

--- a/Get.cpp	Sun Sep 27 00:58:18 2015 +0000
+++ b/Get.cpp	Sun Sep 27 09:26:00 2015 +0000
@@ -54,8 +54,12 @@
     double cur_Latitude = Deg2Rad(D_GPS_Latitude);
     double cur_Logntitude = D_GPS_Longitude;
     double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
-    double dest_Longitude = Longitude_Path[task_id-1];   
-    return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS;
+    double dest_Longitude = Longitude_Path[task_id-1];
+    if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
+        return -1;
+    } else {
+        return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS;
+    }
 }
 
 
@@ -63,14 +67,16 @@
     double cur_Latitude = Deg2Rad(D_GPS_Latitude);
     double cur_Logntitude = D_GPS_Longitude;
     double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
-    double dest_Longitude = Longitude_Path[task_id-1];  
-    Initial_Bearing = Rad2Degree(atan2(cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)),
-                      sin(Deg2Rad(dest_Longitude-cur_Logntitude))*cos(dest_Latitude)));
-    Final_Bearing = ((int)(Initial_Bearing+180))%360;
-    if(D_IMU_Y<=0) {
-        D_IMU_Y_north = D_IMU_Y + 180;
+    double dest_Longitude = Longitude_Path[task_id-1];
+    if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
+        return -361;
     } else {
-        D_IMU_Y_north = D_IMU_Y - 180;
+        Initial_Bearing = Rad2Degree(atan2(cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)),
+                          sin(Deg2Rad(dest_Longitude-cur_Logntitude))*cos(dest_Latitude)));
+        Final_Bearing = ((int)(Initial_Bearing+180))%360;
+        D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
+        double out =  (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
+                      (Initial_Bearing - D_IMU_Y_north>  180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
+        return out;
     }
-    return Initial_Bearing - D_IMU_Y_north;
 }