Senior_Design_Sailboat / Mbed 2 deprecated ECE4012_Sailboat

GPS and IMU reading works

Dependencies:   mbed Servo SDFileSystem

/media/uploads/taoqiuyang/img_2352.jpg

Get.cpp@37:7a136279daf3, 2015-10-17 (annotated)

Committer:
dem123456789
Date:
Sat Oct 17 08:27:54 2015 +0000
Revision:
37:7a136279daf3
Parent:
35:009cc4509a90
Child:
38:528e410f2f7d
more GET added, latitude longtitude string format fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dem123456789 20:a820531c78bc 1 #include "Config.h"
dem123456789 13:e18e7b1cb900 2
dem123456789 28:ae857c247fbd 3 double Initial_Bearing;
dem123456789 28:ae857c247fbd 4 double Final_Bearing;
dem123456789 29:95b0320bf779 5 double D_IMU_Y_north;
dem123456789 13:e18e7b1cb900 6
dem123456789 20:a820531c78bc 7 string decodeCommandGET(string cmd) {
dem123456789 15:dbf20c1209ae 8 if (cmd == "IMU_Y") {
dem123456789 25:30966ed7f7e8 9 return(IMU_Y);
dem123456789 15:dbf20c1209ae 10 } else if (cmd == "IMU_P") {
dem123456789 25:30966ed7f7e8 11 return(IMU_P);
dem123456789 15:dbf20c1209ae 12 } else if (cmd == "IMU_R") {
dem123456789 25:30966ed7f7e8 13 return(IMU_R);
dem123456789 15:dbf20c1209ae 14 } else if (cmd == "GPS_Quality") {
dem123456789 25:30966ed7f7e8 15 return(GPS_Quality);
dem123456789 15:dbf20c1209ae 16 } else if (cmd == "GPS_UTC") {
dem123456789 25:30966ed7f7e8 17 return(GPS_UTC);
dem123456789 15:dbf20c1209ae 18 } else if (cmd == "GPS_Latitude") {
dem123456789 25:30966ed7f7e8 19 return(GPS_Latitude);
dem123456789 27:1be1f25be449 20 } else if (cmd == "GPS_Longitude") {
dem123456789 27:1be1f25be449 21 return(GPS_Longitude);
dem123456789 15:dbf20c1209ae 22 } else if (cmd == "GPS_Altitude") {
dem123456789 25:30966ed7f7e8 23 return(GPS_Altitude);
dem123456789 15:dbf20c1209ae 24 } else if (cmd == "GPS_Num_Satellite") {
dem123456789 25:30966ed7f7e8 25 return(GPS_Num_Satellite);
dem123456789 15:dbf20c1209ae 26 } else if (cmd == "GPS_HDOP") {
dem123456789 25:30966ed7f7e8 27 return(GPS_HDOP);
dem123456789 15:dbf20c1209ae 28 } else if (cmd == "GPS_VDOP") {
dem123456789 25:30966ed7f7e8 29 return(GPS_VDOP);
dem123456789 15:dbf20c1209ae 30 } else if (cmd == "GPS_PDOP") {
dem123456789 25:30966ed7f7e8 31 return(GPS_PDOP);
dem123456789 15:dbf20c1209ae 32 } else if (cmd == "GPS_Date") {
dem123456789 25:30966ed7f7e8 33 return(GPS_Date);
dem123456789 15:dbf20c1209ae 34 } else if (cmd == "GPS_VelocityKnot") {
dem123456789 25:30966ed7f7e8 35 return(GPS_VelocityKnot);
dem123456789 15:dbf20c1209ae 36 } else if (cmd == "GPS_VelocityKph") {
dem123456789 25:30966ed7f7e8 37 return(GPS_VelocityKph);
dem123456789 37:7a136279daf3 38 } else if (cmd == "Path") {
dem123456789 37:7a136279daf3 39 printPath();
dem123456789 37:7a136279daf3 40 return NULL;
dem123456789 37:7a136279daf3 41 } else if (cmd == "Distance") {
dem123456789 37:7a136279daf3 42 printDistance();
dem123456789 37:7a136279daf3 43 return NULL;
dem123456789 37:7a136279daf3 44 } else if (cmd == "Angle") {
dem123456789 37:7a136279daf3 45 printAngle();
dem123456789 37:7a136279daf3 46 return NULL;
dem123456789 15:dbf20c1209ae 47 } else {
dem123456789 20:a820531c78bc 48 return("Not valid command, example: @GET=GPS_Quality");
dem123456789 15:dbf20c1209ae 49 }
dem123456789 25:30966ed7f7e8 50 }
dem123456789 25:30966ed7f7e8 51
dem123456789 28:ae857c247fbd 52
dem123456789 28:ae857c247fbd 53
dem123456789 28:ae857c247fbd 54 double Deg2Rad(double degree) {
dem123456789 28:ae857c247fbd 55 return degree*DEG2RAD_RATIO;
dem123456789 28:ae857c247fbd 56 }
dem123456789 28:ae857c247fbd 57
dem123456789 28:ae857c247fbd 58 double Rad2Degree(double radian) {
dem123456789 28:ae857c247fbd 59 return radian*RAD2DEG_RATIO;
dem123456789 28:ae857c247fbd 60 }
dem123456789 28:ae857c247fbd 61
dem123456789 25:30966ed7f7e8 62 double getDistance(int task_id) {
dem123456789 27:1be1f25be449 63 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 64 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 27:1be1f25be449 65 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 66 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 67 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 68 return -1;
dem123456789 32:263d39ea6d3e 69 } else {
dem123456789 35:009cc4509a90 70 return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude))*EARTH_RADIUS;
dem123456789 32:263d39ea6d3e 71 }
dem123456789 28:ae857c247fbd 72 }
dem123456789 28:ae857c247fbd 73
dem123456789 34:610d315c1bab 74 double getDistance_2dest(int task_id_start, int task_id_end) {
dem123456789 34:610d315c1bab 75 double dest_Latitude_start = Deg2Rad(Latitude_Path[task_id_start-1]);
dem123456789 35:009cc4509a90 76 double dest_Longitude_start = Deg2Rad(Longitude_Path[task_id_start-1]);
dem123456789 34:610d315c1bab 77 double dest_Latitude_end = Deg2Rad(Latitude_Path[task_id_end-1]);
dem123456789 35:009cc4509a90 78 double dest_Longitude_end = Deg2Rad(Longitude_Path[task_id_end-1]);
dem123456789 34:610d315c1bab 79 if(Latitude_Path[task_id_start-1] == 181 or Longitude_Path[task_id_start-1] == 181 or Latitude_Path[task_id_end-1] == 181 or Longitude_Path[task_id_end-1] == 181) {
dem123456789 34:610d315c1bab 80 return -1;
dem123456789 34:610d315c1bab 81 } else {
dem123456789 35:009cc4509a90 82 return acos(sin(dest_Latitude_start)*sin(dest_Latitude_end)+cos(dest_Latitude_start)*cos(dest_Latitude_end)*cos(dest_Longitude_end-dest_Longitude_start))*EARTH_RADIUS;
dem123456789 34:610d315c1bab 83 }
dem123456789 34:610d315c1bab 84 }
dem123456789 29:95b0320bf779 85
dem123456789 29:95b0320bf779 86 double getAngle(int task_id) {
dem123456789 28:ae857c247fbd 87 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 88 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 28:ae857c247fbd 89 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 90 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 91 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 92 return -361;
dem123456789 29:95b0320bf779 93 } else {
dem123456789 35:009cc4509a90 94 Initial_Bearing = Rad2Degree(atan2(sin(dest_Longitude-cur_Logntitude)*cos(dest_Latitude),
dem123456789 35:009cc4509a90 95 cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude)));
dem123456789 32:263d39ea6d3e 96 Final_Bearing = ((int)(Initial_Bearing+180))%360;
dem123456789 32:263d39ea6d3e 97 D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
dem123456789 32:263d39ea6d3e 98 double out = (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
dem123456789 32:263d39ea6d3e 99 (Initial_Bearing - D_IMU_Y_north> 180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
dem123456789 32:263d39ea6d3e 100 return out;
dem123456789 29:95b0320bf779 101 }
dem123456789 29:95b0320bf779 102 }
dem123456789 34:610d315c1bab 103
dem123456789 34:610d315c1bab 104 double getCTE(int task_id) {
dem123456789 34:610d315c1bab 105 double dis_start = getDistance(task_id);
dem123456789 34:610d315c1bab 106 double dis_end = getDistance(task_id+1);
dem123456789 34:610d315c1bab 107 double dis_between = getDistance_2dest(task_id,task_id+1);
dem123456789 34:610d315c1bab 108 if (dis_start == -1 or dis_end == -1 or dis_between == -1) {
dem123456789 34:610d315c1bab 109 return -1;
dem123456789 34:610d315c1bab 110 } else {
dem123456789 34:610d315c1bab 111 double p = (dis_start+dis_end+dis_between)/2;
dem123456789 34:610d315c1bab 112 double area = sqrt(p*(p-dis_start)*(p-dis_end)*(p-dis_between));
dem123456789 34:610d315c1bab 113 return 2*area/dis_between;
dem123456789 34:610d315c1bab 114 }
dem123456789 34:610d315c1bab 115 }