Diff: Get.cpp

Revision:

29:95b0320bf779

Parent:

28:ae857c247fbd

Child:

32:263d39ea6d3e

--- a/Get.cpp	Fri Sep 11 23:06:51 2015 +0000
+++ b/Get.cpp	Sat Sep 19 18:40:43 2015 +0000
@@ -2,6 +2,7 @@
 
 double Initial_Bearing;
 double Final_Bearing;
+double D_IMU_Y_north;
 
 string decodeCommandGET(string cmd) {
     if (cmd == "IMU_Y") {
@@ -57,7 +58,8 @@
     return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS;
 }
 
-void updateBearing(int task_id) {
+
+double getAngle(int task_id) {
     double cur_Latitude = Deg2Rad(D_GPS_Latitude);
     double cur_Logntitude = D_GPS_Longitude;
     double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
@@ -65,4 +67,10 @@
     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;
-}
\ No newline at end of file
+    if(D_IMU_Y<=0) {
+        D_IMU_Y_north = D_IMU_Y + 180;
+    } else {
+        D_IMU_Y_north = D_IMU_Y - 180;
+    }
+    return Initial_Bearing - D_IMU_Y_north;
+}