fixes

main/hw/gps.c

@@ -9,11 +9,15 @@
 #define BUF_SIZE 1024
 #define GPS_LINE_MAX_LEN 128
 
+gps_binary_struct_t gpsDataOut;
 static QueueHandle_t uart_queue;
 
+predicted_binary_position_struct_t predictedPosition;
+
 // Initializes UART for GPS
 void gps_init()
 {
+    memset(&gpsDataOut, 0, sizeof(gpsDataOut));
     uart_config_t uart_config = {
         .baud_rate = 9600,
         .data_bits = UART_DATA_8_BITS,
@@ -49,9 +53,12 @@ void gps_task(void *arg)
 
                     if (strstr(line, "$GPGGA") == line) {
                         parse_gpgga(line);
+                        parse_gpgga_to_struct(line, &gpsDataOut);
                     } else if (strstr(line, "$GPRMC") == line) {
                         parse_gprmc(line);
+                        parse_gprmc_to_struct(line, &gpsDataOut);
                     }
+
                 }
 
                 line_pos = 0;
@@ -134,3 +141,104 @@ void parse_gprmc(const char *nmea)
              date, utc_time, lat, lat_dir, lon, lon_dir, speed_knots, status);
 }
 
+
+// Function to convert time string (hhmmss.sss) to seconds since start of day
+static uint32_t time_to_seconds_struct(const char *time_str) {
+    if (time_str == NULL || strlen(time_str) < 6) return 0;
+    uint32_t hours = (time_str[0] - '0') * 10 + (time_str[1] - '0');
+    uint32_t minutes = (time_str[2] - '0') * 10 + (time_str[3] - '0');
+    uint32_t seconds = (time_str[4] - '0') * 10 + (time_str[5] - '0');
+    return hours * 3600 + minutes * 60 + seconds;
+}
+
+// Function to convert DMS (degrees minutes.decimalminutes) to centi-degrees
+static int32_t dms_to_centi_degrees_struct(const char *dms_str, const char *direction) {
+    if (dms_str == NULL || direction == NULL || strlen(dms_str) < 7) return 0;
+    char degrees_str[4] = {0};
+    char minutes_decimal_str[10] = {0};
+    strncpy(degrees_str, dms_str, 2);
+    strncpy(minutes_decimal_str, dms_str + 2, strlen(dms_str) - 2);
+    double degrees = atof(degrees_str);
+    double minutes = atof(minutes_decimal_str);
+    double decimal_degrees = degrees + (minutes / 60.0);
+    if (direction[0] == 'S' || direction[0] == 'W') {
+        decimal_degrees *= -1.0;
+    }
+    return (int32_t)(decimal_degrees * 10000);
+}
+
+// Function to convert altitude string to centi-meters
+static int16_t altitude_to_centi_meters_struct(const char *alt_str) {
+    if (alt_str == NULL) return 0;
+    return (int16_t)(atof(alt_str) * 100);
+}
+
+// Function to convert speed from knots to centi-knots
+static uint16_t speed_to_centi_knots_struct(const char *speed_str) {
+    if (speed_str == NULL) return 0;
+    return (uint16_t)(atof(speed_str) * 100);
+}
+
+// Function to convert date string (ddmmyy) to yymmdd integer
+static uint16_t date_to_yyddmm_struct(const char *date_str) {
+    if (date_str == NULL || strlen(date_str) != 6) return 0;
+    uint16_t day = (date_str[0] - '0') * 10 + (date_str[1] - '0');
+    uint16_t month = (date_str[2] - '0') * 10 + (date_str[3] - '0');
+    uint16_t year_short = (date_str[4] - '0') * 10 + (date_str[5] - '0');
+    // Assuming year is in the 21st century for simplicity
+    return (2000 + year_short) * 10000 + month * 100 + day;
+}
+
+// Function to parse GPGGA NMEA string and return the struct
+void parse_gpgga_to_struct(const char *nmea, gps_binary_struct_t *data)
+{
+    char *fields[15];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 15) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i >= 10) {
+        data->time_seconds = time_to_seconds_struct(fields[1]);
+        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[2], fields[3]);
+        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[4], fields[5]);
+        data->fix_quality = atoi(fields[6]);
+        data->num_satellites = atoi(fields[7]);
+        data->altitude_centi_meters = altitude_to_centi_meters_struct(fields[9]);
+    } else {
+        ESP_LOGW(TAG, "GPGGA: Not enough fields to parse struct");
+    }
+}
+
+// Function to parse GPRMC NMEA string and return the struct
+void parse_gprmc_to_struct(const char *nmea, gps_binary_struct_t *data)
+{
+    char *fields[13];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 13) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i >= 12) {
+        data->time_seconds = time_to_seconds_struct(fields[1]);
+        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[3], fields[4]);
+        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[5], fields[6]);
+        data->date_yyddmm = date_to_yyddmm_struct(fields[9]);
+        data->speed_centi_knots = speed_to_centi_knots_struct(fields[7]);
+        // Fix quality and num_satellites are typically in GPGGA, so they might be 0 here.
+    } else {
+        ESP_LOGW(TAG, "GPRMC: Not enough fields to parse struct");
+    }
+}