Diff: Get.cpp

Revision:

27:1be1f25be449

Parent:

25:30966ed7f7e8

Child:

28:ae857c247fbd

--- a/Get.cpp	Thu Sep 10 18:15:16 2015 +0000
+++ b/Get.cpp	Fri Sep 11 00:11:31 2015 +0000
@@ -14,8 +14,8 @@
         return(GPS_UTC);
     } else if (cmd == "GPS_Latitude") {
         return(GPS_Latitude);
-    } else if (cmd == "GPS_Longtitude") {
-        return(GPS_Longtitude);
+    } else if (cmd == "GPS_Longitude") {
+        return(GPS_Longitude);
     } else if (cmd == "GPS_Altitude") {
         return(GPS_Altitude);
     } else if (cmd == "GPS_Num_Satellite") {
@@ -38,16 +38,9 @@
 }
 
 double getDistance(int task_id) {
-    double cur_Latitude = D_GPS_Latitude;
-    double cur_Logntitude = D_GPS_Longtitude;
-    double dest_Latitude = Latitude_Path[task_id-1];
-    double dest_Longtitude = Longtitude_Path[task_id-1];
-
-
-
-
-
-
-
-
+    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;
 }
\ No newline at end of file