GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Get.cpp@46:305112d73c69, 2015-11-04 (annotated)
- Committer:
- dem123456789
- Date:
- Wed Nov 04 21:45:34 2015 +0000
- Revision:
- 46:305112d73c69
- Parent:
- 38:528e410f2f7d
path complete and update finish
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 | 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 | } |