GPS and IMU reading works

Dependencies:   mbed Servo SDFileSystem

/media/uploads/taoqiuyang/img_2352.jpg

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?

UserRevisionLine numberNew 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 }