From 07f4b198bce76b53405426a4488aa28d33aefbbd Mon Sep 17 00:00:00 2001 From: Mark Qvist Date: Thu, 7 Nov 2019 20:52:48 +0100 Subject: [PATCH] Implemented setting internal clock from GPS --- hardware/GPS.c | 105 ++++++++++++++++++++++++++++++++++-------------- hardware/GPS.h | 2 + hardware/SD.c | 26 ++++++++---- protocol/KISS.c | 31 +++++++++++++- protocol/KISS.h | 4 +- util/Config.c | 11 +++++ util/Config.h | 7 ++++ util/time.h | 17 ++++++++ 8 files changed, 162 insertions(+), 41 deletions(-) diff --git a/hardware/GPS.c b/hardware/GPS.c index a028035..62b547c 100644 --- a/hardware/GPS.c +++ b/hardware/GPS.c @@ -1,4 +1,8 @@ #include "GPS.h" +#include "util/Config.h" +#include "protocol/KISS.h" + +#include Serial *serial; @@ -35,13 +39,20 @@ void gps_init(Serial *ser) { gps_speed_kmh = 0; gps_bearing = 0; + gps_time_set = false; + if (gps_detect()) { gps_installed = true; serial_setbaudrate_9600(1); - gps_send_command(PMTK_SET_BAUD_57600); - serial_setbaudrate_57600(1); + delay_ms(100); + gps_send_command(PMTK_SET_BAUD_57600); + delay_ms(100); + + serial_setbaudrate_57600(1); + delay_ms(100); + gps_send_command(PMTK_API_SET_FIX_CTL_1HZ); gps_send_command(PMTK_SET_NMEA_OUTPUT_RMCGGA); @@ -55,7 +66,19 @@ void gps_init(Serial *ser) { } void gps_update_rtc(void) { - // TODO: implement this + struct tm now; + + now.tm_year = (gps_t_year+2000) - 1900; + now.tm_mon = gps_t_month-1; + now.tm_mday = gps_t_day; + now.tm_hour = gps_t_hour; + now.tm_min = gps_t_minute; + now.tm_sec = gps_t_second; + now.tm_isdst = -1; + + time_t timestamp = mktime(&now); + rtc_set_seconds(timestamp); + gps_time_set = true; } void gps_jobs(void) { @@ -67,6 +90,14 @@ void gps_send_command(const char *cmd) { } void gps_nmea_parse(uint8_t sentence_length) { + if (config_gps_nmea_output == CONFIG_GPS_NMEA_RAW) { + printf("%s\n\r", nmea_parse_buf); + } + + if (config_gps_nmea_output == CONFIG_GPS_NMEA_ENCAP) { + kiss_output_nmea(nmea_parse_buf, sentence_length); + } + if (sentence_length > 4) { if (nmea_parse_buf[sentence_length-4] == '*') { uint16_t checksum = gps_nmea_parse_hex(nmea_parse_buf[sentence_length-3]); @@ -84,9 +115,8 @@ void gps_nmea_parse(uint8_t sentence_length) { if (strstr(nmea_parse_buf, "$GPGGA")) { char *pointer = nmea_parse_buf; - // Parse UTC time + // Ignore UTC time pointer = strchr(pointer, ',')+1; - uint32_t nmea_time = (float)atof(pointer); // Parse latitude pointer = strchr(pointer, ',')+1; @@ -138,19 +168,13 @@ void gps_nmea_parse(uint8_t sentence_length) { // Get fix quality pointer = strchr(pointer, ',')+1; uint8_t nmea_fix = atoi(pointer); - if (nmea_fix == 1) { + if (nmea_fix > 0 && nmea_fix < 7) { gps_fix = true; } else { gps_fix = false; } if (gps_fix) { - // Set times - gps_t_hour = nmea_time / 10000; - gps_t_minute = (nmea_time % 10000) / 100; - gps_t_second = (nmea_time % 100); - gps_update_rtc(); - // Set latitude and longtitude gps_lat_degrees = atoi(nmea_lat_deg_str); gps_lat_minutes = atoi(nmea_lat_min_str); @@ -188,23 +212,7 @@ void gps_nmea_parse(uint8_t sentence_length) { gps_lat_sign = nmea_lat_sign; gps_lon_sign = nmea_lon_sign; gps_lat *= (gps_lat_sign == 'N' ? 1 : -1); - gps_lon *= (gps_lon_sign == 'E' ? 1 : -1); - - // TODO: Remove this - // printf("GPS fix: %d\r\n", nmea_fix); - // printf("GPS satellites: %d\r\n", gps_sats); - // printf("GPS latitude: %d\" %d' %.2fs %c\r\n", gps_lat_degrees, gps_lat_minutes, gps_lat_seconds, gps_lat_sign); - // printf("GPS longtitude: %d\" %d' %.2fs %c\r\n", gps_lon_degrees, gps_lon_minutes, gps_lon_seconds, gps_lon_sign); - // printf("GPS coords: %.6f,%.6f\r\n", gps_lat, gps_lon); - // printf("GPS speed %.2f Km/h\r\n", gps_speed_kmh); - // printf("GPS speed %.2f knots\r\n", gps_speed_knots); - // printf("GPS bearing %.2f\r\n", gps_bearing); - // printf("GPS height above MSL: %.2f\r\n", gps_height_above_msl); - // printf("GPS altitude: %.2f\r\n", gps_altitude); - // printf("GPS geoid height: %.2f\r\n", gps_geoid_height); - // printf("GPS HDOP: %.2f\r\n", gps_hdop); - // printf("GPS time %d:%d:%d UTC\r\n", gps_t_hour, gps_t_minute, gps_t_second); - + gps_lon *= (gps_lon_sign == 'E' ? 1 : -1); } } @@ -212,10 +220,13 @@ void gps_nmea_parse(uint8_t sentence_length) { if (strstr(nmea_parse_buf, "$GPRMC")) { if (gps_fix) { char *pointer = nmea_parse_buf; + uint32_t nmea_date = 0; + uint32_t nmea_time = 0; - // Ignore UTC time + // Get UTC time pointer = strchr(pointer, ',')+1; - + if (!gps_time_set) nmea_time = (float)atof(pointer); + // Ignore navigation receiver warning pointer = strchr(pointer, ',')+1; @@ -239,6 +250,36 @@ void gps_nmea_parse(uint8_t sentence_length) { // Get bearing pointer = strchr(pointer, ',')+1; gps_bearing = atof(pointer); + + // Get date + pointer = strchr(pointer, ',')+1; + if (!gps_time_set) nmea_date = (float)atof(pointer); + + // Set times + if (!gps_time_set) { + gps_t_hour = nmea_time / 10000; + gps_t_minute = (nmea_time % 10000) / 100; + gps_t_second = (nmea_time % 100); + gps_t_day = nmea_date / 10000; + gps_t_month = (nmea_date % 10000) / 100; + gps_t_year = (nmea_date % 100); + gps_update_rtc(); + } + + // TODO: Remove this + // printf("GPS fix\r\n"); + // printf("GPS satellites: %d\r\n", gps_sats); + // printf("GPS latitude: %d\" %d' %.2fs %c\r\n", gps_lat_degrees, gps_lat_minutes, gps_lat_seconds, gps_lat_sign); + // printf("GPS longtitude: %d\" %d' %.2fs %c\r\n", gps_lon_degrees, gps_lon_minutes, gps_lon_seconds, gps_lon_sign); + // printf("GPS coords: %.6f,%.6f\r\n", gps_lat, gps_lon); + // printf("GPS speed %.2f Km/h\r\n", gps_speed_kmh); + // printf("GPS speed %.2f knots\r\n", gps_speed_knots); + // printf("GPS bearing %.2f\r\n", gps_bearing); + // printf("GPS height above MSL: %.2f\r\n", gps_height_above_msl); + // printf("GPS altitude: %.2f\r\n", gps_altitude); + // printf("GPS geoid height: %.2f\r\n", gps_geoid_height); + // printf("GPS HDOP: %.2f\r\n", gps_hdop); + // printf("GPS time %d/%d/%d %d:%d:%d UTC\r\n", gps_t_year, gps_t_month, gps_t_day, gps_t_hour, gps_t_minute, gps_t_second); } else { gps_speed_knots = 0; gps_speed_kmh = 0; @@ -257,6 +298,8 @@ void gps_serial_callback(char byte) { memset(nmea_input_buf, 0, sizeof(nmea_input_buf)); gps_nmea_parse(nmea_read_length); + memset(nmea_parse_buf, 0, sizeof(nmea_parse_buf)); + nmea_read_length = 0; } else { if (nmea_read_length < NMEA_MAX_LENGTH) { diff --git a/hardware/GPS.h b/hardware/GPS.h index c9166f3..831d760 100644 --- a/hardware/GPS.h +++ b/hardware/GPS.h @@ -31,6 +31,8 @@ void gps_send_command(const char *cmd); uint8_t gps_parse_nmea(char *nmea); uint8_t gps_nmea_parse_hex(char c); +bool gps_time_set; + uint8_t gps_t_year; uint8_t gps_t_month; uint8_t gps_t_day; diff --git a/hardware/SD.c b/hardware/SD.c index a578dd4..b41d2c8 100644 --- a/hardware/SD.c +++ b/hardware/SD.c @@ -1,5 +1,7 @@ #include "SD.h" #include "hardware/Crypto.h" +#include "util/time.h" +#include FATFS sdfs; @@ -163,11 +165,21 @@ void sd_scheduler(void) { // TODO: Get time from RTC or host DWORD get_fattime (void) { - // Returns current time packed into a DWORD variable - return ((DWORD)(2018 - 1980) << 25) // Year 2013 - | ((DWORD)8 << 21) // Month 7 - | ((DWORD)2 << 16) // Mday 28 - | ((DWORD)20 << 11) // Hour 0..24 - | ((DWORD)30 << 5) // Min 0 - | ((DWORD)0 >> 1); // Sec 0 + time_t timestamp = rtc_seconds(); + struct tm now; + gmtime_r(×tamp, &now); + + int16_t year = now.tm_year; + int8_t month = now.tm_mon+1; + int8_t day = now.tm_mday; + int8_t hour = now.tm_hour; + int8_t minute = now.tm_min; + int8_t second = now.tm_sec; + + return ((DWORD)(year - 2000) << 25) + | ((DWORD)month << 21) + | ((DWORD)day << 16) + | ((DWORD)hour << 11) + | ((DWORD)minute << 5) + | ((DWORD)second >> 1); } \ No newline at end of file diff --git a/protocol/KISS.c b/protocol/KISS.c index 58ebbc3..853de2d 100755 --- a/protocol/KISS.c +++ b/protocol/KISS.c @@ -157,7 +157,7 @@ void kiss_messageCallback(AX25Ctx *ctx) { if (log_fr == FR_OK) { // Write PCAP segment to file UINT written = 0; - uint32_t pcap_ts_sec = rtc_seconds(); + uint32_t pcap_ts_sec = rtc_unix_timestamp(); uint32_t pcap_ts_usec = (rtc_milliseconds()*(uint32_t)1000); uint32_t pcap_incl_len = ctx->frame_len-2; uint32_t pcap_orig_len = ctx->frame_len-2; @@ -416,7 +416,7 @@ void kiss_flushQueue(void) { if (log_fr == FR_OK) { // Write PCAP segment to file UINT written = 0; - uint32_t pcap_ts_sec = rtc_seconds(); + uint32_t pcap_ts_sec = rtc_unix_timestamp(); uint32_t pcap_ts_usec = (rtc_milliseconds()*(uint32_t)1000); uint32_t pcap_incl_len = length; uint32_t pcap_orig_len = length; @@ -574,6 +574,15 @@ void kiss_serialCallback(uint8_t sbyte) { } config_set_gps_mode(sbyte); } + } else if (command == CMD_NMEA) { + if (sbyte == FESC) { ESCAPE = true; } else { + if (ESCAPE) { + if (sbyte == TFEND) sbyte = FEND; + if (sbyte == TFESC) sbyte = FESC; + ESCAPE = false; + } + config_set_nmea_output(sbyte); + } } else if (command == CMD_BT_MODE) { if (sbyte == FESC) { ESCAPE = true; } else { if (ESCAPE) { @@ -638,6 +647,24 @@ void kiss_output_afsk_peak(void) { fputc(FEND, &serial->uart0); } +void kiss_output_nmea(char* data, size_t length) { + fputc(FEND, &serial->uart0); + fputc(CMD_NMEA, &serial->uart0); + for (unsigned i = 0; i < length; i++) { + uint8_t b = data[i]; + if (b == FEND) { + fputc(FESC, &serial->uart0); + fputc(TFEND, &serial->uart0); + } else if (b == FESC) { + fputc(FESC, &serial->uart0); + fputc(TFESC, &serial->uart0); + } else { + fputc(b, &serial->uart0); + } + } + fputc(FEND, &serial->uart0); +} + void kiss_output_config(uint8_t* data, size_t length) { fputc(FEND, &serial->uart0); fputc(CMD_PRINT_CONFIG, &serial->uart0); diff --git a/protocol/KISS.h b/protocol/KISS.h index c65071c..6b6bb51 100755 --- a/protocol/KISS.h +++ b/protocol/KISS.h @@ -34,6 +34,7 @@ #define CMD_AUDIO_PEAK 0x12 #define CMD_ENABLE_DIAGNOSTICS 0x13 #define CMD_MODE 0x14 +#define CMD_NMEA 0x40 #define CMD_PRINT_CONFIG 0xF0 #define CMD_RETURN 0xFF @@ -45,8 +46,9 @@ void kiss_csma(void); void kiss_poll(void); void kiss_output_modem_mode(void); -void kiss_output_config(uint8_t* data, size_t length); void kiss_output_afsk_peak(void); +void kiss_output_config(uint8_t* data, size_t length); +void kiss_output_nmea(char* data, size_t length); bool log_init(void); bool load_log_index(void); diff --git a/util/Config.c b/util/Config.c index 7849b9b..b6e3cb0 100644 --- a/util/Config.c +++ b/util/Config.c @@ -25,9 +25,14 @@ void config_init(void) { config_save_to_eeprom(); } + config_init_ephemeral(); config_apply(); } +void config_init_ephemeral(void) { + config_gps_nmea_output = CONFIG_GPS_NMEA_NONE; +} + void config_apply(void) { if (config_serial_baudrate == CONFIG_BAUDRATE_1200) serial_setbaudrate_1200(0); if (config_serial_baudrate == CONFIG_BAUDRATE_2400) serial_setbaudrate_2400(0); @@ -221,6 +226,12 @@ void config_set_gps_mode(uint8_t mode) { if (mode == CONFIG_GPS_REQUIRED) config_gps_mode = CONFIG_GPS_REQUIRED; } +void config_set_nmea_output(uint8_t nmea_output) { + if (nmea_output == CONFIG_GPS_NMEA_NONE) config_gps_nmea_output = nmea_output; + if (nmea_output == CONFIG_GPS_NMEA_RAW) config_gps_nmea_output = nmea_output; + if (nmea_output == CONFIG_GPS_NMEA_ENCAP) config_gps_nmea_output = nmea_output; +} + void config_set_bt_mode(uint8_t mode) { if (mode == CONFIG_BLUETOOTH_OFF) config_bluetooth_mode = CONFIG_BLUETOOTH_OFF; if (mode == CONFIG_BLUETOOTH_AUTODETECT) config_bluetooth_mode = CONFIG_BLUETOOTH_AUTODETECT; diff --git a/util/Config.h b/util/Config.h index d97942d..8833c51 100644 --- a/util/Config.h +++ b/util/Config.h @@ -29,6 +29,10 @@ #define CONFIG_GPS_AUTODETECT 0x01 #define CONFIG_GPS_REQUIRED 0x02 +#define CONFIG_GPS_NMEA_NONE 0x00 +#define CONFIG_GPS_NMEA_RAW 0x01 +#define CONFIG_GPS_NMEA_ENCAP 0x02 + #define CONFIG_BLUETOOTH_OFF 0x00 #define CONFIG_BLUETOOTH_AUTODETECT 0x01 #define CONFIG_BLUETOOTH_REQUIRED 0x02 @@ -66,12 +70,14 @@ bool config_passall; bool config_log_packets; bool config_crypto_lock; uint8_t config_gps_mode; +uint8_t config_gps_nmea_output; uint8_t config_bluetooth_mode; uint8_t config_serial_baudrate; bool config_output_diagnostics; void config_init(void); +void config_init_ephemeral(void); void config_apply(void); void config_save(void); @@ -94,6 +100,7 @@ void config_set_output_gain(uint8_t gain); void config_set_input_gain(uint8_t gain); void config_set_passall(uint8_t passall); void config_set_log_packets(uint8_t log_packets); +void config_set_nmea_output(uint8_t nmea_output); void config_set_gps_mode(uint8_t mode); void config_set_bt_mode(uint8_t mode); diff --git a/util/time.h b/util/time.h index 40274e0..c361b5c 100755 --- a/util/time.h +++ b/util/time.h @@ -6,6 +6,8 @@ #include "hardware/LED.h" #include "hardware/sdcard/diskio.h" +#define UNIX_EPOCH_OFFSET 946684800 + #define DIV_ROUND(dividend, divisor) (((dividend) + (divisor) / 2) / (divisor)) typedef int32_t ticks_t; @@ -43,10 +45,25 @@ static inline uint32_t rtc_seconds(void) { return result; } +static inline uint32_t rtc_unix_timestamp(void) { + uint32_t result; + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + result = _rtc_seconds; + } + + return result+UNIX_EPOCH_OFFSET; +} + static inline mtime_t rtc_milliseconds(void) { return ticks_to_ms(timer_clock() % CLOCK_TICKS_PER_SEC); } +static inline void rtc_set_seconds(uint32_t seconds) { + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + _rtc_seconds = seconds; + } +} + inline void cpu_relax(void) { // Do nothing! }