Update gps.c

correctly verifies checksum before modifying read NMEA gps data

src/main/io/gps.c

@@ -1052,6 +1052,8 @@ static uint8_t hex_c(uint8_t n)
 /* This is a light implementation of a GPS frame decoding
    This should work with most of modern GPS devices configured to output NMEA frames.
    It assumes there are some NMEA GGA frames to decode on the serial bus
+   Now verifies checksum correctly before applying data 
+
    Here we use only the following data :
      - latitude
      - longitude
@@ -1061,76 +1063,128 @@ static uint8_t hex_c(uint8_t n)
      - GPS altitude (for OSD displaying)
      - GPS speed (for OSD displaying)
 */
+
+#define NO_FRAME   0
 #define FRAME_GGA  1
 #define FRAME_RMC  2
 
 static bool gpsNewFrameNMEA(char c)
 {
+    typedef struct gpsdata_t {
+        int32_t latitude;
+        int32_t longitude; 
+        uint8_t numSat;
+        uint16_t altitude;
+        uint16_t speed;
+        uint16_t ground_course;
+    };
+    
+    static  gpsdata_t gps_Msg;
+
     uint8_t frameOK = 0;
     static uint8_t param = 0, offset = 0, parity = 0;
     static char string[15];
-    static uint8_t checksum_param, frame = 0;
+    static uint8_t checksum_param, gps_frame = NO_FRAME;
 
-    if (c == '$') {
+    switch (c) {
+    case '$':
         param = 0;
         offset = 0;
         parity = 0;
-    } else if (c == ',' || c == '*') {
+        break;
+    case ',':
+    case '*':
         string[offset] = 0;
         if (param == 0) {       //frame identification
-            frame = 0;
+            gps_frame = NO_FRAME;
             if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
-                frame = FRAME_GGA;
+                gps_frame = FRAME_GGA;
             if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
-                frame = FRAME_RMC;
+                gps_frame = FRAME_RMC;
-        } else if (frame == FRAME_GGA) {
-            if (param == 2) {
-                GPS_coord[LAT] = GPS_coord_to_degrees(string);
-            } else if (param == 3 && string[0] == 'S')
-                GPS_coord[LAT] = -GPS_coord[LAT];
-            else if (param == 4) {
-                GPS_coord[LON] = GPS_coord_to_degrees(string);
-            } else if (param == 5 && string[0] == 'W')
-                GPS_coord[LON] = -GPS_coord[LON];
-            else if (param == 6) {
-                f.GPS_FIX = string[0] > '0';
-            } else if (param == 7) {
-                GPS_numSat = grab_fields(string, 0);
-            } else if (param == 9) {
-                GPS_altitude = grab_fields(string, 0);  // altitude in meters added by Mis
-            }
-        } else if (frame == FRAME_RMC) {
-            if (param == 7) {
-                GPS_speed = (grab_fields(string, 1) * 5144L) / 1000L;   // speed in cm/s added by Mis
-            } else if (param == 8) {
-                GPS_ground_course = grab_fields(string, 1);             // ground course deg * 10
-            }
         }
+
+        switch (gps_frame) {
+        case FRAME_GGA:        //************* GPGGA FRAME parsing
+            switch (param) {
+//          case 1:             // Time information
+//              break;
+            case 2:
+                gps_Msg.latitude = GPS_coord_to_degrees(string);
+                break;
+            case 3:
+                if (string[0] == 'S')
+                    gps_Msg.latitude *= -1;
+                break;
+            case 4:
+                gps_Msg.longitude = GPS_coord_to_degrees(string);
+                break;
+            case 5:
+                if (string[0] == 'W')
+                    gps_Msg.longitude *= -1;
+                break;
+            case 6:
+                f.GPS_FIX = string[0] > '0';
+                break;
+            case 7:
+                gps_Msg.numSat = grab_fields(string, 0);
+                break;
+            case 9:
+                gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
+                break;
+            }
+            break;
+        case FRAME_RMC:        //************* GPRMC FRAME parsing
+            switch (param) {
+            case 7:
+                gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
+                break;
+            case 8:
+                gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
+                break;
+            }
+            break;
+        }
+
         param++;
         offset = 0;
         if (c == '*')
             checksum_param = 1;
         else
             parity ^= c;
-    } else if (c == '\r' || c == '\n') {
+        break;
-        if (checksum_param) {   // parity checksum
+    case '\r':
-            uint8_t checksum = hex_c(string[0]);
+    case '\n':
-            checksum <<= 4;
+        if (checksum_param) {   //parity checksum
-            checksum += hex_c(string[1]);
+            uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
-            if (checksum == parity)
+            if (checksum == parity) {
-                frameOK = 1;
+                switch (gps_frame) {
+                case FRAME_GGA:
+                  frameOK = 1;
+                  if (f.GPS_FIX) {
+                        GPS_coord[LAT] = gps_Msg.latitude;
+                        GPS_coord[LON] = gps_Msg.longitude;
+                        GPS_numSat = gps_Msg.numSat;
+                        GPS_altitude = gps_Msg.altitude;
+                    }
+                    break;
+                case FRAME_RMC:
+                    GPS_speed = gps_Msg.speed;
+                    GPS_ground_course = gps_Msg.ground_course;
+                    break;
+                } // end switch
+            }
         }
         checksum_param = 0;
-    } else {
+        break;
+    default:
         if (offset < 15)
             string[offset++] = c;
         if (!checksum_param)
             parity ^= c;
     }
-    return frameOK && (frame == FRAME_GGA);
+    return frameOK;
 }
 
-
 // UBX support
 typedef struct {
     uint8_t preamble1;