GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: Get.cpp
- Revision:
- 32:263d39ea6d3e
- Parent:
- 29:95b0320bf779
- Child:
- 34:610d315c1bab
diff -r e3339036c98b -r 263d39ea6d3e Get.cpp --- 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; }