GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Get.cpp@27:1be1f25be449, 2015-09-11 (annotated)
- Committer:
- dem123456789
- Date:
- Fri Sep 11 00:11:31 2015 +0000
- Revision:
- 27:1be1f25be449
- Parent:
- 25:30966ed7f7e8
- Child:
- 28:ae857c247fbd
Distance part done (not tested)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dem123456789 | 20:a820531c78bc | 1 | #include "Config.h" |
dem123456789 | 13:e18e7b1cb900 | 2 | |
dem123456789 | 13:e18e7b1cb900 | 3 | |
dem123456789 | 20:a820531c78bc | 4 | string decodeCommandGET(string cmd) { |
dem123456789 | 15:dbf20c1209ae | 5 | if (cmd == "IMU_Y") { |
dem123456789 | 25:30966ed7f7e8 | 6 | return(IMU_Y); |
dem123456789 | 15:dbf20c1209ae | 7 | } else if (cmd == "IMU_P") { |
dem123456789 | 25:30966ed7f7e8 | 8 | return(IMU_P); |
dem123456789 | 15:dbf20c1209ae | 9 | } else if (cmd == "IMU_R") { |
dem123456789 | 25:30966ed7f7e8 | 10 | return(IMU_R); |
dem123456789 | 15:dbf20c1209ae | 11 | } else if (cmd == "GPS_Quality") { |
dem123456789 | 25:30966ed7f7e8 | 12 | return(GPS_Quality); |
dem123456789 | 15:dbf20c1209ae | 13 | } else if (cmd == "GPS_UTC") { |
dem123456789 | 25:30966ed7f7e8 | 14 | return(GPS_UTC); |
dem123456789 | 15:dbf20c1209ae | 15 | } else if (cmd == "GPS_Latitude") { |
dem123456789 | 25:30966ed7f7e8 | 16 | return(GPS_Latitude); |
dem123456789 | 27:1be1f25be449 | 17 | } else if (cmd == "GPS_Longitude") { |
dem123456789 | 27:1be1f25be449 | 18 | return(GPS_Longitude); |
dem123456789 | 15:dbf20c1209ae | 19 | } else if (cmd == "GPS_Altitude") { |
dem123456789 | 25:30966ed7f7e8 | 20 | return(GPS_Altitude); |
dem123456789 | 15:dbf20c1209ae | 21 | } else if (cmd == "GPS_Num_Satellite") { |
dem123456789 | 25:30966ed7f7e8 | 22 | return(GPS_Num_Satellite); |
dem123456789 | 15:dbf20c1209ae | 23 | } else if (cmd == "GPS_HDOP") { |
dem123456789 | 25:30966ed7f7e8 | 24 | return(GPS_HDOP); |
dem123456789 | 15:dbf20c1209ae | 25 | } else if (cmd == "GPS_VDOP") { |
dem123456789 | 25:30966ed7f7e8 | 26 | return(GPS_VDOP); |
dem123456789 | 15:dbf20c1209ae | 27 | } else if (cmd == "GPS_PDOP") { |
dem123456789 | 25:30966ed7f7e8 | 28 | return(GPS_PDOP); |
dem123456789 | 15:dbf20c1209ae | 29 | } else if (cmd == "GPS_Date") { |
dem123456789 | 25:30966ed7f7e8 | 30 | return(GPS_Date); |
dem123456789 | 15:dbf20c1209ae | 31 | } else if (cmd == "GPS_VelocityKnot") { |
dem123456789 | 25:30966ed7f7e8 | 32 | return(GPS_VelocityKnot); |
dem123456789 | 15:dbf20c1209ae | 33 | } else if (cmd == "GPS_VelocityKph") { |
dem123456789 | 25:30966ed7f7e8 | 34 | return(GPS_VelocityKph); |
dem123456789 | 15:dbf20c1209ae | 35 | } else { |
dem123456789 | 20:a820531c78bc | 36 | return("Not valid command, example: @GET=GPS_Quality"); |
dem123456789 | 15:dbf20c1209ae | 37 | } |
dem123456789 | 25:30966ed7f7e8 | 38 | } |
dem123456789 | 25:30966ed7f7e8 | 39 | |
dem123456789 | 25:30966ed7f7e8 | 40 | double getDistance(int task_id) { |
dem123456789 | 27:1be1f25be449 | 41 | double cur_Latitude = Deg2Rad(D_GPS_Latitude); |
dem123456789 | 27:1be1f25be449 | 42 | double cur_Logntitude = D_GPS_Longitude; |
dem123456789 | 27:1be1f25be449 | 43 | double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]); |
dem123456789 | 27:1be1f25be449 | 44 | double dest_Longitude = Longitude_Path[task_id-1]; |
dem123456789 | 27:1be1f25be449 | 45 | return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS; |
dem123456789 | 15:dbf20c1209ae | 46 | } |