GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
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