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@29:95b0320bf779, 2015-09-19 (annotated)

Committer:
dem123456789
Date:
Sat Sep 19 18:40:43 2015 +0000
Revision:
29:95b0320bf779
Parent:
28:ae857c247fbd
Child:
32:263d39ea6d3e
angle done not tested

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