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@32:263d39ea6d3e, 2015-09-27 (annotated)

Committer:
dem123456789
Date:
Sun Sep 27 09:26:00 2015 +0000
Revision:
32:263d39ea6d3e
Parent:
29:95b0320bf779
Child:
34:610d315c1bab
test preparation

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 27:1be1f25be449 55 double cur_Logntitude = D_GPS_Longitude;
dem123456789 27:1be1f25be449 56 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 57 double dest_Longitude = 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 32:263d39ea6d3e 61 return acos(sin(cur_Latitude)*sin(dest_Latitude)+cos(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)))*EARTH_RADIUS;
dem123456789 32:263d39ea6d3e 62 }
dem123456789 28:ae857c247fbd 63 }
dem123456789 28:ae857c247fbd 64
dem123456789 29:95b0320bf779 65
dem123456789 29:95b0320bf779 66 double getAngle(int task_id) {
dem123456789 28:ae857c247fbd 67 double cur_Latitude = Deg2Rad(D_GPS_Latitude);
dem123456789 28:ae857c247fbd 68 double cur_Logntitude = D_GPS_Longitude;
dem123456789 28:ae857c247fbd 69 double dest_Latitude = Deg2Rad(Latitude_Path[task_id-1]);
dem123456789 32:263d39ea6d3e 70 double dest_Longitude = Longitude_Path[task_id-1];
dem123456789 32:263d39ea6d3e 71 if(Latitude_Path[task_id-1] == 181 or Longitude_Path[task_id-1] == 181) {
dem123456789 32:263d39ea6d3e 72 return -361;
dem123456789 29:95b0320bf779 73 } else {
dem123456789 32:263d39ea6d3e 74 Initial_Bearing = Rad2Degree(atan2(cos(cur_Latitude)*sin(dest_Latitude)-sin(cur_Latitude)*cos(dest_Latitude)*cos(Deg2Rad(dest_Longitude-cur_Logntitude)),
dem123456789 32:263d39ea6d3e 75 sin(Deg2Rad(dest_Longitude-cur_Logntitude))*cos(dest_Latitude)));
dem123456789 32:263d39ea6d3e 76 Final_Bearing = ((int)(Initial_Bearing+180))%360;
dem123456789 32:263d39ea6d3e 77 D_IMU_Y_north = (D_IMU_Y<=0)?(D_IMU_Y + 180):(D_IMU_Y - 180);
dem123456789 32:263d39ea6d3e 78 double out = (Initial_Bearing - D_IMU_Y_north< -180)?(Initial_Bearing - D_IMU_Y_north + 360):
dem123456789 32:263d39ea6d3e 79 (Initial_Bearing - D_IMU_Y_north> 180)?(Initial_Bearing - D_IMU_Y_north - 360):(Initial_Bearing - D_IMU_Y_north);
dem123456789 32:263d39ea6d3e 80 return out;
dem123456789 29:95b0320bf779 81 }
dem123456789 29:95b0320bf779 82 }