54 lines
1.8 KiB
C
54 lines
1.8 KiB
C
#include "gps_handler.h"
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
static char buffer[BUFFER_SIZE];
|
|
static int gps_index = 0;
|
|
|
|
static float convert_to_decimal(char *coord, char dir) {
|
|
float raw = atof(coord);
|
|
int degrees = (int)(raw / 100);
|
|
float minutes = raw - (degrees * 100);
|
|
float decimal = degrees + (minutes / 60.0f);
|
|
if (dir == 'S' || dir == 'W') decimal *= -1.0f;
|
|
return decimal;
|
|
}
|
|
|
|
void gps_init(void) {
|
|
uart_init(UART_ID, BAUD_RATE);
|
|
gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
|
|
gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
|
|
|
|
uart_set_hw_flow(UART_ID, false, false);
|
|
uart_set_format(UART_ID, 8, 1, UART_PARITY_NONE);
|
|
}
|
|
|
|
void gps_update(GPS_Data_t *data) {
|
|
while (uart_is_readable(UART_ID)) {
|
|
char c = uart_getc(UART_ID);
|
|
if (c == '\n') {
|
|
buffer[gps_index] = '\0';
|
|
if (strstr(buffer, "$GPRMC")) {
|
|
char *token = strtok(buffer, ",");
|
|
int field = 0;
|
|
char lat[16] = {0}, lon[16] = {0}, lat_dir = 0, lon_dir = 0;
|
|
while (token != NULL) {
|
|
field++;
|
|
if (field == 4) strcpy(lat, token);
|
|
if (field == 5) lat_dir = token[0];
|
|
if (field == 6) strcpy(lon, token);
|
|
if (field == 7) lon_dir = token[0];
|
|
token = strtok(NULL, ",");
|
|
}
|
|
if (lat[0] != 0 && lon[0] != 0) {
|
|
data->latitude = convert_to_decimal(lat, lat_dir);
|
|
data->longitude = convert_to_decimal(lon, lon_dir);
|
|
data->has_fix = true;
|
|
}
|
|
}
|
|
gps_index = 0;
|
|
} else if (gps_index < BUFFER_SIZE - 1) {
|
|
buffer[gps_index++] = c;
|
|
}
|
|
}
|
|
} |