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@48:575ec42c5f58, 2015-11-27 (annotated)

Committer:
dem123456789
Date:
Fri Nov 27 02:24:42 2015 +0000
Revision:
48:575ec42c5f58
Parent:
46:305112d73c69 
update with gui

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 38:528e410f2f7d 38 } else if (cmd == "PATH") {
dem123456789 37:7a136279daf3 39 printPath();
dem123456789 37:7a136279daf3 40 return NULL;
dem123456789 38:528e410f2f7d 41 } else if (cmd == "DISTANCE") {
dem123456789 37:7a136279daf3 42 printDistance();
dem123456789 37:7a136279daf3 43 return NULL;
dem123456789 38:528e410f2f7d 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 46:305112d73c69 52 int get_current_task() {
dem123456789 46:305112d73c69 53 for(int i=0;i<MAX_TASK_SIZE;i++){
dem123456789 46:305112d73c69 54 if(IF_Path_Complete[i]==0) {
dem123456789 46:305112d73c69 55 return i+1;
dem123456789 46:305112d73c69 56 }
dem123456789 46:305112d73c69 57 }
dem123456789 46:305112d73c69 58 return 1;
dem123456789 46:305112d73c69 59 }
dem123456789 28:ae857c247fbd 60
dem123456789 28:ae857c247fbd 61 double Deg2Rad(double degree) {
dem123456789 28:ae857c247fbd 62 return degree*DEG2RAD_RATIO;
dem123456789 28:ae857c247fbd 63 }
dem123456789 28:ae857c247fbd 64
dem123456789 28:ae857c247fbd 65 double Rad2Degree(double radian) {
dem123456789 28:ae857c247fbd 66 return radian*RAD2DEG_RATIO;
dem123456789 28:ae857c247fbd 67 }
dem123456789 28:ae857c247fbd 68
dem123456789 25:30966ed7f7e8 69 double getDistance(int task_id) {
dem123456789 27:1be1f25be449 70 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 71 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 27:1be1f25be449 72 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 73 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 74 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 75 return -1;
dem123456789 32:263d39ea6d3e 76 } else {
dem123456789 35:009cc4509a90 77 return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude))*EARTH_RADIUS;
dem123456789 32:263d39ea6d3e 78 }
dem123456789 28:ae857c247fbd 79 }
dem123456789 28:ae857c247fbd 80
dem123456789 34:610d315c1bab 81 double getDistance_2dest(int task_id_start, int task_id_end) {
dem123456789 34:610d315c1bab 82 double dest_Latitude_start = Deg2Rad(Latitude_Path[task_id_start-1]);
dem123456789 35:009cc4509a90 83 double dest_Longitude_start = Deg2Rad(Longitude_Path[task_id_start-1]);
dem123456789 34:610d315c1bab 84 double dest_Latitude_end = Deg2Rad(Latitude_Path[task_id_end-1]);
dem123456789 35:009cc4509a90 85 double dest_Longitude_end = Deg2Rad(Longitude_Path[task_id_end-1]);
dem123456789 34:610d315c1bab 86 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 87 return -1;
dem123456789 34:610d315c1bab 88 } else {
dem123456789 35:009cc4509a90 89 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 90 }
dem123456789 34:610d315c1bab 91 }
dem123456789 29:95b0320bf779 92
dem123456789 29:95b0320bf779 93 double getAngle(int task_id) {
dem123456789 28:ae857c247fbd 94 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 35:009cc4509a90 95 double cur_Logntitude = Deg2Rad(D_GPS_Longitude);
dem123456789 28:ae857c247fbd 96 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 35:009cc4509a90 97 double dest_Longitude = Deg2Rad(Longitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 98 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 99 return -361;
dem123456789 29:95b0320bf779 100 } else {
dem123456789 35:009cc4509a90 101 Initial_Bearing = Rad2Degree(atan2(sin(dest_Longitude-cur_Logntitude)*cos(dest_Latitude),
dem123456789 35:009cc4509a90 102 cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(dest_Longitude-cur_Logntitude)));
dem123456789 32:263d39ea6d3e 103 Final_Bearing = ((int)(Initial_Bearing+180))%360;
dem123456789 32:263d39ea6d3e 104 D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
dem123456789 32:263d39ea6d3e 105 double out = (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
dem123456789 32:263d39ea6d3e 106 (Initial_Bearing - D_IMU_Y_north> 180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
dem123456789 32:263d39ea6d3e 107 return out;
dem123456789 29:95b0320bf779 108 }
dem123456789 29:95b0320bf779 109 }
dem123456789 34:610d315c1bab 110
dem123456789 34:610d315c1bab 111 double getCTE(int task_id) {
dem123456789 34:610d315c1bab 112 double dis_start = getDistance(task_id);
dem123456789 34:610d315c1bab 113 double dis_end = getDistance(task_id+1);
dem123456789 34:610d315c1bab 114 double dis_between = getDistance_2dest(task_id,task_id+1);
dem123456789 34:610d315c1bab 115 if (dis_start == -1 or dis_end == -1 or dis_between == -1) {
dem123456789 34:610d315c1bab 116 return -1;
dem123456789 34:610d315c1bab 117 } else {
dem123456789 34:610d315c1bab 118 double p = (dis_start+dis_end+dis_between)/2;
dem123456789 34:610d315c1bab 119 double area = sqrt(p*(p-dis_start)*(p-dis_end)*(p-dis_between));
dem123456789 34:610d315c1bab 120 return 2*area/dis_between;
dem123456789 34:610d315c1bab 121 }
dem123456789 34:610d315c1bab 122 }