Diff: main.cpp

Revision:

27:1be1f25be449

Parent:

26:353a80179346

Child:

28:ae857c247fbd

--- a/main.cpp	Thu Sep 10 18:15:16 2015 +0000
+++ b/main.cpp	Fri Sep 11 00:11:31 2015 +0000
@@ -26,7 +26,7 @@
 string GPS_Quality="N/A";
 string GPS_UTC="N/A";
 string GPS_Latitude="N/A";
-string GPS_Longtitude="N/A";
+string GPS_Longitude="N/A";
 string GPS_Altitude="N/A";
 string GPS_Altitude_Unit="N/A";
 string GPS_Num_Satellite="N/A";
@@ -47,8 +47,8 @@
 double D_GPS_UTC=0;
 double D_GPS_Latitude=0;
 double D_GPS_Latitude_Direction=0;
-double D_GPS_Longtitude=0;
-double D_GPS_Longtitude_Direction=0;
+double D_GPS_Longitude=0;
+double D_GPS_Longitude_Direction=0;
 double D_GPS_Altitude=0;
 double D_GPS_Num_Satellite=0;
 double D_GPS_HDOP=0;
@@ -57,10 +57,10 @@
 double D_GPS_Date=0;
 double D_GPS_VelocityKnot=0;
 double D_GPS_VelocityKph=0;
-
+double D_temp=0;
 int asterisk_idx;
 
-double Longtitude_Path[MAX_TASK_SIZE];
+double Longitude_Path[MAX_TASK_SIZE];
 double Latitude_Path[MAX_TASK_SIZE];
 
 double angle_to_path_point,distance_to_path_point,desired_speed;
@@ -82,6 +82,17 @@
     return tokens;
 }
 
+double GPSdecimal(double coordinate) {
+     //Convert a NMEA decimal-decimal value into decimal degree value
+     //5144.3855 (ddmm.mmmm) = 51 44.3855 = 51 + 44.3855/60 = 51.7397583 degrees
+     coordinate = coordinate/100;
+     return ((int) coordinate) + ((coordinate-((int) coordinate))/60);    
+}
+
+double Deg2Rad(double degree) {
+    return degree*DEG2RAD_RATIO;
+}
+
 void updateIMU(string IMU_data) {
     string IMU_data_string(IMU_data);
     if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
@@ -107,27 +118,23 @@
             GPS_UTC = result.at(0);
             D_GPS_UTC = strtod(GPS_UTC.c_str(), NULL);
             GPS_Latitude = result.at(1) + result.at(2);
-            temp = result.at(1);
-            if (temp[0] == '0') {
-                temp[0] = '-';
-                }
-            D_GPS_Latitude = strtod(temp.c_str(), NULL);
             if (result.at(2) == "N") { // 0 means North
               D_GPS_Latitude_Direction = 0;
             } else if (result.at(2) == "S") { // 1 means South
-              D_GPS_Latitude_Direction = 1;  
+              D_GPS_Latitude_Direction = 1;
+              temp = "-" + result.at(1);  
             }
-            GPS_Longtitude = result.at(3) + result.at(4);
-            temp = result.at(3);
-            if (temp[0] == '0') {
-                temp[0] = '-';
-                }
-            D_GPS_Longtitude = strtod(temp.c_str(), NULL);
+            D_temp = strtod(temp.c_str(), NULL);
+            D_GPS_Latitude = GPSdecimal(D_temp);
+            GPS_Longitude = result.at(3) + result.at(4);
             if (result.at(4) == "E") { // 0 means East
-              D_GPS_Longtitude_Direction = 0;
+              D_GPS_Longitude_Direction = 0;
             } else if (result.at(4) == "W") { // 1 means West
-              D_GPS_Longtitude_Direction = 1;  
+              D_GPS_Longitude_Direction = 1; 
+              temp = "-" + result.at(3); 
             }
+            D_temp = strtod(temp.c_str(), NULL);
+            D_GPS_Longitude = GPSdecimal(D_temp);
             GPS_Num_Satellite = result.at(6);
             D_GPS_Num_Satellite = strtod(result.at(6).c_str(), NULL);
             GPS_HDOP = result.at(7);
@@ -165,10 +172,10 @@
 /*  Get data from SailBoat
     @GET=parameter
     Ex: @GET=GPS_Quality
-    Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude,
+    Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longitude, GPS_Altitude,
     GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
     Set path to SailBoat
-    @SET=PATH, Latitude, Longtitude, Task id
+    @SET=PATH, Latitude, Longitude, Task id
     @SET=PATH, 33.776318, -84.407590, 3 
 */
 void decodePC(string PC_data) {
@@ -182,9 +189,9 @@
         PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
         string claim = decodeCommandSET(PC_data_string);
         if (claim == "DONE") {
-            pc.printf("Current Path: Longtitude, Latitude\n");
+            pc.printf("Current Path: Longitude, Latitude\n");
             for (int i=0;i<MAX_TASK_SIZE;++i) {
-                pc.printf("              %f, %f\n", Longtitude_Path[i], Latitude_Path[i]);
+                pc.printf("              %f, %f\n", Longitude_Path[i], Latitude_Path[i]);
             }
         }
         pc.printf("%s\n", claim.c_str());
@@ -200,15 +207,15 @@
  
 void printStateGPS() {
    /*
-   pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, "
+   pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longitude: %s, GPS_Altitude: %s, "
             "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_Date: %s, GPS_VelocityKnot: %s, GPS_VelocityKph: %s\n",
-            GPS_Quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longtitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(),
+            GPS_Quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(),
             GPS_HDOP.c_str(), GPS_VDOP.c_str(), GPS_PDOP.c_str(), GPS_Date.c_str(), GPS_VelocityKnot.c_str(), GPS_VelocityKph.c_str());
    */  
     
-    pc.printf("D_GPS_Quality: %f, D_GPS_UTC: %f, D_GPS_Latitude: %f, D_GPS_Latitude_Direction: %f, D_GPS_Longtitude: %f, D_GPS_Longtitude_Direction: %f, D_GPS_Altitude: %f,\n"
+    pc.printf("D_GPS_Quality: %f, D_GPS_UTC: %f, D_GPS_Latitude: %f, D_GPS_Latitude_Direction: %f, D_GPS_Longitude: %f, D_GPS_Longitude_Direction: %f, D_GPS_Altitude: %f,\n"
             "D_GPS_Num_Satellite: %f, D_GPS_HDOP: %f, D_GPS_VDOP: %f, D_GPS_PDOP: %f, D_GPS_Date: %f, D_GPS_VelocityKnot: %f, D_GPS_VelocityKph: %f\n",
-            D_GPS_Quality, D_GPS_UTC, D_GPS_Latitude, D_GPS_Latitude_Direction, D_GPS_Longtitude, D_GPS_Longtitude_Direction, D_GPS_Altitude, D_GPS_Num_Satellite,
+            D_GPS_Quality, D_GPS_UTC, D_GPS_Latitude, D_GPS_Latitude_Direction, D_GPS_Longitude, D_GPS_Longitude_Direction, D_GPS_Altitude, D_GPS_Num_Satellite,
             D_GPS_HDOP, D_GPS_VDOP, D_GPS_PDOP, D_GPS_Date, D_GPS_VelocityKnot, D_GPS_VelocityKph);      
           
 }