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@35:009cc4509a90, 2015-10-06 (annotated)

Committer:
dem123456789
Date:
Tue Oct 06 23:06:09 2015 +0000
Revision:
35:009cc4509a90
Parent:
34:610d315c1bab
Child:
37:7a136279daf3
Distance finalized

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 15:dbf20c1209ae 38 } else {
dem123456789 20:a820531c78bc 39 return("Not valid command, example: @GET=GPS_Quality");
dem123456789 15:dbf20c1209ae 40 }
dem123456789 25:30966ed7f7e8 41 }
dem123456789 25:30966ed7f7e8 42
dem123456789 28:ae857c247fbd 43
dem123456789 28:ae857c247fbd 44
dem123456789 28:ae857c247fbd 45 double Deg2Rad(double degree) {
dem123456789 28:ae857c247fbd 46 return degree*DEG2RAD_RATIO;
dem123456789 28:ae857c247fbd 47 }
dem123456789 28:ae857c247fbd 48
dem123456789 28:ae857c247fbd 49 double Rad2Degree(double radian) {
dem123456789 28:ae857c247fbd 50 return radian*RAD2DEG_RATIO;
dem123456789 28:ae857c247fbd 51 }
dem123456789 28:ae857c247fbd 52
dem123456789 25:30966ed7f7e8 53 double getDistance(int task_id) {
dem123456789 27:1be1f25be449 54 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 55 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 27:1be1f25be449 56 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 57 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 58 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 59 return -1;
dem123456789 32:263d39ea6d3e 60 } else {
dem123456789 35:009cc4509a90 61 return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude))*EARTH_RADIUS;
dem123456789 32:263d39ea6d3e 62 }
dem123456789 28:ae857c247fbd 63 }
dem123456789 28:ae857c247fbd 64
dem123456789 34:610d315c1bab 65 double getDistance_2dest(int task_id_start, int task_id_end) {
dem123456789 34:610d315c1bab 66 double dest_Latitude_start = Deg2Rad(Latitude_Path[task_id_start-1]);
dem123456789 35:009cc4509a90 67 double dest_Longitude_start = Deg2Rad(Longitude_Path[task_id_start-1]);
dem123456789 34:610d315c1bab 68 double dest_Latitude_end = Deg2Rad(Latitude_Path[task_id_end-1]);
dem123456789 35:009cc4509a90 69 double dest_Longitude_end = Deg2Rad(Longitude_Path[task_id_end-1]);
dem123456789 34:610d315c1bab 70 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 71 return -1;
dem123456789 34:610d315c1bab 72 } else {
dem123456789 35:009cc4509a90 73 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 74 }
dem123456789 34:610d315c1bab 75 }
dem123456789 29:95b0320bf779 76
dem123456789 29:95b0320bf779 77 double getAngle(int task_id) {
dem123456789 28:ae857c247fbd 78 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 79 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 28:ae857c247fbd 80 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 81 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 82 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 83 return -361;
dem123456789 29:95b0320bf779 84 } else {
dem123456789 35:009cc4509a90 85 Initial_Bearing = Rad2Degree(atan2(sin(dest_Longitude-cur_Logntitude)*cos(dest_Latitude),
dem123456789 35:009cc4509a90 86 cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude)));
dem123456789 32:263d39ea6d3e 87 Final_Bearing = ((int)(Initial_Bearing+180))%360;
dem123456789 32:263d39ea6d3e 88 D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
dem123456789 32:263d39ea6d3e 89 double out = (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
dem123456789 32:263d39ea6d3e 90 (Initial_Bearing - D_IMU_Y_north> 180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
dem123456789 32:263d39ea6d3e 91 return out;
dem123456789 29:95b0320bf779 92 }
dem123456789 29:95b0320bf779 93 }
dem123456789 34:610d315c1bab 94
dem123456789 34:610d315c1bab 95 double getCTE(int task_id) {
dem123456789 34:610d315c1bab 96 double dis_start = getDistance(task_id);
dem123456789 34:610d315c1bab 97 double dis_end = getDistance(task_id+1);
dem123456789 34:610d315c1bab 98 double dis_between = getDistance_2dest(task_id,task_id+1);
dem123456789 34:610d315c1bab 99 if (dis_start == -1 or dis_end == -1 or dis_between == -1) {
dem123456789 34:610d315c1bab 100 return -1;
dem123456789 34:610d315c1bab 101 } else {
dem123456789 34:610d315c1bab 102 double p = (dis_start+dis_end+dis_between)/2;
dem123456789 34:610d315c1bab 103 double area = sqrt(p*(p-dis_start)*(p-dis_end)*(p-dis_between));
dem123456789 34:610d315c1bab 104 return 2*area/dis_between;
dem123456789 34:610d315c1bab 105 }
dem123456789 34:610d315c1bab 106 }