diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 5e6ac432ac..9b21ef198c 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -104,6 +104,7 @@ const char * const debugModeNames[DEBUG_COUNT] = { "GPS_RESCUE_VELOCITY", "GPS_RESCUE_HEADING", "GPS_RESCUE_TRACKING", + "GPS_CONNECTION", "ATTITUDE", "VTX_MSP", "GPS_DOP", diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 7b71d583e9..0662e342a7 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -104,6 +104,7 @@ typedef enum { DEBUG_GPS_RESCUE_VELOCITY, DEBUG_GPS_RESCUE_HEADING, DEBUG_GPS_RESCUE_TRACKING, + DEBUG_GPS_CONNECTION, DEBUG_ATTITUDE, DEBUG_VTX_MSP, DEBUG_GPS_DOP, diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 4d5f1d6b26..83a914cb4f 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4787,7 +4787,7 @@ if (buildKey) { #ifdef USE_GPS cliPrint("GPS: "); if (featureIsEnabled(FEATURE_GPS)) { - if (gpsIsHealthy()) { + if (gpsData.state >= GPS_STATE_CONFIGURE) { cliPrint("connected, "); } else { cliPrint("NOT CONNECTED, "); @@ -4808,7 +4808,7 @@ if (buildKey) { cliPrint("), "); } } - if (!gpsIsHealthy()) { + if (gpsData.state <= GPS_STATE_CONFIGURE) { cliPrint("NOT CONFIGURED"); } else { if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) { @@ -4817,6 +4817,12 @@ if (buildKey) { cliPrint("configured"); } } + if (gpsData.platformVersion != UBX_VERSION_UNDEF) { + cliPrint(", version = "); + cliPrintf("%s", ubloxVersionMap[gpsData.platformVersion].str); + } else { + cliPrint("unknown"); + } } else { cliPrint("NOT ENABLED"); } @@ -4864,15 +4870,15 @@ static void cliTasks(const char *cmdName, char *cmdline) if (systemConfig()->task_statistics) { #if defined(USE_LATE_TASK_STATISTICS) cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d %6d %6d %7d", - taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10, - maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, - taskInfo.totalExecutionTimeUs / 1000, - taskInfo.lateCount, taskInfo.runCount, taskInfo.execTime); + taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10, + maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, + taskInfo.totalExecutionTimeUs / 1000, + taskInfo.lateCount, taskInfo.runCount, taskInfo.execTime); #else cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d", - taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10, - maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, - taskInfo.totalExecutionTimeUs / 1000); + taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10, + maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, + taskInfo.totalExecutionTimeUs / 1000); #endif } else { cliPrintLinef("%6d", taskFrequency); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 79a2642cf2..8cb0437b77 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -198,17 +198,21 @@ static const char * const lookupTableGyro[] = { #endif #ifdef USE_GPS -static const char * const lookupTableGPSProvider[] = { +static const char * const lookupTableGpsProvider[] = { "NMEA", "UBLOX", "MSP" }; -static const char * const lookupTableGPSSBASMode[] = { +static const char * const lookupTableGpsSbasMode[] = { "AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE" }; -static const char * const lookupTableGPSUBLOXModels[] = { +static const char * const lookupTableGpsUbloxModels[] = { "PORTABLE", "STATIONARY", "PEDESTRIAN", "AUTOMOTIVE", "AT_SEA", "AIRBORNE_1G", "AIRBORNE_2G", "AIRBORNE_4G" }; + +static const char * const lookupTableGpsUbloxUtcStandard[] = { + "AUTO", "USNO", "EU", "SU", "NTSC" +}; #endif #ifdef USE_SERVOS @@ -533,9 +537,10 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableUnit), LOOKUP_TABLE_ENTRY(lookupTableAlignment), #ifdef USE_GPS - LOOKUP_TABLE_ENTRY(lookupTableGPSProvider), - LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode), - LOOKUP_TABLE_ENTRY(lookupTableGPSUBLOXModels), + LOOKUP_TABLE_ENTRY(lookupTableGpsProvider), + LOOKUP_TABLE_ENTRY(lookupTableGpsSbasMode), + LOOKUP_TABLE_ENTRY(lookupTableGpsUbloxModels), + LOOKUP_TABLE_ENTRY(lookupTableGpsUbloxUtcStandard), #ifdef USE_GPS_RESCUE LOOKUP_TABLE_ENTRY(lookupTableRescueSanityType), LOOKUP_TABLE_ENTRY(lookupTableRescueAltitudeMode), @@ -1017,9 +1022,9 @@ const clivalue_t valueTable[] = { { PARAM_NAME_GPS_AUTO_BAUD, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, { PARAM_NAME_GPS_UBLOX_ACQUIRE_MODEL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_acquire_model) }, { PARAM_NAME_GPS_UBLOX_FLIGHT_MODEL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_flight_model) }, - { PARAM_NAME_GPS_UPDATE_RATE_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {1, 19}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_update_rate_hz) }, + { PARAM_NAME_GPS_UPDATE_RATE_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {1, 20}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_update_rate_hz) }, + { PARAM_NAME_GPS_UBLOX_UTC_STANDARD, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_UTC_STANDARD }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_utc_standard) }, { PARAM_NAME_GPS_UBLOX_USE_GALILEO, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) }, - { PARAM_NAME_GPS_UBLOX_FULL_POWER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_full_power) }, { PARAM_NAME_GPS_SET_HOME_POINT_ONCE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) }, { PARAM_NAME_GPS_USE_3D_SPEED, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) }, { PARAM_NAME_GPS_SBAS_INTEGRITY, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 0ee10597a8..06150196b8 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -33,6 +33,7 @@ typedef enum { TABLE_GPS_PROVIDER, TABLE_GPS_SBAS_MODE, TABLE_GPS_UBLOX_MODELS, + TABLE_GPS_UBLOX_UTC_STANDARD, #endif #ifdef USE_GPS_RESCUE TABLE_GPS_RESCUE_SANITY_CHECK, diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index 3f354ddeef..51b28876a2 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -26,7 +26,7 @@ #define UARTDEV_COUNT_MAX 6 #define UARTHARDWARE_MAX_PINS 4 #ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 128 +#define UART_RX_BUFFER_SIZE 256 #endif #ifndef UART_TX_BUFFER_SIZE #ifdef USE_MSP_DISPLAYPORT @@ -39,7 +39,7 @@ #define UARTDEV_COUNT_MAX 8 #define UARTHARDWARE_MAX_PINS 4 #ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 128 +#define UART_RX_BUFFER_SIZE 256 #endif #ifndef UART_TX_BUFFER_SIZE #ifdef USE_MSP_DISPLAYPORT @@ -52,7 +52,7 @@ #define UARTDEV_COUNT_MAX 11 // UARTs 1 to 10 + LPUART1 #define UARTHARDWARE_MAX_PINS 5 #ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 128 +#define UART_RX_BUFFER_SIZE 256 #endif #ifndef UART_TX_BUFFER_SIZE #ifdef USE_MSP_DISPLAYPORT @@ -65,7 +65,7 @@ #define UARTDEV_COUNT_MAX 11 // UARTs 1 to 5 + LPUART1 (index 10) #define UARTHARDWARE_MAX_PINS 3 #ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 128 +#define UART_RX_BUFFER_SIZE 256 #endif #ifndef UART_TX_BUFFER_SIZE #ifdef USE_MSP_DISPLAYPORT @@ -78,7 +78,7 @@ #define UARTDEV_COUNT_MAX 8 // UARTs 1 to 5 + LPUART1 (index 9) #define UARTHARDWARE_MAX_PINS 5 #ifndef UART_RX_BUFFER_SIZE -#define UART_RX_BUFFER_SIZE 128 +#define UART_RX_BUFFER_SIZE 256 #endif #ifndef UART_TX_BUFFER_SIZE #ifdef USE_MSP_DISPLAYPORT diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 02d7cba577..4f3133a8ff 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -143,9 +143,9 @@ #define PARAM_NAME_GPS_AUTO_CONFIG "gps_auto_config" #define PARAM_NAME_GPS_AUTO_BAUD "gps_auto_baud" #define PARAM_NAME_GPS_UBLOX_USE_GALILEO "gps_ublox_use_galileo" -#define PARAM_NAME_GPS_UBLOX_FULL_POWER "gps_ublox_full_power" #define PARAM_NAME_GPS_UBLOX_ACQUIRE_MODEL "gps_ublox_acquire_model" #define PARAM_NAME_GPS_UBLOX_FLIGHT_MODEL "gps_ublox_flight_model" +#define PARAM_NAME_GPS_UBLOX_UTC_STANDARD "gps_ublox_utc_standard" #define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once" #define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed" #define PARAM_NAME_GPS_NMEA_CUSTOM_COMMANDS "gps_nmea_custom_commands" diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 636c12b3e3..f1a1b52970 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -251,8 +251,6 @@ static void taskGpsRescue(timeUs_t currentTimeUs) if (gpsRescueIsConfigured()) { gpsRescueUpdate(); - } else { - schedulerIgnoreTaskStateTime(); } } #endif diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index a6732ef7da..ff6f763c02 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -379,8 +379,8 @@ static void showGpsPage(void) static uint8_t gpsTicker = 0; static uint32_t lastGPSSvInfoReceivedCount = 0; - if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) { - lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount; + if (dashboardGpsNavSvInfoRcvCount != lastGPSSvInfoReceivedCount) { + lastGPSSvInfoReceivedCount = dashboardGpsNavSvInfoRcvCount; gpsTicker++; gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT; } @@ -419,7 +419,7 @@ static void showGpsPage(void) i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(dev, lineBuffer); - tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount); + tfp_sprintf(lineBuffer, "RX: %d", dashboardGpsPacketCount); padHalfLineBuffer(); i2c_OLED_set_line(dev, rowIndex); i2c_OLED_send_string(dev, lineBuffer); @@ -429,7 +429,7 @@ static void showGpsPage(void) i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(dev, lineBuffer); - tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage); + tfp_sprintf(lineBuffer, "Dt: %d", gpsSol.navIntervalMs); padHalfLineBuffer(); i2c_OLED_set_line(dev, rowIndex); i2c_OLED_send_string(dev, lineBuffer); @@ -439,7 +439,7 @@ static void showGpsPage(void) i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(dev, lineBuffer); - strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); + strncpy(lineBuffer, dashboardGpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); padHalfLineBuffer(); i2c_OLED_set_line(dev, rowIndex++); i2c_OLED_send_string(dev, lineBuffer); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index a05220d6e9..cae97378fd 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -19,6 +19,8 @@ */ #include +#include +#include #include #include @@ -40,7 +42,9 @@ #include "drivers/time.h" #include "io/beeper.h" +#ifdef USE_DASHBOARD #include "io/dashboard.h" +#endif #include "io/gps.h" #include "io/serial.h" @@ -56,26 +60,8 @@ #include "scheduler/scheduler.h" #include "sensors/sensors.h" +#include "common/typeconversion.h" -#define LOG_ERROR '?' -#define LOG_IGNORED '!' -#define LOG_SKIPPED '>' -#define LOG_NMEA_GGA 'g' -#define LOG_NMEA_GSA 's' -#define LOG_NMEA_RMC 'r' -#define LOG_UBLOX_DOP 'D' -#define LOG_UBLOX_SOL 'O' -#define LOG_UBLOX_STATUS 'S' -#define LOG_UBLOX_SVINFO 'I' -#define LOG_UBLOX_POSLLH 'P' -#define LOG_UBLOX_VELNED 'V' - -#define DEBUG_SERIAL_BAUD 0 // set to 1 to debug serial port baud config (/100) -#define DEBUG_UBLOX_INIT 0 // set to 1 to debug ublox initialization -#define DEBUG_UBLOX_FRAMES 0 // set to 1 to debug ublox received frames - -char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; -static char *gpsPacketLogChar = gpsPacketLog; // ********************** // GPS // ********************** @@ -84,13 +70,10 @@ uint16_t GPS_distanceToHome; // distance to home point in meters uint32_t GPS_distanceToHomeCm; int16_t GPS_directionToHome; // direction to home or hol point in degrees * 10 uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters -int16_t nav_takeoff_bearing; #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph gpsSolutionData_t gpsSol; -uint32_t GPS_packetCount = 0; -uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. uint8_t GPS_update = 0; // toogle to distinct a GPS position update (directly or via MSP) uint8_t GPS_numCh; // Details on numCh/svinfo in gps.h @@ -99,13 +82,17 @@ uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N]; uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N]; uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; -// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second) -#define GPS_TIMEOUT (2500) -// How many entries in gpsInitData array below -#define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1) -#define GPS_BAUDRATE_CHANGE_DELAY (200) -// Timeout for waiting ACK/NAK in GPS task cycles (0.25s at 100Hz) -#define UBLOX_ACK_TIMEOUT_MAX_COUNT (25) +// GPS LOST_COMMUNICATION timeout in ms (max time between received nav solutions) +#define GPS_TIMEOUT_MS 2500 +// Timeout for waiting for an ACK or NAK response to a configuration command +#define UBLOX_ACK_TIMEOUT_MS 150 +// Time allowed for module to respond to baud rate change during initial configuration +#define GPS_CONFIG_BAUD_CHANGE_INTERVAL 330 // Time to wait, in ms, between 'test this baud rate' messages +#define GPS_CONFIG_CHANGE_INTERVAL 110 // Time to wait, in ms, between CONFIG steps +#define GPS_BAUDRATE_TEST_COUNT 3 // Number of times to repeat the test message when setting baudrate +#define GPS_RECV_TIME_MAX 25 // Max permitted time, in us, for the Receive Data process +// Decay the estimated max task duration by 1/(1 << GPS_TASK_DECAY_SHIFT) on every invocation +#define GPS_TASK_DECAY_SHIFT 9 // Smoothing factor for GPS task re-scheduler static serialPort_t *gpsPort; static float gpsDataIntervalSeconds; @@ -132,32 +119,52 @@ static const gpsInitData_t gpsInitData[] = { #define DEFAULT_BAUD_RATE_INDEX 0 #ifdef USE_GPS_UBLOX +#define MAX_VALSET_SIZE 128 + typedef enum { PREAMBLE1 = 0xB5, PREAMBLE2 = 0x62, CLASS_NAV = 0x01, CLASS_ACK = 0x05, CLASS_CFG = 0x06, + CLASS_MON = 0x0a, + CLASS_NMEA_STD = 0xf0, MSG_ACK_NACK = 0x00, MSG_ACK_ACK = 0x01, - MSG_POSLLH = 0x02, - MSG_STATUS = 0x03, - MSG_DOP = 0x04, - MSG_SOL = 0x06, - MSG_PVT = 0x07, - MSG_VELNED = 0x12, - MSG_SVINFO = 0x30, - MSG_SAT = 0x35, + MSG_NAV_POSLLH = 0x02, + MSG_NAV_STATUS = 0x03, + MSG_NAV_DOP = 0x04, + MSG_NAV_SOL = 0x06, + MSG_NAV_PVT = 0x07, + MSG_NAV_VELNED = 0x12, + MSG_NAV_SVINFO = 0x30, + MSG_NAV_SAT = 0x35, + MSG_CFG_VALSET = 0x8a, + MSG_CFG_VALGET = 0x8b, MSG_CFG_MSG = 0x01, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, MSG_CFG_SBAS = 0x16, MSG_CFG_NAV_SETTINGS = 0x24, + MSG_CFG_NAVX_SETTINGS = 0x23, MSG_CFG_PMS = 0x86, - MSG_CFG_GNSS = 0x3E + MSG_CFG_GNSS = 0x3E, + MSG_MON_VER = 0x04, + MSG_NMEA_GGA = 0x00, + MSG_NMEA_GLL = 0x01, + MSG_NMEA_GSA = 0x02, + MSG_NMEA_GSV = 0x03, + MSG_NMEA_RMC = 0x04, + MSG_NMEA_VTG = 0x05, } ubxProtocolBytes_e; +typedef enum { + UBX_POWER_MODE_FULL = 0x00, + UBX_POWER_MODE_PSMOO = 0x01, + UBX_POWER_MODE_PSMCT = 0x02, +} ubloxPowerMode_e; + #define UBLOX_MODE_ENABLED 0x1 #define UBLOX_MODE_TEST 0x2 @@ -201,6 +208,20 @@ typedef struct ubxCfgRate_s { uint16_t timeRef; } ubxCfgRate_t; +typedef struct ubxCfgValSet_s { + uint8_t version; + uint8_t layer; + uint8_t reserved[2]; + uint8_t cfgData[MAX_VALSET_SIZE]; +} ubxCfgValSet_t; + +typedef struct ubxCfgValGet_s { + uint8_t version; + uint8_t layer; + uint16_t position; + uint8_t cfgData[MAX_VALSET_SIZE]; +} ubxCfgValGet_t; + typedef struct ubxCfgSbas_s { uint8_t mode; uint8_t usage; @@ -247,11 +268,39 @@ typedef struct ubxCfgNav5_s { uint8_t reserved1[5]; } ubxCfgNav5_t; +typedef struct ubxCfgNav5x_s { + uint16_t version; + uint16_t mask1; + uint32_t mask2; + uint8_t reserved0[2]; + uint8_t minSVs; + uint8_t maxSVs; + uint8_t minCNO; + uint8_t reserved1; + uint8_t iniFix3D; + uint8_t reserved2[2]; + uint8_t ackAiding; + uint16_t wknRollover; + uint8_t sigAttenCompMode; + uint8_t reserved3; + uint8_t reserved4[2]; + uint8_t reserved5[2]; + uint8_t usePPP; + uint8_t aopCfg; + uint8_t reserved6[2]; + uint8_t reserved7[4]; + uint8_t reserved8[3]; + uint8_t useAdr; +} ubxCfgNav5x_t; + typedef union ubxPayload_s { ubxPollMsg_t poll_msg; ubxCfgMsg_t cfg_msg; ubxCfgRate_t cfg_rate; + ubxCfgValSet_t cfg_valset; + ubxCfgValGet_t cfg_valget; ubxCfgNav5_t cfg_nav5; + ubxCfgNav5x_t cfg_nav5x; ubxCfgSbas_t cfg_sbas; ubxCfgGnss_t cfg_gnss; ubxCfgPms_t cfg_pms; @@ -263,52 +312,65 @@ typedef struct ubxMessage_s { } __attribute__((packed)) ubxMessage_t; typedef enum { - UBLOX_INITIALIZE, - UBLOX_MSG_VGS, // 1. VGS: Course over ground and Ground speed - UBLOX_MSG_GSV, // 2. GSV: GNSS Satellites in View - UBLOX_MSG_GLL, // 3. GLL: Latitude and longitude, with time of position fix and status - UBLOX_MSG_GGA, // 4. GGA: Global positioning system fix data - UBLOX_MSG_GSA, // 5. GSA: GNSS DOP and Active Satellites - UBLOX_MSG_RMC, // 6. RMC: Recommended Minimum data - UBLOX_MSG_RATE, // 7. set PVT or SOL MSG rate - UBLOX_MSG_POSLLH, // 8. SOL: set POSLLH MSG rate - UBLOX_MSG_STATUS, // 9: set STATUS MSG rate - UBLOX_MSG_VELNED, // 10. SOL: set VELNED MSG rate - UBLOX_MSG_SATINFO, // 11. MSG_SAT or MSG_SVINFO if not ubloxUseSAT - UBLOX_MSG_DOP, // 12. MSG_DOP - UBLOX_SET_NAV_RATE, // 13. set GPS sample rate - UBLOX_SET_SBAS, // 14. Sets SBAS - UBLOX_SET_PMS, // 15. Sets Power Mode - UBLOX_MSG_CFG_GNSS // 16. For not SBAS or GALILEO otherwise GPS_STATE_REVEIVING_DATA + UBLOX_DETECT_UNIT, // 0 + UBLOX_SLOW_NAV_RATE, // 1. + UBLOX_MSG_DISABLE_NMEA, // 2. Disable NMEA, config message + UBLOX_MSG_VGS, // 3. VGS: Course over ground and Ground speed + UBLOX_MSG_GSV, // 4. GSV: GNSS Satellites in View + UBLOX_MSG_GLL, // 5. GLL: Latitude and longitude, with time of position fix and status + UBLOX_MSG_GGA, // 6. GGA: Global positioning system fix data + UBLOX_MSG_GSA, // 7. GSA: GNSS DOP and Active Satellites + UBLOX_MSG_RMC, // 8. RMC: Recommended Minimum data + UBLOX_ACQUIRE_MODEL, // 9 +// UBLOX_CFG_ANA, // . ANA: if M10, enable autonomous mode : temporarily disabled. + UBLOX_SET_SBAS, // 10. Sets SBAS + UBLOX_SET_PMS, // 11. Sets Power Mode + UBLOX_MSG_NAV_PVT, // 12. set NAV-PVT rate + UBLOX_MSG_SOL, // 13. set SOL MSG rate + UBLOX_MSG_POSLLH, // 14. set POSLLH MSG rate + UBLOX_MSG_STATUS, // 15: set STATUS MSG rate + UBLOX_MSG_VELNED, // 16. set VELNED MSG rate + UBLOX_MSG_DOP, // 17. MSG_NAV_DOP + UBLOX_SAT_INFO, // 18. MSG_NAV_SAT message + UBLOX_SET_NAV_RATE, // 19. set to user requested GPS sample rate + UBLOX_MSG_CFG_GNSS, // 20. For not SBAS or GALILEO + UBLOX_CONFIG_COMPLETE // 21. Config finished, start receiving data } ubloxStatePosition_e; +baudRate_e initBaudRateIndex; +size_t initBaudRateCycleCount; +static void ubloxSendClassMessage(ubxProtocolBytes_e class_id, ubxProtocolBytes_e msg_id, uint16_t length); + #endif // USE_GPS_UBLOX -typedef enum { - GPS_STATE_UNKNOWN, - GPS_STATE_INITIALIZING, - GPS_STATE_INITIALIZED, - GPS_STATE_CHANGE_BAUD, - GPS_STATE_CONFIGURE, - GPS_STATE_RECEIVING_DATA, - GPS_STATE_LOST_COMMUNICATION, - GPS_STATE_COUNT -} gpsState_e; - -// Max time to wait for received data -#define GPS_MAX_WAIT_DATA_RX 30 - gpsData_t gpsData; +#ifdef USE_DASHBOARD +// Functions & data used *only* in support of the dashboard device (OLED display). +// Note this should be refactored to move dashboard functionality to the dashboard module, and only have generic hooks in the gps module... + +char dashboardGpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; // OLED display of a char for each packet type/event received. +char *dashboardGpsPacketLogCurrentChar = dashboardGpsPacketLog; // Current character of log being updated. +uint32_t dashboardGpsPacketCount = 0; // Packet received count. +uint32_t dashboardGpsNavSvInfoRcvCount = 0; // Count of times sat info updated. + static void shiftPacketLog(void) { uint32_t i; - for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) { - gpsPacketLog[i] = gpsPacketLog[i-1]; + for (i = ARRAYLEN(dashboardGpsPacketLog) - 1; i > 0 ; i--) { + dashboardGpsPacketLog[i] = dashboardGpsPacketLog[i-1]; } } +static void logErrorToPacketLog(void) +{ + shiftPacketLog(); + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_ERROR; + gpsData.errors++; +} +#endif // USE_DASHBOARD + static bool isConfiguratorConnected(void) { return (getArmingDisableFlags() & ARMING_DISABLED_MSP); @@ -324,29 +386,35 @@ static bool gpsNewFrameUBLOX(uint8_t data); static void gpsSetState(gpsState_e state) { - gpsData.lastMessage = millis(); + gpsData.lastNavMessage = gpsData.now; sensorsClear(SENSOR_GPS); - gpsData.state = state; - gpsData.state_position = GPS_STATE_UNKNOWN; - gpsData.state_ts = millis(); + gpsData.state_position = 0; + gpsData.state_ts = gpsData.now; gpsData.ackState = UBLOX_ACK_IDLE; } void gpsInit(void) { gpsDataIntervalSeconds = 0.1f; - gpsData.baudrateIndex = 0; - gpsData.errors = 0; + gpsData.userBaudRateIndex = 0; gpsData.timeouts = 0; + gpsData.satMessagesDisabled = false; + gpsData.state_ts = millis(); +#ifdef USE_GPS_UBLOX + gpsData.ubloxUsingFlightModel = false; +#endif + gpsData.updateRateHz = 10; // initialise at 10hz + gpsData.platformVersion = UBX_VERSION_UNDEF; - memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog)); +#ifdef USE_DASHBOARD + gpsData.errors = 0; + memset(dashboardGpsPacketLog, 0x00, sizeof(dashboardGpsPacketLog)); +#endif // init gpsData structure. if we're not actually enabled, don't bother doing anything else gpsSetState(GPS_STATE_UNKNOWN); - gpsData.lastMessage = millis(); - if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured gpsSetState(GPS_STATE_INITIALIZED); return; @@ -357,15 +425,21 @@ void gpsInit(void) return; } - while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) { - gpsData.baudrateIndex++; - if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) { - gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX; - break; - } + // set the user's intended baud rate + initBaudRateIndex = BAUD_COUNT; + initBaudRateCycleCount = 0; + gpsData.userBaudRateIndex = DEFAULT_BAUD_RATE_INDEX; + for (unsigned i = 0; i < GPS_INIT_DATA_ENTRY_COUNT; i++) { + if (gpsInitData[i].baudrateIndex == gpsPortConfig->gps_baudrateIndex) { + gpsData.userBaudRateIndex = i; + break; + } } + // the user's intended baud rate will be used as the initial baud rate when connecting + gpsData.tempBaudRateIndex = gpsData.userBaudRateIndex; portMode_e mode = MODE_RXTX; + #if defined(GPS_NMEA_TX_ONLY) if (gpsConfig()->provider == GPS_NMEA) { mode &= ~MODE_TX; @@ -373,106 +447,105 @@ void gpsInit(void) #endif // no callback - buffer will be consumed in gpsUpdate() - gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED); + gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED); if (!gpsPort) { return; } // signal GPS "thread" to initialize when it gets to it - gpsSetState(GPS_STATE_INITIALIZING); + gpsSetState(GPS_STATE_DETECT_BAUD); + // NB gpsData.state_position is set to zero by gpsSetState(), requesting the fastest baud rate option first time around. } -#ifdef USE_GPS_NMEA -void gpsInitNmea(void) -{ - static bool atgmRestartDone = false; -#if !defined(GPS_NMEA_TX_ONLY) - uint32_t now; -#endif - switch (gpsData.state) { - case GPS_STATE_INITIALIZING: -#if !defined(GPS_NMEA_TX_ONLY) - now = millis(); - if (now - gpsData.state_ts < 1000) { - return; - } - gpsData.state_ts = now; - if (gpsData.state_position < GPS_STATE_INITIALIZING) { - serialSetBaudRate(gpsPort, 4800); - gpsData.state_position++; - } else if (gpsData.state_position < GPS_STATE_INITIALIZED) { - // print our FIXED init string for the baudrate we want to be at - serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n"); - gpsData.state_position++; - } else { - // we're now (hopefully) at the correct rate, next state will switch to it - gpsSetState(GPS_STATE_CHANGE_BAUD); - } - break; -#endif - case GPS_STATE_CHANGE_BAUD: -#if !defined(GPS_NMEA_TX_ONLY) - now = millis(); - if (now - gpsData.state_ts < 1000) { - return; - } - gpsData.state_ts = now; - - if (gpsData.state_position < GPS_STATE_INITIALIZING) { - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]); - gpsData.state_position++; - } else if (gpsData.state_position < GPS_STATE_INITIALIZED) { - serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n"); - // special initialization for NMEA ATGM336 and similar GPS recivers - should be done only once - if (!atgmRestartDone) { - atgmRestartDone = true; - serialPrint(gpsPort, "$PCAS02,100*1E\r\n"); // 10Hz refresh rate - serialPrint(gpsPort, "$PCAS10,0*1C\r\n"); // hot restart - } else { - // NMEA custom commands after ATGM336 initialization - static int commandOffset = 0; - const char *commands = gpsConfig()->nmeaCustomCommands; - const char *cmd = commands + commandOffset; - - // skip leading whitespaces and get first command length - int commandLen; - while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) { - cmd++; // skip separators - } - - if (*cmd) { - // Send the current command to the GPS - serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen); - serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2); - - // Move to the next command - cmd += commandLen; - } - - // skip trailing whitespaces - while (*cmd && strcspn(cmd, " \0") == 0) cmd++; - - if (*cmd) { - // more commands to send - commandOffset = cmd - commands; - } else { - gpsData.state_position++; - commandOffset = 0; - } - } - } else -#else - { - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]); - } -#endif - gpsSetState(GPS_STATE_RECEIVING_DATA); - break; - } -} -#endif // USE_GPS_NMEA - #ifdef USE_GPS_UBLOX +const uint8_t ubloxUTCStandardConfig_int[5] = { + UBLOX_UTC_STANDARD_AUTO, + UBLOX_UTC_STANDARD_USNO, + UBLOX_UTC_STANDARD_EU, + UBLOX_UTC_STANDARD_SU, + UBLOX_UTC_STANDARD_NTSC +}; + +struct ubloxVersion_s ubloxVersionMap[] = { + [UBX_VERSION_UNDEF] = {~0, "UNKNOWN"}, + [UBX_VERSION_M5] = {0x00040005, "M5"}, + [UBX_VERSION_M6] = {0x00040007, "M6"}, + [UBX_VERSION_M7] = {0x00070000, "M7"}, + [UBX_VERSION_M8] = {0x00080000, "M8"}, + [UBX_VERSION_M9] = {0x00190000, "M9"}, + [UBX_VERSION_M10] = {0x000A0000, "M10"}, +}; + +static uint8_t ubloxAddValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, const uint8_t * payload, const uint8_t offset) { + size_t len; + switch((key >> (8 * 3)) & 0xff) { + case 0x10: + case 0x20: + len = 1; + break; + case 0x30: + len = 2; + break; + case 0x40: + len = 4; + break; + case 0x50: + len = 8; + break; + default: + return 0; + } + if (offset + 4 + len > MAX_VALSET_SIZE) + { + return 0; + } + + tx_buffer->payload.cfg_valset.cfgData[offset + 0] = (uint8_t)(key >> (8 * 0)); + tx_buffer->payload.cfg_valset.cfgData[offset + 1] = (uint8_t)(key >> (8 * 1)); + tx_buffer->payload.cfg_valset.cfgData[offset + 2] = (uint8_t)(key >> (8 * 2)); + tx_buffer->payload.cfg_valset.cfgData[offset + 3] = (uint8_t)(key >> (8 * 3)); + + for (size_t i = 0; i < len; ++i) { + tx_buffer->payload.cfg_valset.cfgData[offset + 4 + i] = payload[i]; + } + + return 4 + len; +} + +// the following lines are not being used, because we are not currently sending ubloxValGet messages +#if 0 +static size_t ubloxAddValGet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, size_t offset) { + const uint8_t zeroes[8] = {0}; + + return ubloxAddValSet(tx_buffer, key, zeroes, offset); +} + +static size_t ubloxValGet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, ubloxValLayer_e layer) +{ + tx_buffer->header.preamble1 = PREAMBLE1; + tx_buffer->header.preamble2 = PREAMBLE2; + tx_buffer->header.msg_class = CLASS_CFG; + tx_buffer->header.msg_id = MSG_CFG_VALGET; + + tx_buffer->payload.cfg_valget.version = 1; + tx_buffer->payload.cfg_valget.layer = layer; + tx_buffer->payload.cfg_valget.position = 0; + + return ubloxAddValGet(tx_buffer, key, 0); +} +#endif // not used + +static uint8_t ubloxValSet(ubxMessage_t * tx_buffer, ubxValGetSetBytes_e key, uint8_t * payload, ubloxValLayer_e layer) { + memset(&tx_buffer->payload.cfg_valset, 0, sizeof(ubxCfgValSet_t)); + + // tx_buffer->payload.cfg_valset.version = 0; + tx_buffer->payload.cfg_valset.layer = layer; + // tx_buffer->payload.cfg_valset.reserved[0] = 0; + // tx_buffer->payload.cfg_valset.reserved[1] = 0; + + return ubloxAddValSet(tx_buffer, key, payload, 0); +} + static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB) { *checksumA += data; @@ -488,7 +561,7 @@ static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_ } } -static void ubloxSendMessage(const uint8_t *data, uint8_t len) +static void ubloxSendMessage(const uint8_t *data, uint8_t len, bool skipAck) { uint8_t checksumA = 0, checksumB = 0; serialWrite(gpsPort, data[0]); @@ -496,21 +569,31 @@ static void ubloxSendMessage(const uint8_t *data, uint8_t len) ubloxSendDataUpdateChecksum(&data[2], len - 2, &checksumA, &checksumB); serialWrite(gpsPort, checksumA); serialWrite(gpsPort, checksumB); - // Save state for ACK waiting gpsData.ackWaitingMsgId = data[3]; //save message id for ACK - gpsData.ackTimeoutCounter = 0; - gpsData.ackState = UBLOX_ACK_WAITING; + gpsData.ackState = skipAck ? UBLOX_ACK_GOT_ACK : UBLOX_ACK_WAITING; + gpsData.lastMessageSent = gpsData.now; } -static void ubloxSendConfigMessage(ubxMessage_t *message, uint8_t msg_id, uint8_t length) +static void ubloxSendClassMessage(ubxProtocolBytes_e class_id, ubxProtocolBytes_e msg_id, uint16_t length) +{ + ubxMessage_t tx_buffer; + tx_buffer.header.preamble1 = PREAMBLE1; + tx_buffer.header.preamble2 = PREAMBLE2; + tx_buffer.header.msg_class = class_id; + tx_buffer.header.msg_id = msg_id; + tx_buffer.header.length = length; + ubloxSendMessage((const uint8_t *) &tx_buffer, length + 6, false); +} + +static void ubloxSendConfigMessage(ubxMessage_t *message, uint8_t msg_id, uint8_t length, bool skipAck) { message->header.preamble1 = PREAMBLE1; message->header.preamble2 = PREAMBLE2; message->header.msg_class = CLASS_CFG; message->header.msg_id = msg_id; message->header.length = length; - ubloxSendMessage((const uint8_t *) message, length + 6); + ubloxSendMessage((const uint8_t *) message, length + 6, skipAck); } static void ubloxSendPollMessage(uint8_t msg_id) @@ -521,50 +604,159 @@ static void ubloxSendPollMessage(uint8_t msg_id) tx_buffer.header.msg_class = CLASS_CFG; tx_buffer.header.msg_id = msg_id; tx_buffer.header.length = 0; - ubloxSendMessage((const uint8_t *) &tx_buffer, 6); + ubloxSendMessage((const uint8_t *) &tx_buffer, 6, false); } -static void ubloxSendNAV5Message(uint8_t model) +static void ubloxSendNAV5Message(uint8_t model) { + DEBUG_SET(DEBUG_GPS_CONNECTION, 0, model); + ubxMessage_t tx_buffer; + if (gpsData.ubloxM9orAbove) { + uint8_t payload[4]; + payload[0] = (model == 0 ? 0 : model + 1); + size_t offset = ubloxValSet(&tx_buffer, CFG_NAVSPG_DYNMODEL, payload, UBX_VAL_LAYER_RAM); // 5 + + // the commented out payload lines are those which only set the M9 or above module to default values. + +// payload[0] = 3; // set 2D/3D fix mode to auto, which is the default +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_FIXMODE, payload, offset); // 10 + + payload[0] = ubloxUTCStandardConfig_int[gpsConfig()->gps_ublox_utc_standard]; + offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_UTCSTANDARD, payload, offset); // 15 + +// payload[0] = 0; +// payload[1] = (uint8_t)(0 >> (8 * 1)); +// payload[2] = (uint8_t)(0 >> (8 * 2)); +// payload[3] = (uint8_t)(0 >> (8 * 3)); // all payloads are zero, the default MSL for 2D fix +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_CONSTR_ALT, payload, offset); // 23 +// +// payload[0] = (uint8_t)(10000 >> (8 * 0)); +// payload[1] = (uint8_t)(10000 >> (8 * 1)); +// payload[2] = (uint8_t)(10000 >> (8 * 2)); +// payload[3] = (uint8_t)(10000 >> (8 * 3)); // // all payloads are 1000, the default 2D variance factor +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_CONSTR_ALTVAR, payload, offset); // 31 +// +// payload[0] = 5; // sets the default minimum elevation in degrees to the default of 5 +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_INFIL_MINELEV, payload, offset); // 36 +// +// payload[0] = (uint8_t)(250 >> (8 * 0)); +// payload[1] = (uint8_t)(250 >> (8 * 1)); // sets the output filter PDOP mask to default of 250 +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_OUTFIL_PDOP, payload, offset); // 42 +// +// payload[0] = (uint8_t)(250 >> (8 * 0)); +// payload[1] = (uint8_t)(250 >> (8 * 1)); +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_OUTFIL_TDOP, payload, offset); // 48 +// +// payload[0] = (uint8_t)(100 >> (8 * 0)); +// payload[1] = (uint8_t)(100 >> (8 * 1)); +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_OUTFIL_PACC, payload, offset); // 54 +// +// payload[0] = (uint8_t)(300 >> (8 * 0)); +// payload[1] = (uint8_t)(300 >> (8 * 1)); +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_OUTFIL_TACC, payload, offset); // 60 +// +// payload[0] = 0; +// offset += ubloxAddValSet(&tx_buffer, CFG_MOT_GNSSSPEED_THRS, payload, offset); // 65 +// +// payload[0] = (uint8_t)(200 >> (8 * 0)); +// payload[1] = (uint8_t)(200 >> (8 * 1)); +// offset += ubloxAddValSet(&tx_buffer, CFG_MOT_GNSSDIST_THRS, payload, offset); // 71 + +// payload[0] = (uint8_t)(60 >> (8 * 0)); +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_CONSTR_DGNSSTO, payload, offset); // 76 + +// payload[0] = 0; +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_INFIL_NCNOTHRS, payload, offset); // 81 +// +// payload[0] = 0; +// offset += ubloxAddValSet(&tx_buffer, CFG_NAVSPG_INFIL_CNOTHRS, payload, offset); // 86 + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); + } else { + memset(&tx_buffer, 0, sizeof(ubxMessage_t)); + tx_buffer.payload.cfg_nav5.mask = 0xFFFF; + tx_buffer.payload.cfg_nav5.dynModel = model == 0 ? 0 : model + 1; //no model with value 1 + tx_buffer.payload.cfg_nav5.fixMode = 3; + tx_buffer.payload.cfg_nav5.fixedAlt = 0; + tx_buffer.payload.cfg_nav5.fixedAltVar = 10000; + tx_buffer.payload.cfg_nav5.minElev = 5; + tx_buffer.payload.cfg_nav5.drLimit = 0; + tx_buffer.payload.cfg_nav5.pDOP = 250; + tx_buffer.payload.cfg_nav5.tDOP = 250; + tx_buffer.payload.cfg_nav5.pAcc = 100; + tx_buffer.payload.cfg_nav5.tAcc = 300; + tx_buffer.payload.cfg_nav5.staticHoldThresh = 0; + tx_buffer.payload.cfg_nav5.dgnssTimeout = 60; + tx_buffer.payload.cfg_nav5.cnoThreshNumSVs = 0; + tx_buffer.payload.cfg_nav5.cnoThresh = 0; + tx_buffer.payload.cfg_nav5.staticHoldMaxDist = 200; + tx_buffer.payload.cfg_nav5.utcStandard = ubloxUTCStandardConfig_int[gpsConfig()->gps_ublox_utc_standard]; + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAV_SETTINGS, sizeof(ubxCfgNav5_t), false); + } +} + +// *** Assist Now Autonomous temporarily disabled until a subsequent PR either includes, or removes it *** +// static void ubloxSendNavX5Message(void) { +// ubxMessage_t tx_buffer; +// +// if (gpsData.ubloxM9orAbove) { +// uint8_t payload[1]; +// payload[0] = 1; +// size_t offset = ubloxValSet(&tx_buffer, CFG_ANA_USE_ANA, payload, UBX_VAL_LAYER_RAM); // 5 +// +// ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); +// } else { +// memset(&tx_buffer, 0, sizeof(ubxMessage_t)); +// +// tx_buffer.payload.cfg_nav5x.version = 0x0002; +// +// tx_buffer.payload.cfg_nav5x.mask1 = 0x4000; +// tx_buffer.payload.cfg_nav5x.mask2 = 0x0; +// tx_buffer.payload.cfg_nav5x.minSVs = 0; +// tx_buffer.payload.cfg_nav5x.maxSVs = 0; +// tx_buffer.payload.cfg_nav5x.minCNO = 0; +// tx_buffer.payload.cfg_nav5x.reserved1 = 0; +// tx_buffer.payload.cfg_nav5x.iniFix3D = 0; +// tx_buffer.payload.cfg_nav5x.ackAiding = 0; +// tx_buffer.payload.cfg_nav5x.wknRollover = 0; +// tx_buffer.payload.cfg_nav5x.sigAttenCompMode = 0; +// tx_buffer.payload.cfg_nav5x.usePPP = 0; +// +// tx_buffer.payload.cfg_nav5x.aopCfg = 0x1; //bit 0 = useAOP +// +// tx_buffer.payload.cfg_nav5x.useAdr = 0; +// +// ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAVX_SETTINGS, sizeof(ubxCfgNav5x_t), false); +// } +// } + +static void ubloxSetPowerModeValSet(uint8_t powerSetupValue) { ubxMessage_t tx_buffer; - tx_buffer.payload.cfg_nav5.mask = 0xFFFF; - tx_buffer.payload.cfg_nav5.dynModel = model == 0 ? model : model+1; //no model with value 1 - tx_buffer.payload.cfg_nav5.fixMode = 3; - tx_buffer.payload.cfg_nav5.fixedAlt = 0; - tx_buffer.payload.cfg_nav5.fixedAltVar = 10000; - tx_buffer.payload.cfg_nav5.minElev = 5; - tx_buffer.payload.cfg_nav5.drLimit = 0; - tx_buffer.payload.cfg_nav5.pDOP = 250; - tx_buffer.payload.cfg_nav5.tDOP = 250; - tx_buffer.payload.cfg_nav5.pAcc = 100; - tx_buffer.payload.cfg_nav5.tAcc = 300; - tx_buffer.payload.cfg_nav5.staticHoldThresh = 0; - tx_buffer.payload.cfg_nav5.dgnssTimeout = 60; - tx_buffer.payload.cfg_nav5.cnoThreshNumSVs = 0; - tx_buffer.payload.cfg_nav5.cnoThresh = 0; - tx_buffer.payload.cfg_nav5.reserved0[0] = 0; - tx_buffer.payload.cfg_nav5.reserved0[1] = 0; - tx_buffer.payload.cfg_nav5.staticHoldMaxDist = 200; - tx_buffer.payload.cfg_nav5.utcStandard = 0; - tx_buffer.payload.cfg_nav5.reserved1[0] = 0; - tx_buffer.payload.cfg_nav5.reserved1[1] = 0; - tx_buffer.payload.cfg_nav5.reserved1[2] = 0; - tx_buffer.payload.cfg_nav5.reserved1[3] = 0; - tx_buffer.payload.cfg_nav5.reserved1[4] = 0; - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAV_SETTINGS, sizeof(ubxCfgNav5_t)); + uint8_t payload[1]; + payload[0] = powerSetupValue; + + size_t offset = ubloxValSet(&tx_buffer, CFG_PM_OPERATEMODE, payload, UBX_VAL_LAYER_RAM); + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); } static void ubloxSendPowerMode(void) { - ubxMessage_t tx_buffer; - tx_buffer.payload.cfg_pms.version = 0; - tx_buffer.payload.cfg_pms.powerSetupValue = !gpsConfig()->gps_ublox_full_power; - tx_buffer.payload.cfg_pms.period = 0; - tx_buffer.payload.cfg_pms.onTime = 0; - tx_buffer.payload.cfg_pms.reserved1[0] = 0; - tx_buffer.payload.cfg_pms.reserved1[1] = 0; - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_PMS, sizeof(ubxCfgPms_t)); + if (gpsData.ubloxM9orAbove) { + ubloxSetPowerModeValSet(UBX_POWER_MODE_FULL); + } else if (gpsData.ubloxM8orAbove) { + ubxMessage_t tx_buffer; + tx_buffer.payload.cfg_pms.version = 0; + tx_buffer.payload.cfg_pms.powerSetupValue = UBX_POWER_MODE_FULL; + tx_buffer.payload.cfg_pms.period = 0; + tx_buffer.payload.cfg_pms.onTime = 0; + tx_buffer.payload.cfg_pms.reserved1[0] = 0; + tx_buffer.payload.cfg_pms.reserved1[1] = 0; + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_PMS, sizeof(ubxCfgPms_t), false); + } + // M7 and below do not support this type of power mode, so we leave at default. } static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t rate) @@ -573,109 +765,391 @@ static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t tx_buffer.payload.cfg_msg.msgClass = messageClass; tx_buffer.payload.cfg_msg.msgID = messageID; tx_buffer.payload.cfg_msg.rate = rate; - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_MSG, sizeof(ubxCfgMsg_t)); + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_MSG, sizeof(ubxCfgMsg_t), false); } -static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint16_t timeRef) +static void ubloxSetMessageRateValSet(ubxValGetSetBytes_e msgClass, uint8_t rate) { ubxMessage_t tx_buffer; - uint16_t measRateMilliseconds = 1000 / measRate; - // Testing has revealed this is the max rate common modules can achieve - if (measRateMilliseconds < 53) measRateMilliseconds = 53; + uint8_t payload[1]; + payload[0] = rate; - tx_buffer.payload.cfg_rate.measRate = measRateMilliseconds; - tx_buffer.payload.cfg_rate.navRate = navRate; - tx_buffer.payload.cfg_rate.timeRef = timeRef; - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubxCfgRate_t)); + size_t offset = ubloxValSet(&tx_buffer, msgClass, payload, UBX_VAL_LAYER_RAM); + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); +} + +static void ubloxDisableNMEAValSet(void) +{ + ubxMessage_t tx_buffer; + + uint8_t payload[1]; + + payload[0] = 0; + +// size_t offset = ubloxValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GGA_I2C, payload, UBX_VAL_LAYER_RAM); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GGA_SPI, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GGA_UART1, payload, offset); + size_t offset = ubloxValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GGA_UART1, payload, UBX_VAL_LAYER_RAM); + +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_VTG_I2C, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_VTG_SPI, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_VTG_UART1, payload, offset); + +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSV_I2C, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSV_SPI, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSV_UART1, payload, offset); + +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GLL_I2C, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GLL_SPI, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GLL_UART1, payload, offset); + +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSA_I2C, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSA_SPI, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_GSA_UART1, payload, offset); + +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_RMC_I2C, payload, offset); +// offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_RMC_SPI, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_MSGOUT_NMEA_ID_RMC_UART1, payload, offset); + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); +} + +static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint8_t timeRef) +{ + uint16_t measRateMilliseconds = 1000 / measRate; + + ubxMessage_t tx_buffer; + if (gpsData.ubloxM9orAbove) { + uint8_t offset = 0; + uint8_t payload[2]; + payload[0] = (uint8_t)(measRateMilliseconds >> (8 * 0)); + payload[1] = (uint8_t)(measRateMilliseconds >> (8 * 1)); + //rate meas is U2 + offset = ubloxValSet(&tx_buffer, CFG_RATE_MEAS, payload, UBX_VAL_LAYER_RAM); + + payload[0] = (uint8_t)(navRate >> (8 * 0)); + payload[1] = (uint8_t)(navRate >> (8 * 1)); + //rate nav is U2 + offset += ubloxAddValSet(&tx_buffer, CFG_RATE_NAV, payload, 6); + + payload[0] = timeRef; + //rate timeref is E1 = U1 + offset += ubloxAddValSet(&tx_buffer, CFG_RATE_TIMEREF, payload, 12); + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, false); + } else { + tx_buffer.payload.cfg_rate.measRate = measRateMilliseconds; + tx_buffer.payload.cfg_rate.navRate = navRate; + tx_buffer.payload.cfg_rate.timeRef = timeRef; + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubxCfgRate_t), true); + } } static void ubloxSetSbas(void) { ubxMessage_t tx_buffer; - //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled - tx_buffer.payload.cfg_sbas.mode = UBLOX_MODE_TEST; - if (gpsConfig()->sbasMode != SBAS_NONE) { - tx_buffer.payload.cfg_sbas.mode |= UBLOX_MODE_ENABLED; - } + if (gpsData.ubloxM9orAbove) { + uint8_t payload[8]; + payload[0] = gpsConfig()->sbasMode != SBAS_NONE; - //NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled - tx_buffer.payload.cfg_sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR; - if (gpsConfig()->sbas_integrity) { - tx_buffer.payload.cfg_sbas.usage |= UBLOX_USAGE_INTEGRITY; - } + size_t offset = ubloxValSet(&tx_buffer, CFG_SBAS_USE_TESTMODE, payload, UBX_VAL_LAYER_RAM); - tx_buffer.payload.cfg_sbas.maxSBAS = 3; - tx_buffer.payload.cfg_sbas.scanmode2 = 0; - switch (gpsConfig()->sbasMode) { - case SBAS_AUTO: - tx_buffer.payload.cfg_sbas.scanmode1 = 0; - break; - case SBAS_EGNOS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136 - break; - case SBAS_WAAS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138 - break; - case SBAS_MSAS: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137 - break; - case SBAS_GAGAN: - tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132 - break; - default: - tx_buffer.payload.cfg_sbas.scanmode1 = 0; - break; + payload[0] = 1; + offset += ubloxAddValSet(&tx_buffer, CFG_SBAS_USE_RANGING, payload, offset); + offset += ubloxAddValSet(&tx_buffer, CFG_SBAS_USE_DIFFCORR, payload, offset); + + if (gpsConfig()->sbas_integrity) { + offset += ubloxAddValSet(&tx_buffer, CFG_SBAS_USE_INTEGRITY, payload, offset); + } + + uint64_t mask = SBAS_SEARCH_ALL; + switch (gpsConfig()->sbasMode) { + case SBAS_EGNOS: + mask = SBAS_SEARCH_PRN(123) | SBAS_SEARCH_PRN(126) | SBAS_SEARCH_PRN(136); + break; + case SBAS_WAAS: + mask = SBAS_SEARCH_PRN(131) | SBAS_SEARCH_PRN(133) | SBAS_SEARCH_PRN(135) | SBAS_SEARCH_PRN(138); + break; + case SBAS_MSAS: + mask = SBAS_SEARCH_PRN(129) | SBAS_SEARCH_PRN(137); + break; + case SBAS_GAGAN: + mask = SBAS_SEARCH_PRN(127) | SBAS_SEARCH_PRN(128) | SBAS_SEARCH_PRN(132); + break; + case SBAS_AUTO: + default: + break; + } + + payload[0] = (uint8_t)(mask >> (8 * 0)); + payload[1] = (uint8_t)(mask >> (8 * 1)); + payload[2] = (uint8_t)(mask >> (8 * 2)); + payload[3] = (uint8_t)(mask >> (8 * 3)); + payload[4] = (uint8_t)(mask >> (8 * 4)); + payload[5] = (uint8_t)(mask >> (8 * 5)); + payload[6] = (uint8_t)(mask >> (8 * 6)); + payload[7] = (uint8_t)(mask >> (8 * 7)); + + offset += ubloxAddValSet(&tx_buffer, CFG_SBAS_PRNSCANMASK, payload, offset); + + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_VALSET, offsetof(ubxCfgValSet_t, cfgData) + offset, true); + } else { + //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled + tx_buffer.payload.cfg_sbas.mode = UBLOX_MODE_TEST; + if (gpsConfig()->sbasMode != SBAS_NONE) { + tx_buffer.payload.cfg_sbas.mode |= UBLOX_MODE_ENABLED; + } + + // NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled + tx_buffer.payload.cfg_sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR; + if (gpsConfig()->sbas_integrity) { + tx_buffer.payload.cfg_sbas.usage |= UBLOX_USAGE_INTEGRITY; + } + + tx_buffer.payload.cfg_sbas.maxSBAS = 3; + tx_buffer.payload.cfg_sbas.scanmode2 = 0; + switch (gpsConfig()->sbasMode) { + case SBAS_AUTO: + tx_buffer.payload.cfg_sbas.scanmode1 = 0; + break; + case SBAS_EGNOS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136 + break; + case SBAS_WAAS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138 + break; + case SBAS_MSAS: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137 + break; + case SBAS_GAGAN: + tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132 + break; + default: + tx_buffer.payload.cfg_sbas.scanmode1 = 0; + break; + } + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubxCfgSbas_t), false); } - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubxCfgSbas_t)); } -void gpsInitUblox(void) +void setSatInfoMessageRate(uint8_t divisor) { - uint32_t now; - // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate + // enable satInfoMessage at 1:5 of the nav rate if configurator is connected + divisor = (isConfiguratorConnected()) ? 5 : 0; + if (gpsData.ubloxM9orAbove) { + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_UART1, divisor); + } else if (gpsData.ubloxM8orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SAT, divisor); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SVINFO, divisor); + } +} - // Wait until GPS transmit buffer is empty - if (!isSerialTransmitBufferEmpty(gpsPort)) - return; +#endif // USE_GPS_UBLOX - switch (gpsData.state) { - case GPS_STATE_INITIALIZING: - now = millis(); - if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY) - return; - - if (gpsData.state_position < GPS_INIT_ENTRIES) { - // try different speed to INIT - baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex; - - gpsData.state_ts = now; - - if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) { - // change the rate if needed and wait a little - serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]); -#if DEBUG_SERIAL_BAUD - debug[0] = baudRates[newBaudRateIndex] / 100; +#ifdef USE_GPS_NMEA +void gpsConfigureNmea(void) +{ + static bool atgmRestartDone = false; +#if !defined(GPS_NMEA_TX_ONLY) #endif - return; + DEBUG_SET(DEBUG_GPS_CONNECTION, 4, (gpsData.state * 100 + gpsData.state_position)); + switch (gpsData.state) { + case GPS_STATE_DETECT_BAUD: +#if !defined(GPS_NMEA_TX_ONLY) + if (cmp32(gpsData.now, gpsData.state_ts) < 1000) { + return; + } + gpsData.state_ts = gpsData.now; + switch (gpsData.state_position) { + case 0: // first run after bootup + serialSetBaudRate(gpsPort, 4800); + gpsData.state_position++; + break; + case 1: // second run + // print the init string for the baudrate we want to be at + serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n"); + gpsData.state_position++; + break; + default: + // we're now (hopefully) at the correct rate, next state should switch to it + // TO DO: Check that this actually works, it seems broken at present + gpsData.state_position = 0; + gpsSetState(GPS_STATE_CHANGE_BAUD); + break; + } + break; +#endif + case GPS_STATE_CHANGE_BAUD: +#if !defined(GPS_NMEA_TX_ONLY) + // wait a short time between sending commands + // note that no commands are sent to request the packets we need + if (cmp32(gpsData.now, gpsData.state_ts) < 500) { + return; + } + gpsData.state_ts = gpsData.now; + + if (gpsData.state_position < 1) { // first run after boot up + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); + gpsData.state_position++; + } else if (gpsData.state_position < 2) { + // *** this message also appears to fail ***// + // NMEA reports back at whatever speed the module is configured to send at, not 5Hz + serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n"); // set GGA rate to 5Hz + gpsData.state_position++; + } else if (gpsData.state_position < 3) { + // special initialization for NMEA ATGM336 and similar GPS recivers - should be done only once + if (!atgmRestartDone) { + atgmRestartDone = true; + serialPrint(gpsPort, "$PCAS02,100*1E\r\n"); // 10Hz refresh rate + serialPrint(gpsPort, "$PCAS10,0*1C\r\n"); // hot restart + } + gpsData.state_position++; + } else if (gpsData.state_position < 4) { +#ifdef USE_GPS_UBLOX + // try to disable UBlox NAV-PVT and NAV-SAT, using ValSet, to which M9 or above should respond + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_PVT_UART1, 0); + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_UART1, 0); +#endif + gpsData.state_position++; + } else if (gpsData.state_position < 5) { +#ifdef USE_GPS_UBLOX + // try to disable UBX NAV-PVT, NAV-SAT and NAV_SVINFO, to which M7 and M8 should respond + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_PVT, 0); + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SAT, 0); + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SVINFO, 0); +#endif + gpsData.state_position++; + } else if (gpsData.state_position < 6) { + if (!(isConfiguratorConnected())) { + // disable GSV MESSAGES + // *** THIS COMMAND FAILS TO DISABLE GSV MESSAGES WHEN SENT **** + // bug?? why?? + serialPrint(gpsPort, "$PSRF103,03,00,00,01*27\r\n"); // disable GSV (Sat info) messages + // *** BUT THE UBLOX EQUIVALENT WORKS **** + // so I'll send both for now... + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSV, 0); + } + gpsData.state_position++; + } else if (gpsData.state_position < 7) { + // NMEA custom commands after ATGM336 initialization + static int commandOffset = 0; + const char *commands = gpsConfig()->nmeaCustomCommands; + const char *cmd = commands + commandOffset; + + // skip leading whitespaces and get first command length + int commandLen; + while (*cmd && (commandLen = strcspn(cmd, " \0")) == 0) { + cmd++; // skip separators } - // print our FIXED init string for the baudrate we want to be at - serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx); + if (*cmd) { + // Send the current command to the GPS + serialWriteBuf(gpsPort, (uint8_t *)cmd, commandLen); + serialWriteBuf(gpsPort, (uint8_t *)"\r\n", 2); - gpsData.state_position++; - } else { - // we're now (hopefully) at the correct rate, next state will switch to it - gpsSetState(GPS_STATE_CHANGE_BAUD); + // Move to the next command + cmd += commandLen; + } + + // skip trailing whitespaces + while (*cmd && strcspn(cmd, " \0") == 0) cmd++; + + if (*cmd) { + // more commands to send + commandOffset = cmd - commands; + } else { + gpsData.state_position++; + commandOffset = 0; + } + } else +#else + { + serialSetBaudRate (gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); } +#endif + gpsSetState(GPS_STATE_RECEIVING_DATA); + break; + } +} +#endif // USE_GPS_NMEA + +#ifdef USE_GPS_UBLOX + +void gpsConfigureUblox(void) +{ + + // Wait until GPS transmit buffer is empty + if (!isSerialTransmitBufferEmpty(gpsPort)) { + return; + } + + switch (gpsData.state) { + case GPS_STATE_DETECT_BAUD: + + DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex] / 100); + + // check to see if there has been a response to the version command + // initially the FC will be at the user-configured baud rate. + if (gpsData.platformVersion > UBX_VERSION_UNDEF) { + // set the GPS module's serial port to the user's intended baud rate + serialPrint(gpsPort, gpsInitData[gpsData.userBaudRateIndex].ubx); + // use this baud rate for re-connections + gpsData.tempBaudRateIndex = gpsData.userBaudRateIndex; + // we're done here, let's move the the next state + gpsSetState(GPS_STATE_CHANGE_BAUD); + return; + } + + // Send MON-VER messages at GPS_CONFIG_BAUD_CHANGE_INTERVAL for GPS_BAUDRATE_TEST_COUNT times + static bool messageSent = false; + static uint8_t messageCounter = 0; + DEBUG_SET(DEBUG_GPS_CONNECTION, 2, initBaudRateCycleCount * 100 + messageCounter); + + if (messageCounter < GPS_BAUDRATE_TEST_COUNT) { + if (!messageSent) { + gpsData.platformVersion = UBX_VERSION_UNDEF; + ubloxSendClassMessage(CLASS_MON, MSG_MON_VER, 0); + gpsData.ackState = UBLOX_ACK_IDLE; // ignore ACK for this message + messageSent = true; + } + if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) { + gpsData.state_ts = gpsData.now; + messageSent = false; + ++ messageCounter; + } + return; + } + messageCounter = 0; + gpsData.state_ts = gpsData.now; + + // failed to connect at that rate after five attempts + // try other GPS baudrates, starting at 9600 and moving up + if (gpsData.tempBaudRateIndex == 0) { + gpsData.tempBaudRateIndex = GPS_BAUDRATE_MAX; // slowest baud rate 9600 + } else { + gpsData.tempBaudRateIndex--; + } + // set the FC baud rate to the new temp baud rate + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex]); + initBaudRateCycleCount++; + break; case GPS_STATE_CHANGE_BAUD: - serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]); -#if DEBUG_SERIAL_BAUD - debug[0] = baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex] / 100; -#endif + // give time for the GPS module's serial port to settle + // very important for M8 to give the module a lot of time before sending commands + // M10 only need about 200ms but M8 and below sometimes need as long as 1000ms + if (cmp32(gpsData.now, gpsData.state_ts) < (3 * GPS_CONFIG_BAUD_CHANGE_INTERVAL)) { + return; + } + // set the FC's serial port to the configured rate + serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex]); + DEBUG_SET(DEBUG_GPS_CONNECTION, 3, baudRates[gpsInitData[gpsData.userBaudRateIndex].baudrateIndex] / 100); + // then start sending configuration settings gpsSetState(GPS_STATE_CONFIGURE); break; @@ -686,139 +1160,213 @@ void gpsInitUblox(void) break; } + // allow 3s for the Configurator connection to stabilise, to get the correct answer when we test the state of the connection. + // 3s is an arbitrary time at present, maybe should be defined or user adjustable. + // This delays the appearance of GPS data in OSD when not connected to configurator by 3s. + // Note that state_ts is set to millis() on the previous gpsSetState() command + if (!isConfiguratorConnected()) { + if (cmp32(gpsData.now, gpsData.state_ts) < 3000) { + return; + } + } + if (gpsData.ackState == UBLOX_ACK_IDLE) { + + // short delay before between commands, including the first command + static uint32_t last_state_position_time = 0; + if (last_state_position_time == 0) { + last_state_position_time = gpsData.now; + } + if (cmp32(gpsData.now, last_state_position_time) < GPS_CONFIG_CHANGE_INTERVAL) { + return; + } + last_state_position_time = gpsData.now; + switch (gpsData.state_position) { - case UBLOX_INITIALIZE: - gpsData.ubloxUsePVT = true; - gpsData.ubloxUseSAT = true; - ubloxSendNAV5Message(gpsConfig()->gps_ublox_acquire_model); + // if a UBX command is sent, ack is supposed to give position++ once the reply happens + case UBLOX_DETECT_UNIT: // 400 in debug + if (gpsData.platformVersion == UBX_VERSION_UNDEF) { + ubloxSendClassMessage(CLASS_MON, MSG_MON_VER, 0); + } else { + gpsData.state_position++; + } + break; + case UBLOX_SLOW_NAV_RATE: // 401 in debug + ubloxSetNavRate(1, 1, 1); // throttle nav data rate to one per second, until configured + break; + case UBLOX_MSG_DISABLE_NMEA: + if (gpsData.ubloxM9orAbove) { + ubloxDisableNMEAValSet(); + gpsData.state_position = UBLOX_MSG_RMC; // skip UBX NMEA entries - goes to RMC plus one, or ACQUIRE_MODEL + } else { + gpsData.state_position++; + } break; case UBLOX_MSG_VGS: //Disable NMEA Messages - ubloxSetMessageRate(0xF0, 0x05, 0); // VGS: Course over ground and Ground speed + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_VTG, 0); // VGS: Course over ground and Ground speed break; case UBLOX_MSG_GSV: - ubloxSetMessageRate(0xF0, 0x03, 0); // GSV: GNSS Satellites in View + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSV, 0); // GSV: GNSS Satellites in View break; case UBLOX_MSG_GLL: - ubloxSetMessageRate(0xF0, 0x01, 0); // GLL: Latitude and longitude, with time of position fix and status + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GLL, 0); // GLL: Latitude and longitude, with time of position fix and status break; case UBLOX_MSG_GGA: - ubloxSetMessageRate(0xF0, 0x00, 0); // GGA: Global positioning system fix data + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GGA, 0); // GGA: Global positioning system fix data break; case UBLOX_MSG_GSA: - ubloxSetMessageRate(0xF0, 0x02, 0); // GSA: GNSS DOP and Active Satellites + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_GSA, 0); // GSA: GNSS DOP and Active Satellites break; case UBLOX_MSG_RMC: - ubloxSetMessageRate(0xF0, 0x04, 0); // RMC: Recommended Minimum data + ubloxSetMessageRate(CLASS_NMEA_STD, MSG_NMEA_RMC, 0); // RMC: Recommended Minimum data break; - case UBLOX_MSG_RATE: //Enable UBLOX Messages - if (gpsData.ubloxUsePVT) { - ubloxSetMessageRate(CLASS_NAV, MSG_PVT, 1); // set PVT MSG rate - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SOL, 1); // set SOL MSG rate - } - break; - case UBLOX_MSG_POSLLH: - if (gpsData.ubloxUsePVT) { - gpsData.state_position++; - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_POSLLH, 1); // set POSLLH MSG rate - } - break; - case UBLOX_MSG_STATUS: - if (gpsData.ubloxUsePVT) { - gpsData.state_position++; - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_STATUS, 1); // set STATUS MSG rate - } - break; - case UBLOX_MSG_VELNED: - if (gpsData.ubloxUsePVT) { - gpsData.state_position++; - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_VELNED, 1); // set VELNED MSG rate - } - break; - case UBLOX_MSG_SATINFO: - if (gpsData.ubloxUseSAT) { - ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles) - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles) - } - break; - case UBLOX_MSG_DOP: - ubloxSetMessageRate(CLASS_NAV, MSG_DOP, 1); // set DOP MSG rate - break; - case UBLOX_SET_NAV_RATE: - ubloxSetNavRate(gpsConfig()->gps_update_rate_hz, 1, 1); + case UBLOX_ACQUIRE_MODEL: + ubloxSendNAV5Message(gpsConfig()->gps_ublox_acquire_model); break; +// *** temporarily disabled +// case UBLOX_CFG_ANA: +// i f (gpsData.ubloxM7orAbove) { // NavX5 support existed in M5 - in theory we could remove that check +// ubloxSendNavX5Message(); +// } else { +// gpsData.state_position++; +// } +// break; case UBLOX_SET_SBAS: ubloxSetSbas(); break; case UBLOX_SET_PMS: - ubloxSendPowerMode(); + if (gpsData.ubloxM8orAbove) { + ubloxSendPowerMode(); + } else { + gpsData.state_position++; + } + break; + case UBLOX_MSG_NAV_PVT: //Enable NAV-PVT Messages + if (gpsData.ubloxM9orAbove) { + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_PVT_UART1, 1); + } else if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_PVT, 1); + } else { + gpsData.state_position++; + } + break; + // if NAV-PVT is enabled, we don't need the older nav messages + case UBLOX_MSG_SOL: + if (gpsData.ubloxM9orAbove) { + // SOL is deprecated above M8 + gpsData.state_position++; + } else if (gpsData.ubloxM7orAbove) { + // use NAV-PVT, so don't use NAV-SOL + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 0); + } else { + // Only use NAV-SOL below M7 + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_SOL, 1); + } + break; + case UBLOX_MSG_POSLLH: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_POSLLH, 1); + } + break; + case UBLOX_MSG_STATUS: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_STATUS, 1); + } + break; + case UBLOX_MSG_VELNED: + if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_VELNED, 1); + } + break; + case UBLOX_MSG_DOP: + // nav-pvt has what we need and is available M7 and above + if (gpsData.ubloxM9orAbove) { + ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_DOP_UART1, 0); + } else if (gpsData.ubloxM7orAbove) { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 0); + } else { + ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 1); + } + break; + case UBLOX_SAT_INFO: + // enable by default, turned off when armed and receiving data to reduce in-flight traffic + setSatInfoMessageRate(5); + break; + case UBLOX_SET_NAV_RATE: + // set the nav solution rate to the user's configured update rate + gpsData.updateRateHz = gpsConfig()->gps_update_rate_hz; + ubloxSetNavRate(gpsData.updateRateHz, 1, 1); break; case UBLOX_MSG_CFG_GNSS: if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) { - ubloxSendPollMessage(MSG_CFG_GNSS); + ubloxSendPollMessage(MSG_CFG_GNSS); // poll messages wait for ACK } else { - gpsSetState(GPS_STATE_RECEIVING_DATA); + gpsData.state_position++; } break; + case UBLOX_CONFIG_COMPLETE: + gpsSetState(GPS_STATE_RECEIVING_DATA); + break; + // TO DO: (separate PR) add steps that remove I2C or SPI data on ValSet aware units, eg + // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_I2C, 0); + // ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_SPI, 0); default: break; } - } + } + // check the ackState after changing CONFIG state, or every iteration while not UBLOX_ACK_IDLE switch (gpsData.ackState) { case UBLOX_ACK_IDLE: break; case UBLOX_ACK_WAITING: - if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) { - gpsSetState(GPS_STATE_LOST_COMMUNICATION); + if (cmp32(gpsData.now, gpsData.lastMessageSent) > UBLOX_ACK_TIMEOUT_MS){ + // give up, treat it like receiving ack + gpsData.ackState = UBLOX_ACK_GOT_ACK; } break; case UBLOX_ACK_GOT_ACK: - if (gpsData.state_position == UBLOX_MSG_CFG_GNSS) { - // ublox should be initialised, try receiving - gpsSetState(GPS_STATE_RECEIVING_DATA); + // move forward one position, and clear the ack state + gpsData.state_position++; + gpsData.ackState = UBLOX_ACK_IDLE; + break; + case UBLOX_ACK_GOT_NACK: + // this is the tricky bit + // and we absolutely must get the unit type right + if (gpsData.state_position == UBLOX_DETECT_UNIT) { + gpsSetState(GPS_STATE_CONFIGURE); + gpsData.ackState = UBLOX_ACK_IDLE; } else { + // otherwise, for testing: just ignore nacks gpsData.state_position++; gpsData.ackState = UBLOX_ACK_IDLE; } break; - case UBLOX_ACK_GOT_NACK: - if (gpsData.state_position == UBLOX_MSG_RATE) { // If we were asking for NAV-PVT... - gpsData.ubloxUsePVT = false; // ...retry asking for NAV-SOL - gpsData.ackState = UBLOX_ACK_IDLE; - } else { - if (gpsData.state_position == UBLOX_MSG_SATINFO) { // If we were asking for NAV-SAT... - gpsData.ubloxUseSAT = false; // ...retry asking for NAV-SVINFO - gpsData.ackState = UBLOX_ACK_IDLE; - } else { - gpsSetState(GPS_STATE_CONFIGURE); - } - } + default: break; } - - break; } } #endif // USE_GPS_UBLOX -void gpsInitHardware(void) +void gpsConfigureHardware(void) { switch (gpsConfig()->provider) { case GPS_NMEA: #ifdef USE_GPS_NMEA - gpsInitNmea(); + gpsConfigureNmea(); #endif break; case GPS_UBLOX: #ifdef USE_GPS_UBLOX - gpsInitUblox(); + gpsConfigureUblox(); #endif break; default: @@ -837,112 +1385,91 @@ static void updateGpsIndicator(timeUs_t currentTimeUs) void gpsUpdate(timeUs_t currentTimeUs) { - static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT]; - timeUs_t executeTimeUs; + static timeDelta_t gpsStateDurationFractionUs[GPS_STATE_COUNT]; + timeDelta_t executeTimeUs; gpsState_e gpsCurrentState = gpsData.state; + gpsData.now = millis(); - // read out available GPS bytes if (gpsPort) { + DEBUG_SET(DEBUG_GPS_CONNECTION, 7, serialRxBytesWaiting(gpsPort)); + static uint8_t wait = 0; + static bool isFast = false; while (serialRxBytesWaiting(gpsPort)) { - if (cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX) { - // Wait 1ms and come back + wait = 0; + if (!isFast) { rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST)); - return; + isFast = true; } + if (cmpTimeUs(micros(), currentTimeUs) > GPS_RECV_TIME_MAX) { + break; + } + // Add every byte to _buffer, when enough bytes are received, convert data to values gpsNewData(serialRead(gpsPort)); } - // Restore default task rate - rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE)); - } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP + if (wait < 1) { + wait++; + } else if (wait == 1) { + wait++; + // wait one iteration be sure the buffer is empty, then reset to the slower task interval + isFast = false; + rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE)); + } + } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP gpsSetState(GPS_STATE_RECEIVING_DATA); onGpsNewData(); GPS_update &= ~GPS_MSP_UPDATE; } -#if DEBUG_UBLOX_INIT - debug[0] = gpsData.state; - debug[1] = gpsData.state_position; - debug[2] = gpsData.ackState; -#endif - switch (gpsData.state) { case GPS_STATE_UNKNOWN: case GPS_STATE_INITIALIZED: break; - case GPS_STATE_INITIALIZING: + case GPS_STATE_DETECT_BAUD: case GPS_STATE_CHANGE_BAUD: case GPS_STATE_CONFIGURE: - gpsInitHardware(); + gpsConfigureHardware(); break; case GPS_STATE_LOST_COMMUNICATION: gpsData.timeouts++; - if (gpsConfig()->autoBaud) { - // try another rate - gpsData.baudrateIndex++; - gpsData.baudrateIndex %= GPS_INIT_ENTRIES; - } + // previously we would attempt a different baud rate here if gps auto-baud was enabled. that code has been removed. gpsSol.numSat = 0; DISABLE_STATE(GPS_FIX); - gpsSetState(GPS_STATE_INITIALIZING); + gpsSetState(GPS_STATE_DETECT_BAUD); break; case GPS_STATE_RECEIVING_DATA: - // check for no data/gps timeout/cable disconnection etc - if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { - gpsSetState(GPS_STATE_LOST_COMMUNICATION); #ifdef USE_GPS_UBLOX - } else { - if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled - switch (gpsData.state_position) { - case UBLOX_INITIALIZE: - if (!isConfiguratorConnected()) { - if (gpsData.ubloxUseSAT) { - ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG - } - gpsData.state_position = UBLOX_MSG_VGS; - } - break; - case UBLOX_MSG_VGS: - if (STATE(GPS_FIX)) { - ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model); - gpsData.state_position = UBLOX_MSG_GSV; - } - if (isConfiguratorConnected()) { - gpsData.state_position = UBLOX_MSG_GSV; - } - break; - case UBLOX_MSG_GSV: - if (isConfiguratorConnected()) { - if (gpsData.ubloxUseSAT) { - ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles) - } else { - ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles) - } - gpsData.state_position = UBLOX_INITIALIZE; - } - break; - } + if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { + // when we are connected up, and get a 3D fix, enable the 'flight' fix model + if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) { + gpsData.ubloxUsingFlightModel = true; + ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model); } -#endif //USE_GPS_UBLOX + } +#endif + DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval + // check for no data/gps timeout/cable disconnection etc + if (cmp32(gpsData.now, gpsData.lastNavMessage) > GPS_TIMEOUT_MS) { + gpsSetState(GPS_STATE_LOST_COMMUNICATION); } break; } + DEBUG_SET(DEBUG_GPS_CONNECTION, 4, (gpsData.state * 100 + gpsData.state_position)); + DEBUG_SET(DEBUG_GPS_CONNECTION, 6, gpsData.ackState); + if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); } - if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) { - // clear the home fix icon between arms if the user configuration is to reset home point between arms - DISABLE_STATE(GPS_FIX_HOME); - } - static bool hasBeeped = false; if (!ARMING_FLAG(ARMED)) { + if (!gpsConfig()->gps_set_home_point_once) { + // clear the home fix icon between arms if the user configuration is to reset home point between arms + DISABLE_STATE(GPS_FIX_HOME); + } // while disarmed, beep when requirements for a home fix are met // ?? should we also beep if home fix requirements first appear after arming? if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) { @@ -957,34 +1484,48 @@ void gpsUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_GPS_DOP, 3, gpsSol.dop.vdop); executeTimeUs = micros() - currentTimeUs; - if (executeTimeUs > gpsStateDurationUs[gpsCurrentState]) { - gpsStateDurationUs[gpsCurrentState] = executeTimeUs; + if (executeTimeUs > (gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT)) { + gpsStateDurationFractionUs[gpsCurrentState] += (2 << GPS_TASK_DECAY_SHIFT); + } else { + // Slowly decay the max time + gpsStateDurationFractionUs[gpsCurrentState]--; } - schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state]); + schedulerSetNextStateTime(gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT); + + DEBUG_SET(DEBUG_GPS_CONNECTION, 5, executeTimeUs); +// keeping temporarily, to be used when debugging the sheduler stuff +// DEBUG_SET(DEBUG_GPS_CONNECTION, 6, (gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT)); } static void gpsNewData(uint16_t c) { - if (!gpsNewFrame(c)) { + DEBUG_SET(DEBUG_GPS_CONNECTION, 1, gpsSol.navIntervalMs); + if (!gpsNewFrame(c)) { + // no new nav solution data return; } - if (gpsData.state == GPS_STATE_RECEIVING_DATA) { - // new data received and parsed, we're in business - gpsData.lastLastMessage = gpsData.lastMessage; - gpsData.lastMessage = millis(); + DEBUG_SET(DEBUG_GPS_CONNECTION, 3, gpsData.now - gpsData.lastNavMessage); // interval since last Nav data was received + gpsData.lastNavMessage = gpsData.now; sensorsSet(SENSOR_GPS); + // use the baud rate debug once receiving data } - GPS_update ^= GPS_DIRECT_TICK; - -#if DEBUG_UBLOX_INIT - debug[3] = GPS_update; -#endif - onGpsNewData(); } +#ifdef USE_GPS_UBLOX +ubloxVersion_e ubloxParseVersion(const uint32_t version) { + for (size_t i = 0; i < ARRAYLEN(ubloxVersionMap); ++i) { + if (version == ubloxVersionMap[i].hw) { + return (ubloxVersion_e) i; + } + } + + return UBX_VERSION_UNDEF;// (ubloxVersion_e) version; +} +#endif + bool gpsNewFrame(uint8_t c) { switch (gpsConfig()->provider) { @@ -1126,8 +1667,9 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin case FRAME_GGA: //************* GPGGA FRAME parsing switch (idx) { - // case 1: // Time information - // break; + case 1: + data->time = ((uint8_t)(str[5] - '0') * 10 + (uint8_t)(str[7] - '0')) * 100; + break; case 2: data->latitude = GPS_coord_to_degrees(str); break; @@ -1212,7 +1754,9 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin break; } - GPS_svInfoReceivedCount++; +#ifdef USE_DASHBOARD + dashboardGpsNavSvInfoRcvCount++; +#endif break; case FRAME_GSA: @@ -1233,28 +1777,39 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin static bool writeGpsSolutionNmea(gpsSolutionData_t *sol, const gpsDataNmea_t *data, uint8_t gpsFrame) { + int navDeltaTimeMs = 100; + const uint32_t msInTenSeconds = 10000; switch (gpsFrame) { case FRAME_GGA: - *gpsPacketLogChar = LOG_NMEA_GGA; +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GGA; +#endif if (STATE(GPS_FIX)) { sol->llh.lat = data->latitude; sol->llh.lon = data->longitude; sol->numSat = data->numSat; sol->llh.altCm = data->altitudeCm; } + navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds; + gpsData.lastNavSolTs = data->time; + sol->navIntervalMs = constrain(navDeltaTimeMs, 100, 2500); // return only one true statement to trigger one "newGpsDataReady" flag per GPS loop return true; case FRAME_GSA: - *gpsPacketLogChar = LOG_NMEA_GSA; +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_GSA; +#endif sol->dop.pdop = data->pdop; sol->dop.hdop = data->hdop; sol->dop.vdop = data->vdop; return false; case FRAME_RMC: - *gpsPacketLogChar = LOG_NMEA_RMC; +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_NMEA_RMC; +#endif sol->groundSpeed = data->speed; sol->groundCourse = data->ground_course; #ifdef USE_RTC_TIME @@ -1284,7 +1839,7 @@ static bool gpsNewFrameNMEA(char c) static char string[15]; static uint8_t param = 0, offset = 0, parity = 0; static uint8_t checksum_param, gps_frame = NO_FRAME; - bool isFrameOk = false; + bool receivedNavMessage = false; switch (c) { @@ -1324,15 +1879,22 @@ static bool gpsNewFrameNMEA(char c) case '\r': case '\n': if (checksum_param) { //parity checksum +#ifdef USE_DASHBOARD shiftPacketLog(); +#endif 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) { - *gpsPacketLogChar = LOG_IGNORED; - GPS_packetCount++; - isFrameOk = writeGpsSolutionNmea(&gpsSol, &gps_msg, gps_frame); // // write gps_msg into gpsSol - } else { - *gpsPacketLogChar = LOG_ERROR; +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_IGNORED; + dashboardGpsPacketCount++; +#endif + receivedNavMessage = writeGpsSolutionNmea(&gpsSol, &gps_msg, gps_frame); // // write gps_msg into gpsSol } +#ifdef USE_DASHBOARD + else { + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_ERROR; + } +#endif } checksum_param = 0; break; @@ -1345,7 +1907,7 @@ static bool gpsNewFrameNMEA(char c) break; } - return isFrameOk; + return receivedNavMessage; } #endif // USE_GPS_NMEA @@ -1382,7 +1944,7 @@ typedef struct ubxNavDop_s { uint16_t edop; // Easting DOP } ubxNavDop_t; -typedef struct ubxNavSolution_s { +typedef struct ubxNavSol_s { uint32_t time; int32_t time_nsec; int16_t week; @@ -1400,7 +1962,7 @@ typedef struct ubxNavSolution_s { uint8_t res; uint8_t satellites; uint32_t res2; -} ubxNavSolution_t; +} ubxNavSol_t; typedef struct ubxNavPvt_s { uint32_t time; @@ -1476,7 +2038,7 @@ typedef struct ubxNavSvinfo_s { uint8_t numCh; // Number of channels uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 uint16_t reserved2; // Reserved - ubxNavSvinfoChannel_t channel[GPS_SV_MAXSATS_M8N]; // 32 satellites * 12 byte + ubxNavSvinfoChannel_t channel[GPS_SV_MAXSATS_M8N]; // 32 satellites * 12 bytes } ubxNavSvinfo_t; typedef struct ubxNavSat_s { @@ -1484,7 +2046,7 @@ typedef struct ubxNavSat_s { uint8_t version; uint8_t numSvs; uint8_t reserved0[2]; - ubxNavSatSv_t svs[GPS_SV_MAXSATS_M9N]; + ubxNavSatSv_t svs[GPS_SV_MAXSATS_M8N]; } ubxNavSat_t; typedef struct ubxAck_s { @@ -1512,185 +2074,244 @@ typedef enum { NAV_VALID_TIME = 2 } ubxNavPvtValid_e; -// Packet checksum accumulators -static uint8_t _ck_a; -static uint8_t _ck_b; +// Do we have a new valid fix? +static bool ubxHaveNewValidFix = false; -// State machine state -static bool _skip_packet; -static uint8_t _step = 0; -static uint8_t _msg_id; -static uint16_t _payload_length; -static uint16_t _payload_counter; +// Do we have new position information? +static bool ubxHaveNewPosition = false; -static bool next_fix = false; -static uint8_t _class; +// Do we have new speed information? +static bool ubxHaveNewSpeed = false; -// do we have new position information? -static bool _new_position; +// From the UBX protocol docs, the largest payload we receive is NAV-SAT, which +// is calculated as 8 + 12*numCh. Max reported sats can be up to 56. +// We're using the max for M8 (32) for our sizing, since Configurator only +// supports a max of 32 sats and we want to limit the payload buffer space used. +#define UBLOX_PAYLOAD_SIZE (8 + 12 * GPS_SV_MAXSATS_M8N) +#define UBLOX_MAX_PAYLOAD_SANITY_SIZE 776 // Any returned payload length greater than a 64 sat NAV-SAT is considered unreasonable, and probably corrupted data. -// do we have new speed information? -static bool _new_speed; - -// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. -//15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status' -//15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position' -//15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84' -//15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status' -//15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status' -//15:17:55 R -> UBX 03-09, Size 208, 'Unknown' -//15:17:55 R -> UBX 03-10, Size 336, 'Unknown' -//15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution' -//15:17:55 R -> UBX NAV, Size 100, 'Navigation' -//15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information' - -// from the UBlox9 document, the largest payout we receive is the NAV-SAT and the payload size -// is calculated as 8 + 12*numCh. numCh in the case of a M9N is 42. -#define UBLOX_PAYLOAD_SIZE (8 + 12 * 42) - - -// Receive buffer +// Received message frame fields. +// - Preamble sync character 1 & 2 are not saved, only detected for parsing. +// - Message class & message ID indicate the type of message receieved. +static uint8_t ubxRcvMsgClass; +static uint8_t ubxRcvMsgID; +// - Payload length assembled from the length LSB & MSB bytes. +static uint16_t ubxRcvMsgPayloadLength; +// - Payload, each message type has its own payload field layout, represented by the elements of this union. +// Note that the size of the buffer is determined by the longest possible payload, currently UBX-NAV-SAT. +// See size define comments above. Warning, this is fragile! If another message type becomes the largest +// payload instead of UBX-NAV-SAT, UBLOX_PAYLOAD_SIZE above needs to be adjusted! static union { - ubxNavPosllh_t posllh; - ubxNavStatus_t status; - ubxNavDop_t dop; - ubxNavSolution_t solution; - ubxNavVelned_t velned; - ubxNavPvt_t pvt; - ubxNavSvinfo_t svinfo; - ubxNavSat_t sat; - ubxCfgGnss_t gnss; - ubxAck_t ack; - uint8_t bytes[UBLOX_PAYLOAD_SIZE]; -} _buffer; + ubxNavPosllh_t ubxNavPosllh; + ubxNavStatus_t ubxNavStatus; + ubxNavDop_t ubxNavDop; + ubxNavSol_t ubxNavSol; + ubxNavVelned_t ubxNavVelned; + ubxNavPvt_t ubxNavPvt; + ubxNavSvinfo_t ubxNavSvinfo; + ubxNavSat_t ubxNavSat; + ubxCfgGnss_t ubxCfgGnss; + ubxMonVer_t ubxMonVer; + ubxAck_t ubxAck; + uint8_t rawBytes[UBLOX_PAYLOAD_SIZE]; // Used for adding raw bytes to the payload. WARNING: This byte array must be as large as the largest payload for any message type above! +} ubxRcvMsgPayload; +// - Checksum A & B. Uses the 8-bit Fletcher algorithm (TCP standard RFC 1145). +static uint8_t ubxRcvMsgChecksumA; +static uint8_t ubxRcvMsgChecksumB; -void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) +// Message frame parsing state machine control. +typedef enum { + UBX_PARSE_PREAMBLE_SYNC_1, + UBX_PARSE_PREAMBLE_SYNC_2, + UBX_PARSE_MESSAGE_CLASS, + UBX_PARSE_MESSAGE_ID, + UBX_PARSE_PAYLOAD_LENGTH_LSB, + UBX_PARSE_PAYLOAD_LENGTH_MSB, + UBX_PARSE_PAYLOAD_CONTENT, + UBX_PARSE_CHECKSUM_A, + UBX_PARSE_CHECKSUM_B +} ubxFrameParseState_e; +static ubxFrameParseState_e ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; +static uint16_t ubxFrameParsePayloadCounter; + +static void calculateNavInterval (void) { - while (len--) { - *ck_a += *data; - *ck_b += *ck_a; - data++; - } + // calculate the interval between nav packets, handling iTow wraparound at the end of the week + const uint32_t weekDurationMs = 7 * 24 * 3600 * 1000; + const uint32_t navDeltaTimeMs = (weekDurationMs + gpsSol.time - gpsData.lastNavSolTs) % weekDurationMs; + gpsData.lastNavSolTs = gpsSol.time; + // constrain the interval between 50ms / 20hz or 2.5s, when we would get a connection failure anyway + gpsSol.navIntervalMs = constrain(navDeltaTimeMs, 50, 2500); } +// SCEDEBUG To help debug which message is slow to process +// static uint8_t lastUbxRcvMsgClass; +// static uint8_t lastUbxRcvMsgID; + +// Combines message class & ID for a single value to switch on. +#define CLSMSG(cls, msg) (((cls) << 8) | (msg)) + static bool UBLOX_parse_gps(void) { uint32_t i; - *gpsPacketLogChar = LOG_IGNORED; +// lastUbxRcvMsgClass = ubxRcvMsgClass; +// lastUbxRcvMsgID = ubxRcvMsgID; - switch (_msg_id) { - case MSG_POSLLH: - *gpsPacketLogChar = LOG_UBLOX_POSLLH; - //i2c_dataset.time = _buffer.posllh.time; - gpsSol.llh.lon = _buffer.posllh.longitude; - gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm - gpsSol.time = _buffer.posllh.time; - gpsSetFixState(next_fix); - _new_position = true; +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_IGNORED; +#endif + switch (CLSMSG(ubxRcvMsgClass, ubxRcvMsgID)) { + + case CLSMSG(CLASS_MON, MSG_MON_VER): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_MONVER; +#endif + gpsData.platformVersion = ubloxParseVersion(strtoul(ubxRcvMsgPayload.ubxMonVer.hwVersion, NULL, 16)); + gpsData.ubloxM7orAbove = gpsData.platformVersion >= UBX_VERSION_M7; + gpsData.ubloxM8orAbove = gpsData.platformVersion >= UBX_VERSION_M8; + gpsData.ubloxM9orAbove = gpsData.platformVersion >= UBX_VERSION_M9; break; - case MSG_STATUS: - *gpsPacketLogChar = LOG_UBLOX_STATUS; - next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); - if (!next_fix) + case CLSMSG(CLASS_NAV, MSG_NAV_POSLLH): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_POSLLH; +#endif + //i2c_dataset.time = _buffer.ubxNavPosllh.time; + gpsSol.llh.lon = ubxRcvMsgPayload.ubxNavPosllh.longitude; + gpsSol.llh.lat = ubxRcvMsgPayload.ubxNavPosllh.latitude; + gpsSol.llh.altCm = ubxRcvMsgPayload.ubxNavPosllh.altitudeMslMm / 10; //alt in cm + gpsSol.time = ubxRcvMsgPayload.ubxNavPosllh.time; + calculateNavInterval(); + gpsSetFixState(ubxHaveNewValidFix); + ubxHaveNewPosition = true; + break; + case CLSMSG(CLASS_NAV, MSG_NAV_STATUS): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_STATUS; +#endif + ubxHaveNewValidFix = (ubxRcvMsgPayload.ubxNavStatus.fix_status & NAV_STATUS_FIX_VALID) && (ubxRcvMsgPayload.ubxNavStatus.fix_type == FIX_3D); + if (!ubxHaveNewValidFix) DISABLE_STATE(GPS_FIX); break; - case MSG_DOP: - *gpsPacketLogChar = LOG_UBLOX_DOP; - gpsSol.dop.pdop = _buffer.dop.pdop; - gpsSol.dop.hdop = _buffer.dop.hdop; - gpsSol.dop.vdop = _buffer.dop.vdop; + case CLSMSG(CLASS_NAV, MSG_NAV_DOP): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_DOP; +#endif + gpsSol.dop.pdop = ubxRcvMsgPayload.ubxNavDop.pdop; + gpsSol.dop.hdop = ubxRcvMsgPayload.ubxNavDop.hdop; + gpsSol.dop.vdop = ubxRcvMsgPayload.ubxNavDop.vdop; break; - case MSG_SOL: - *gpsPacketLogChar = LOG_UBLOX_SOL; - next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); - if (!next_fix) + case CLSMSG(CLASS_NAV, MSG_NAV_SOL): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SOL; +#endif + ubxHaveNewValidFix = (ubxRcvMsgPayload.ubxNavSol.fix_status & NAV_STATUS_FIX_VALID) && (ubxRcvMsgPayload.ubxNavSol.fix_type == FIX_3D); + if (!ubxHaveNewValidFix) DISABLE_STATE(GPS_FIX); - gpsSol.numSat = _buffer.solution.satellites; + gpsSol.numSat = ubxRcvMsgPayload.ubxNavSol.satellites; #ifdef USE_RTC_TIME //set clock, when gps time is available - if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) { + if (!rtcHasTime() && (ubxRcvMsgPayload.ubxNavSol.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (ubxRcvMsgPayload.ubxNavSol.fix_status & NAV_STATUS_TIME_WEEK_VALID)) { //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds - rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000; + rtcTime_t temp_time = (((int64_t) ubxRcvMsgPayload.ubxNavSol.week) * 7 * 24 * 60 * 60 * 1000) + ubxRcvMsgPayload.ubxNavSol.time + (ubxRcvMsgPayload.ubxNavSol.time_nsec / 1000000) + 315964800000LL - 18000; rtcSet(&temp_time); } #endif break; - case MSG_VELNED: - *gpsPacketLogChar = LOG_UBLOX_VELNED; - gpsSol.speed3d = _buffer.velned.speed_3d; // cm/s - gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - _new_speed = true; + case CLSMSG(CLASS_NAV, MSG_NAV_VELNED): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_VELNED; +#endif + gpsSol.speed3d = ubxRcvMsgPayload.ubxNavVelned.speed_3d; // cm/s + gpsSol.groundSpeed = ubxRcvMsgPayload.ubxNavVelned.speed_2d; // cm/s + gpsSol.groundCourse = (uint16_t) (ubxRcvMsgPayload.ubxNavVelned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + ubxHaveNewSpeed = true; break; - case MSG_PVT: - *gpsPacketLogChar = LOG_UBLOX_SOL; - next_fix = (_buffer.pvt.flags & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fixType == FIX_3D); - gpsSol.llh.lon = _buffer.pvt.lon; - gpsSol.llh.lat = _buffer.pvt.lat; - gpsSol.llh.altCm = _buffer.pvt.hMSL / 10; //alt in cm - gpsSetFixState(next_fix); - _new_position = true; - gpsSol.numSat = _buffer.pvt.numSV; - gpsSol.acc.hAcc = _buffer.pvt.hAcc; - gpsSol.acc.vAcc = _buffer.pvt.vAcc; - gpsSol.acc.sAcc = _buffer.pvt.sAcc; - gpsSol.speed3d = (uint16_t) sqrtf(powf(_buffer.pvt.gSpeed / 10, 2.0f) + powf(_buffer.pvt.velD / 10, 2.0f)); - gpsSol.groundSpeed = _buffer.pvt.gSpeed / 10; // cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.pvt.headMot / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - _new_speed = true; + case CLSMSG(CLASS_NAV, MSG_NAV_PVT): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SOL; +#endif + ubxHaveNewValidFix = (ubxRcvMsgPayload.ubxNavPvt.flags & NAV_STATUS_FIX_VALID) && (ubxRcvMsgPayload.ubxNavPvt.fixType == FIX_3D); + gpsSol.time = ubxRcvMsgPayload.ubxNavPvt.time; + calculateNavInterval(); + gpsSol.llh.lon = ubxRcvMsgPayload.ubxNavPvt.lon; + gpsSol.llh.lat = ubxRcvMsgPayload.ubxNavPvt.lat; + gpsSol.llh.altCm = ubxRcvMsgPayload.ubxNavPvt.hMSL / 10; //alt in cm + gpsSetFixState(ubxHaveNewValidFix); + ubxHaveNewPosition = true; + gpsSol.numSat = ubxRcvMsgPayload.ubxNavPvt.numSV; + gpsSol.acc.hAcc = ubxRcvMsgPayload.ubxNavPvt.hAcc; + gpsSol.acc.vAcc = ubxRcvMsgPayload.ubxNavPvt.vAcc; + gpsSol.acc.sAcc = ubxRcvMsgPayload.ubxNavPvt.sAcc; + gpsSol.speed3d = (uint16_t) sqrtf(powf(ubxRcvMsgPayload.ubxNavPvt.gSpeed / 10, 2.0f) + powf(ubxRcvMsgPayload.ubxNavPvt.velD / 10, 2.0f)); + gpsSol.groundSpeed = ubxRcvMsgPayload.ubxNavPvt.gSpeed / 10; // cm/s + gpsSol.groundCourse = (uint16_t) (ubxRcvMsgPayload.ubxNavPvt.headMot / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSol.dop.pdop = ubxRcvMsgPayload.ubxNavPvt.pDOP; + ubxHaveNewSpeed = true; #ifdef USE_RTC_TIME //set clock, when gps time is available - if (!rtcHasTime() && (_buffer.pvt.valid & NAV_VALID_DATE) && (_buffer.pvt.valid & NAV_VALID_TIME)) { + if (!rtcHasTime() && (ubxRcvMsgPayload.ubxNavPvt.valid & NAV_VALID_DATE) && (ubxRcvMsgPayload.ubxNavPvt.valid & NAV_VALID_TIME)) { dateTime_t dt; - dt.year = _buffer.pvt.year; - dt.month = _buffer.pvt.month; - dt.day = _buffer.pvt.day; - dt.hours = _buffer.pvt.hour; - dt.minutes = _buffer.pvt.min; - dt.seconds = _buffer.pvt.sec; - dt.millis = (_buffer.pvt.nano > 0) ? _buffer.pvt.nano / 1000 : 0; //up to 5ms of error + dt.year = ubxRcvMsgPayload.ubxNavPvt.year; + dt.month = ubxRcvMsgPayload.ubxNavPvt.month; + dt.day = ubxRcvMsgPayload.ubxNavPvt.day; + dt.hours = ubxRcvMsgPayload.ubxNavPvt.hour; + dt.minutes = ubxRcvMsgPayload.ubxNavPvt.min; + dt.seconds = ubxRcvMsgPayload.ubxNavPvt.sec; + dt.millis = (ubxRcvMsgPayload.ubxNavPvt.nano > 0) ? ubxRcvMsgPayload.ubxNavPvt.nano / 1000 : 0; //up to 5ms of error rtcSetDateTime(&dt); } #endif break; - case MSG_SVINFO: - *gpsPacketLogChar = LOG_UBLOX_SVINFO; - GPS_numCh = _buffer.svinfo.numCh; - // If we're getting NAV-SVINFO is because we're dealing with an old receiver that does not support NAV-SAT, so we'll only - // save up to GPS_SV_MAXSATS_LEGACY channels so the BF Configurator knows it's receiving the old sat list info format. + case CLSMSG(CLASS_NAV, MSG_NAV_SVINFO): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; +#endif + GPS_numCh = ubxRcvMsgPayload.ubxNavSvinfo.numCh; + // If we're receiving UBX-NAV-SVINFO messages, we detected a module version M7 or older. + // We can receive far more sats than we can handle for Configurator, which is the primary consumer for sat info. + // We're using the max for legacy (16) for our sizing, this smaller sizing triggers Configurator to know it's + // an M7 or earlier module and to use the older sat list format. + // We simply ignore any sats above that max, the down side is we may not see sats used for the solution, but + // the intent in Configurator is to see if sats are being acquired and their strength, so this is not an issue. if (GPS_numCh > GPS_SV_MAXSATS_LEGACY) GPS_numCh = GPS_SV_MAXSATS_LEGACY; for (i = 0; i < GPS_numCh; i++) { - GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn; - GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid; - GPS_svinfo_quality[i] =_buffer.svinfo.channel[i].quality; - GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno; + GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].chn; + GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].svid; + GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].quality; + GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].cno; } - for (i = GPS_numCh; i < GPS_SV_MAXSATS_LEGACY; i++) { + for (; i < GPS_SV_MAXSATS_LEGACY; i++) { GPS_svinfo_chn[i] = 0; GPS_svinfo_svid[i] = 0; GPS_svinfo_quality[i] = 0; GPS_svinfo_cno[i] = 0; } - GPS_svInfoReceivedCount++; +#ifdef USE_DASHBOARD + dashboardGpsNavSvInfoRcvCount++; +#endif break; - case MSG_SAT: - *gpsPacketLogChar = LOG_UBLOX_SVINFO; // The logger won't show this is NAV-SAT instead of NAV-SVINFO - GPS_numCh = _buffer.sat.numSvs; - // We can receive here upto GPS_SV_MAXSATS_M9N channels, but since the majority of receivers currently in use are M8N or older, - // it would be a waste of RAM to size the arrays that big. For now, they're sized GPS_SV_MAXSATS_M8N which means M9N won't show - // all their channel information on BF Configurator. When M9N's are more widespread it would be a good time to increase those arrays. + case CLSMSG(CLASS_NAV, MSG_NAV_SAT): +#ifdef USE_DASHBOARD + *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; // The display log only shows SVINFO for both SVINFO and SAT. +#endif + GPS_numCh = ubxRcvMsgPayload.ubxNavSat.numSvs; + // If we're receiving UBX-NAV-SAT messages, we detected a module M8 or newer. + // We can receive far more sats than we can handle for Configurator, which is the primary consumer for sat info. + // We're using the max for M8 (32) for our sizing, since Configurator only supports a max of 32 sats and we + // want to limit the payload buffer space used. + // We simply ignore any sats above that max, the down side is we may not see sats used for the solution, but + // the intent in Configurator is to see if sats are being acquired and their strength, so this is not an issue. if (GPS_numCh > GPS_SV_MAXSATS_M8N) GPS_numCh = GPS_SV_MAXSATS_M8N; for (i = 0; i < GPS_numCh; i++) { - GPS_svinfo_chn[i] = _buffer.sat.svs[i].gnssId; - GPS_svinfo_svid[i] = _buffer.sat.svs[i].svId; - GPS_svinfo_cno[i] = _buffer.sat.svs[i].cno; - GPS_svinfo_quality[i] =_buffer.sat.svs[i].flags; + GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].gnssId; + GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].svId; + GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].cno; + GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].flags; } - for (i = GPS_numCh; i < GPS_SV_MAXSATS_M8N; i++) { + for (; i < GPS_SV_MAXSATS_M8N; i++) { GPS_svinfo_chn[i] = 255; GPS_svinfo_svid[i] = 0; GPS_svinfo_quality[i] = 0; @@ -1701,35 +2322,37 @@ static bool UBLOX_parse_gps(void) // enhanced sat list info without changing the MSP protocol. Also, we're sending the complete list each time even if it's empty, so // BF Conf can erase old entries shown on screen when channels are removed from the list. GPS_numCh = GPS_SV_MAXSATS_M8N; - GPS_svInfoReceivedCount++; +#ifdef USE_DASHBOARD + dashboardGpsNavSvInfoRcvCount++; +#endif break; - case MSG_CFG_GNSS: + case CLSMSG(CLASS_CFG, MSG_CFG_GNSS): { bool isSBASenabled = false; bool isM8NwithDefaultConfig = false; - if ((_buffer.gnss.numConfigBlocks >= 2) && - (_buffer.gnss.configblocks[1].gnssId == 1) && //SBAS - (_buffer.gnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled + if ((ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks >= 2) && + (ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].gnssId == 1) && //SBAS + (ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled isSBASenabled = true; } - if ((_buffer.gnss.numTrkChHw == 32) && //M8N - (_buffer.gnss.numTrkChUse == 32) && - (_buffer.gnss.numConfigBlocks == 7) && - (_buffer.gnss.configblocks[2].gnssId == 2) && //Galileo - (_buffer.gnss.configblocks[2].resTrkCh == 4) && //min channels - (_buffer.gnss.configblocks[2].maxTrkCh == 8) && //max channels - !(_buffer.gnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled + if ((ubxRcvMsgPayload.ubxCfgGnss.numTrkChHw == 32) && //M8N + (ubxRcvMsgPayload.ubxCfgGnss.numTrkChUse == 32) && + (ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks == 7) && + (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].gnssId == 2) && //Galileo + (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].resTrkCh == 4) && //min channels + (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].maxTrkCh == 8) && //max channels + !(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled isM8NwithDefaultConfig = true; } - const uint16_t messageSize = 4 + (_buffer.gnss.numConfigBlocks * sizeof(ubxConfigblock_t)); + const uint16_t messageSize = 4 + (ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks * sizeof(ubxConfigblock_t)); ubxMessage_t tx_buffer; - memcpy(&tx_buffer.payload, &_buffer, messageSize); + memcpy(&tx_buffer.payload, &ubxRcvMsgPayload, messageSize); if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) { tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS @@ -1739,27 +2362,29 @@ static bool UBLOX_parse_gps(void) tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo } - ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize); + ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize, false); } break; - case MSG_ACK_ACK: - if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) { + case CLSMSG(CLASS_ACK, MSG_ACK_ACK): + if ((gpsData.ackState == UBLOX_ACK_WAITING) && (ubxRcvMsgPayload.ubxAck.msgId == gpsData.ackWaitingMsgId)) { gpsData.ackState = UBLOX_ACK_GOT_ACK; } break; - case MSG_ACK_NACK: - if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) { + case CLSMSG(CLASS_ACK, MSG_ACK_NACK): + if ((gpsData.ackState == UBLOX_ACK_WAITING) && (ubxRcvMsgPayload.ubxAck.msgId == gpsData.ackWaitingMsgId)) { gpsData.ackState = UBLOX_ACK_GOT_NACK; } break; + default: return false; } +#undef CLSMSG // we only return true when we get new position and speed data // this ensures we don't use stale data - if (_new_position && _new_speed) { - _new_speed = _new_position = false; + if (ubxHaveNewPosition && ubxHaveNewSpeed) { + ubxHaveNewSpeed = ubxHaveNewPosition = false; return true; } return false; @@ -1767,94 +2392,129 @@ static bool UBLOX_parse_gps(void) static bool gpsNewFrameUBLOX(uint8_t data) { - bool parsed = false; + bool newPositionDataReceived = false; - switch (_step) { - case 0: // Sync char 1 (0xB5) + switch (ubxFrameParseState) { + case UBX_PARSE_PREAMBLE_SYNC_1: if (PREAMBLE1 == data) { - _skip_packet = false; - _step++; - } - break; - case 1: // Sync char 2 (0x62) - if (PREAMBLE2 != data) { - _step = 0; + // Might be a new UBX message, go on to look for next preamble byte. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; break; } - _step++; + // Not a new UBX message, stay in this state for the next incoming byte. break; - case 2: // Class - _step++; - _class = data; - _ck_b = _ck_a = data; // reset the checksum accumulators - break; - case 3: // Id - _step++; - _ck_b += (_ck_a += data); // checksum byte - _msg_id = data; -#if DEBUG_UBLOX_FRAMES - debug[2] = _msg_id; -#endif - break; - case 4: // Payload length (part 1) - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length = data; // payload length low byte - break; - case 5: // Payload length (part 2) - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length += (uint16_t)(data << 8); -#if DEBUG_UBLOX_FRAMES - debug[3] = _payload_length; -#endif - if (_payload_length > UBLOX_PAYLOAD_SIZE) { - _skip_packet = true; - } - _payload_counter = 0; // prepare to receive payload - if (_payload_length == 0) { - _step = 7; - } - break; - case 6: - _ck_b += (_ck_a += data); // checksum byte - if (_payload_counter < UBLOX_PAYLOAD_SIZE) { - _buffer.bytes[_payload_counter] = data; - } - if (++_payload_counter >= _payload_length) { - _step++; - } - break; - case 7: - _step++; - if (_ck_a != data) { - _skip_packet = true; // bad checksum - gpsData.errors++; - } - break; - case 8: - _step = 0; - - shiftPacketLog(); - - if (_ck_b != data) { - *gpsPacketLogChar = LOG_ERROR; - gpsData.errors++; - break; // bad checksum - } - - GPS_packetCount++; - - if (_skip_packet) { - *gpsPacketLogChar = LOG_SKIPPED; + case UBX_PARSE_PREAMBLE_SYNC_2: + if (PREAMBLE2 == data) { + // Matches the two-byte preamble, seems to be a legit message, go on to process the rest of the message. + ubxFrameParseState = UBX_PARSE_MESSAGE_CLASS; break; } - - if (UBLOX_parse_gps()) { - parsed = true; + // False start, if this byte is not a preamble 1, restart new message parsing. + // If this byte is a preamble 1, we might have gotten two in a row, so stay here and look for preamble 2 again. + if (PREAMBLE1 != data) { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; } - } - return parsed; + break; + case UBX_PARSE_MESSAGE_CLASS: + ubxRcvMsgChecksumB = ubxRcvMsgChecksumA = data; // Reset & start the checksum A & B accumulators. + ubxRcvMsgClass = data; + ubxFrameParseState = UBX_PARSE_MESSAGE_ID; + break; + case UBX_PARSE_MESSAGE_ID: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + ubxRcvMsgID = data; + ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_LSB; + break; + case UBX_PARSE_PAYLOAD_LENGTH_LSB: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); + ubxRcvMsgPayloadLength = data; // Payload length LSB. + ubxFrameParseState = UBX_PARSE_PAYLOAD_LENGTH_MSB; + break; + case UBX_PARSE_PAYLOAD_LENGTH_MSB: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + ubxRcvMsgPayloadLength += (uint16_t)(data << 8); //Payload length MSB. + if (ubxRcvMsgPayloadLength == 0) { + // No payload for this message, skip to checksum checking. + ubxFrameParseState = UBX_PARSE_CHECKSUM_A; + break; + } + if (ubxRcvMsgPayloadLength > UBLOX_MAX_PAYLOAD_SANITY_SIZE) { + // Payload length is not reasonable, treat as a bad packet, restart new message parsing. + // Note that we do not parse the rest of the message, better to leave it and look for a new message. +#ifdef USE_DASHBOARD + logErrorToPacketLog(); +#endif + if (PREAMBLE1 == data) { + // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; + } else { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + } + // Payload length seems legit, go on to receive the payload content. + ubxFrameParsePayloadCounter = 0; + ubxFrameParseState = UBX_PARSE_PAYLOAD_CONTENT; + break; + case UBX_PARSE_PAYLOAD_CONTENT: + ubxRcvMsgChecksumB += (ubxRcvMsgChecksumA += data); // Accumulate both checksums. + if (ubxFrameParsePayloadCounter < UBLOX_PAYLOAD_SIZE) { + // Only add bytes to the buffer if we haven't reached the max supported payload size. + // Note that we still read & checksum every byte so the checksum calculates correctly. + ubxRcvMsgPayload.rawBytes[ubxFrameParsePayloadCounter] = data; + } + if (++ubxFrameParsePayloadCounter >= ubxRcvMsgPayloadLength) { + // All bytes for payload length processed. + ubxFrameParseState = UBX_PARSE_CHECKSUM_A; + break; + } + // More payload content left, stay in this state. + break; + case UBX_PARSE_CHECKSUM_A: + if (ubxRcvMsgChecksumA == data) { + // Checksum A matches, go on to checksum B. + ubxFrameParseState = UBX_PARSE_CHECKSUM_B; + break; + } + // Bad checksum A, restart new message parsing. + // Note that we do not parse checksum B, new message processing will handle skipping it if needed. +#ifdef USE_DASHBOARD + logErrorToPacketLog(); +#endif + if (PREAMBLE1 == data) { + // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; + } else { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + case UBX_PARSE_CHECKSUM_B: + if (ubxRcvMsgChecksumB == data) { + // Checksum B also matches, successfully received a new full packet! +#ifdef USE_DASHBOARD + dashboardGpsPacketCount++; // Packet counter used by dashboard device. + shiftPacketLog(); // Make space for message handling to add the message type char to the dashboard device packet log. +#endif + // Handle the parsed message. Note this is a questionable inverted call dependency, but something for a later refactoring. + newPositionDataReceived = UBLOX_parse_gps(); // True only when we have new position data from the parsed message. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; // Restart new message parsing. + break; + } + // Bad checksum B, restart new message parsing. +#ifdef USE_DASHBOARD + logErrorToPacketLog(); +#endif + if (PREAMBLE1 == data) { + // If this byte is a preamble 1 value, it might be a new message, so look for preamble 2 instead of starting over. + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_2; + } else { + ubxFrameParseState = UBX_PARSE_PREAMBLE_SYNC_1; + } + break; + } + + // Note this function returns if UBLOX_parse_gps() found new position data, NOT whether this function successfully parsed the frame or not. + return newPositionDataReceived; } #endif // USE_GPS_UBLOX @@ -1863,6 +2523,7 @@ static void gpsHandlePassthrough(uint8_t data) gpsNewData(data); #ifdef USE_DASHBOARD if (featureIsEnabled(FEATURE_DASHBOARD)) { + // Should be handled via a generic callback hook, so the GPS module doesn't have to be coupled to the dashboard module. dashboardUpdate(micros()); } #endif @@ -1878,6 +2539,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) #ifdef USE_DASHBOARD if (featureIsEnabled(FEATURE_DASHBOARD)) { + // Should be handled via a generic callback hook, so the GPS module doesn't have to be coupled to the dashboard module. dashboardShowFixedPage(PAGE_GPS); } #endif @@ -1977,28 +2639,13 @@ void GPS_calculateDistanceAndDirectionToHome(void) void onGpsNewData(void) { - static timeUs_t lastTimeUs = 0; - const timeUs_t timeUs = micros(); - - // calculate GPS solution interval - // !!! TOO MUCH JITTER TO BE USEFUL - need an exact time !!! - const float gpsDataIntervalS = cmpTimeUs(timeUs, lastTimeUs) / 1e6f; - // dirty hack to remove jitter from interval - if (gpsDataIntervalS < 0.15f) { - gpsDataIntervalSeconds = 0.1f; - } else if (gpsDataIntervalS < 0.4f) { - gpsDataIntervalSeconds = 0.2f; - } else { - gpsDataIntervalSeconds = 1.0f; - } - - lastTimeUs = timeUs; - if (!STATE(GPS_FIX)) { // if we don't have a 3D fix don't give data to GPS rescue return; } + gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f; + GPS_calculateDistanceAndDirectionToHome(); if (ARMING_FLAG(ARMED)) { GPS_calculateDistanceFlown(false); @@ -2033,3 +2680,4 @@ baudRate_e getGpsPortActualBaudRateIndex(void) } #endif // USE_GPS + diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 8ebbec9044..43aa9239cb 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -35,6 +35,138 @@ #define GPS_Y 0 #define GPS_MIN_SAT_COUNT 4 // number of sats to trigger low sat count sanity check +#ifdef USE_GPS_UBLOX +typedef enum { + UBX_VERSION_UNDEF = 0, + UBX_VERSION_M5, + UBX_VERSION_M6, + UBX_VERSION_M7, + UBX_VERSION_M8, + UBX_VERSION_M9, + UBX_VERSION_M10, + UBX_VERSION_COUNT +} ubloxVersion_e; + +typedef enum { + CFG_RATE_MEAS = 0x30210001, // U2 + CFG_RATE_NAV = 0x30210002, // U2 + CFG_RATE_TIMEREF = 0x20210003, // E1 + CFG_NAVSPG_FIXMODE = 0x20110011, // E1 + CFG_NAVSPG_DYNMODEL = 0x20110021, // E1 + CFG_NAVSPG_UTCSTANDARD = 0x2011001c, // E1 + CFG_NAVSPG_CONSTR_ALT = 0x401100c1, // I4 + CFG_NAVSPG_CONSTR_ALTVAR = 0x401100c2, // U4 + CFG_NAVSPG_CONSTR_DGNSSTO = 0x201100c4, // U1 + CFG_NAVSPG_INFIL_MINELEV = 0x201100a4, // I1 + CFG_NAVSPG_INFIL_CNOTHRS = 0x201100ab, // U1 + CFG_NAVSPG_INFIL_NCNOTHRS = 0x201100aa, // U1 + CFG_NAVSPG_OUTFIL_PDOP = 0x301100b1, // U2 + CFG_NAVSPG_OUTFIL_TDOP = 0x301100b2, // U2 + CFG_NAVSPG_OUTFIL_PACC = 0x301100b3, // U2 + CFG_NAVSPG_OUTFIL_TACC = 0x301100b4, // U2 + CFG_MOT_GNSSSPEED_THRS = 0x20250038, // U1 + CFG_MOT_GNSSDIST_THRS = 0x3025003b, // U2 + CFG_ANA_USE_ANA = 0x10230001, // L + CFG_MSGOUT_NMEA_ID_VTG_I2C = 0x209100b0, // U1 + CFG_MSGOUT_NMEA_ID_VTG_SPI = 0x209100b4, // U1 + CFG_MSGOUT_NMEA_ID_VTG_UART1 = 0x209100b1, // U1 + CFG_MSGOUT_NMEA_ID_GSV_I2C = 0x209100c4, // U1 + CFG_MSGOUT_NMEA_ID_GSV_SPI = 0x209100c8, // U1 + CFG_MSGOUT_NMEA_ID_GSV_UART1 = 0x209100c5, // U1 + CFG_MSGOUT_NMEA_ID_GLL_I2C = 0x209100c9, // U1 + CFG_MSGOUT_NMEA_ID_GLL_SPI = 0x209100cd, // U1 + CFG_MSGOUT_NMEA_ID_GLL_UART1 = 0x209100ca, // U1 + CFG_MSGOUT_NMEA_ID_GGA_I2C = 0x209100ba, // U1 + CFG_MSGOUT_NMEA_ID_GGA_SPI = 0x209100be, // U1 + CFG_MSGOUT_NMEA_ID_GGA_UART1 = 0x209100bb, // U1 + CFG_MSGOUT_NMEA_ID_GSA_I2C = 0x209100bf, // U1 + CFG_MSGOUT_NMEA_ID_GSA_SPI = 0x209100c3, // U1 + CFG_MSGOUT_NMEA_ID_GSA_UART1 = 0x209100c0, // U1 + CFG_MSGOUT_NMEA_ID_RMC_I2C = 0x209100ab, // U1 + CFG_MSGOUT_NMEA_ID_RMC_SPI = 0x209100af, // U1 + CFG_MSGOUT_NMEA_ID_RMC_UART1 = 0x209100ac, // U1 + CFG_MSGOUT_UBX_NAV_PVT_I2C = 0x20910006, // U1 + CFG_MSGOUT_UBX_NAV_PVT_SPI = 0x2091000a, // U1 + CFG_MSGOUT_UBX_NAV_PVT_UART1 = 0x20910007, // U1 + CFG_MSGOUT_UBX_NAV_SAT_I2C = 0x20910015, // U1 + CFG_MSGOUT_UBX_NAV_SAT_SPI = 0x20910019, // U1 + CFG_MSGOUT_UBX_NAV_SAT_UART1 = 0x20910016, // U1 + CFG_MSGOUT_UBX_NAV_DOP_I2C = 0x20910038, // U1 + CFG_MSGOUT_UBX_NAV_DOP_SPI = 0x2091003c, // U1 + CFG_MSGOUT_UBX_NAV_DOP_UART1 = 0x20910039, // U1 + CFG_SBAS_USE_TESTMODE = 0x10360002, // L + CFG_SBAS_USE_RANGING = 0x10360003, // L + CFG_SBAS_USE_DIFFCORR = 0x10360004, // L + CFG_SBAS_USE_INTEGRITY = 0x10360005, // L + CFG_SBAS_PRNSCANMASK = 0x50360006, // X8 + CFG_SIGNAL_GPS_ENA = 0x1031001f, // L + CFG_SIGNAL_SBAS_ENA = 0x10310020, // L + CFG_SIGNAL_GAL_ENA = 0x10310021, // L + CFG_SIGNAL_BDS_ENA = 0x10310022, // L + CFG_SIGNAL_QZSS_ENA = 0x10310024, // L + CFG_SIGNAL_GLO_ENA = 0x10310025, // L + CFG_PM_OPERATEMODE = 0x20d00001, // E1 +} ubxValGetSetBytes_e; + +/* + * replaced by following macro, to save space; keeping for reference +typedef enum { + SBAS_SEARCH_ALL = 0, + SBAS_SEARCH_PRN120, + SBAS_SEARCH_PRN121, + // repeat to 158 + SBAS_SEARCH_PRN158, +} ubxSbasPrnScan_e; +*/ + +#define SBAS_SEARCH_ALL 0x0 +#define SBAS_SEARCH_PRN(i) (1 << (i - 120)) + +typedef enum { + UBX_VAL_LAYER_RAM = 0x01, + UBX_VAL_LAYER_BBR = 0x02, + UBX_VAL_LAYER_FLASH = 0x04, +} ubloxValLayer_e; + +typedef enum { + UBLOX_MODEL_PORTABLE = 0, + UBLOX_MODEL_STATIONARY, + UBLOX_MODEL_PEDESTRIAN, + UBLOX_MODEL_AUTOMOTIVE, + UBLOX_MODEL_AT_SEA, + UBLOX_MODEL_AIRBORNE_1G, + UBLOX_MODEL_AIRBORNE_2G, + UBLOX_MODEL_AIRBORNE_4G, +} ubloxModel_e; + +typedef enum { + UBLOX_UTC_STANDARD_AUTO = 0, + UBLOX_UTC_STANDARD_USNO = 3, + UBLOX_UTC_STANDARD_EU = 5, + UBLOX_UTC_STANDARD_SU = 6, + UBLOX_UTC_STANDARD_NTSC = 7, +} ubloxUtcStandard_e; + +struct ubloxVersion_s { + uint32_t hw; + const char* str; +}; +extern struct ubloxVersion_s ubloxVersionMap[]; + +#endif // USE_GPS_UBLOX + +typedef enum { + GPS_STATE_UNKNOWN = 0, + GPS_STATE_DETECT_BAUD, + GPS_STATE_INITIALIZED, + GPS_STATE_CHANGE_BAUD, + GPS_STATE_CONFIGURE, + GPS_STATE_RECEIVING_DATA, + GPS_STATE_PROCESS_DATA, + GPS_STATE_LOST_COMMUNICATION, + GPS_STATE_COUNT +} gpsState_e; + typedef enum { GPS_LATITUDE, GPS_LONGITUDE @@ -57,17 +189,6 @@ typedef enum { #define SBAS_MODE_MAX SBAS_GAGAN -typedef enum { - UBLOX_MODEL_PORTABLE = 0, - UBLOX_MODEL_STATIONARY, - UBLOX_MODEL_PEDESTRIAN, - UBLOX_MODEL_AUTOMOTIVE, - UBLOX_MODEL_AT_SEA, - UBLOX_MODEL_AIRBORNE_1G, - UBLOX_MODEL_AIRBORNE_2G, - UBLOX_MODEL_AIRBORNE_4G, -} ubloxModel_e; - typedef enum { GPS_BAUDRATE_115200 = 0, GPS_BAUDRATE_57600, @@ -130,37 +251,61 @@ typedef struct gpsSolutionData_s { uint16_t groundCourse; // degrees * 10 uint8_t numSat; uint32_t time; // GPS msToW + uint32_t navIntervalMs; // interval between nav solutions in ms } gpsSolutionData_t; +/* +* keeping this table for reference +typedef enum { + UBX_CAP_SAT_NONE = 0x0, + UBX_CAP_SAT_GPS = 0x0001, + UBX_CAP_SAT_GLO = 0x0002, + UBX_CAP_SAT_GAL = 0x0004, + UBX_CAP_SAT_BDS = 0x0008, + UBX_CAP_SAT_SBAS = 0x0010, + UBX_CAP_SAT_QZSS = 0x0020, +} ubxCapabilities_e; +*/ + +typedef struct ubxMonVer_s { + char swVersion[30]; + char hwVersion[10]; + char extension[300]; // variable size but not more than 300 values +} ubxMonVer_t; + typedef struct gpsData_s { uint32_t errors; // gps error counter - crc error/lost of data/sync etc.. uint32_t timeouts; - uint32_t lastMessage; // last time valid GPS data was received (millis) - uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta. - + uint32_t lastNavMessage; // time of last valid GPS speed and position data + uint32_t now; + uint32_t lastMessageSent; // time last message was sent uint32_t state_position; // incremental variable for loops uint32_t state_ts; // timestamp for last state_position increment uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices - uint8_t baudrateIndex; // index into auto-detecting or current baudrate + uint8_t userBaudRateIndex; // index into auto-detecting or current baudrate + uint8_t tempBaudRateIndex; // index into auto-detecting or current baudrate uint8_t ackWaitingMsgId; // Message id when waiting for ACK - uint8_t ackTimeoutCounter; // Ack timeout counter - ubloxAckState_e ackState; - bool ubloxUsePVT; - bool ubloxUseSAT; + ubloxAckState_e ackState; // Ack State + uint8_t updateRateHz; + bool ubloxM7orAbove; + bool ubloxM8orAbove; + bool ubloxM9orAbove; + bool ubloxUsingFlightModel; // false = Acquire model, true = Flight model + bool satMessagesDisabled; +#ifdef USE_GPS_UBLOX + uint32_t lastNavSolTs; // time stamp of last UBCX message. Used to calculate message delta + ubloxVersion_e platformVersion; // module platform version, mapped from reported hardware version +#endif } gpsData_t; -#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display. -extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; - extern int32_t GPS_home[2]; -extern uint16_t GPS_distanceToHome; // distance to home point in meters -extern uint32_t GPS_distanceToHomeCm; // distance to home point in cm -extern int16_t GPS_directionToHome; // direction to home or hol point in degrees -extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters -extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction -extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles -extern int16_t nav_takeoff_bearing; +extern uint16_t GPS_distanceToHome; // distance to home point in meters +extern uint32_t GPS_distanceToHomeCm; // distance to home point in cm +extern int16_t GPS_directionToHome; // direction to home or hol point in degrees +extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters +extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction +extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles typedef enum { GPS_DIRECT_TICK = 1 << 0, @@ -174,9 +319,7 @@ extern gpsSolutionData_t gpsSol; #define GPS_SV_MAXSATS_M8N 32U #define GPS_SV_MAXSATS_M9N 42U -extern uint8_t GPS_update; // toogle to distinct a GPS position update (directly or via MSP) -extern uint32_t GPS_packetCount; -extern uint32_t GPS_svInfoReceivedCount; +extern uint8_t GPS_update; // toggles on GPS nav position update (directly or via MSP) extern uint8_t GPS_numCh; // Number of channels extern uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or less: Channel number // When NumCh is more than 16: GNSS Id @@ -204,16 +347,46 @@ extern uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or l // 1 = carrier smoothed pseudorange used extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; // Carrier to Noise Ratio (Signal Strength) +#define TASK_GPS_RATE 100 // default update rate of GPS task +#define TASK_GPS_RATE_FAST 500 // update rate of GPS task while Rx buffer is not empty + + +#ifdef USE_DASHBOARD +// Data used *only* by the dashboard device (OLED display). +// Note this data should probably be in the dashboard module, not here. On the refactor list... + +// OLED display shows a scrolling string of chars for each packet type/event received. +#define DASHBOARD_LOG_ERROR '?' +#define DASHBOARD_LOG_IGNORED '!' +#define DASHBOARD_LOG_SKIPPED '>' +#define DASHBOARD_LOG_NMEA_GGA 'g' +#define DASHBOARD_LOG_NMEA_GSA 's' +#define DASHBOARD_LOG_NMEA_RMC 'r' +#define DASHBOARD_LOG_UBLOX_DOP 'D' +#define DASHBOARD_LOG_UBLOX_SOL 'O' +#define DASHBOARD_LOG_UBLOX_STATUS 'S' +#define DASHBOARD_LOG_UBLOX_SVINFO 'I' +#define DASHBOARD_LOG_UBLOX_POSLLH 'P' +#define DASHBOARD_LOG_UBLOX_VELNED 'V' +#define DASHBOARD_LOG_UBLOX_MONVER 'M' + +#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display. +extern char dashboardGpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; // OLED display of a char for each packet type/event received. +extern uint32_t dashboardGpsPacketCount; // Packet received count. +extern uint32_t dashboardGpsNavSvInfoRcvCount; // Count of times sat info received & updated. + #define GPS_DBHZ_MIN 0 #define GPS_DBHZ_MAX 55 +#endif // USE_DASHBOARD -#define TASK_GPS_RATE 100 -#define TASK_GPS_RATE_FAST 1000 +#ifdef USE_GPS_UBLOX +ubloxVersion_e ubloxParseVersion(const uint32_t version); +#endif void gpsInit(void); void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c); -bool gpsIsHealthy(void); // Check for healthy communications +bool gpsIsHealthy(void); // Returns true when the gps state is RECEIVING_DATA struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); void onGpsNewData(void); @@ -221,5 +394,5 @@ void GPS_reset_home_position(void); void GPS_calc_longitude_scaling(int32_t lat); void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); void gpsSetFixState(bool state); -float getGpsDataIntervalSeconds(void); +float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue baudRate_e getGpsPortActualBaudRateIndex(void); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 6e2abd8b23..cbf04e9a7d 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1114,7 +1114,7 @@ static void osdElementGpsSats(osdElementParms_t *element) int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); if (osdConfig()->gps_sats_show_hdop) { // add on the GPS module HDOP estimate element->buff[pos++] = ' '; - osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.hdop / 100.0f, "", 1, true, SYM_NONE); + osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.pdop / 100.0f, "", 1, true, SYM_NONE); } } } diff --git a/src/main/pg/gps.c b/src/main/pg/gps.c index 4cfcaeaa09..750e43530e 100644 --- a/src/main/pg/gps.c +++ b/src/main/pg/gps.c @@ -29,7 +29,7 @@ #include "gps.h" -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = GPS_UBLOX, @@ -40,10 +40,10 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .gps_ublox_flight_model = UBLOX_MODEL_AIRBORNE_4G, .gps_update_rate_hz = 10, .gps_ublox_use_galileo = false, - .gps_ublox_full_power = true, .gps_set_home_point_once = false, .gps_use_3d_speed = false, .sbas_integrity = false, + .gps_ublox_utc_standard = UBLOX_UTC_STANDARD_AUTO, ); #endif // USE_GPS diff --git a/src/main/pg/gps.h b/src/main/pg/gps.h index 057a1c6b99..5262801370 100644 --- a/src/main/pg/gps.h +++ b/src/main/pg/gps.h @@ -36,10 +36,10 @@ typedef struct gpsConfig_s { uint8_t gps_ublox_flight_model; uint8_t gps_update_rate_hz; bool gps_ublox_use_galileo; - bool gps_ublox_full_power; bool gps_set_home_point_once; bool gps_use_3d_speed; bool sbas_integrity; + uint8_t gps_ublox_utc_standard; char nmeaCustomCommands[NMEA_CUSTOM_COMMANDS_MAX_LENGTH + 1]; } gpsConfig_t; diff --git a/src/test/Makefile b/src/test/Makefile index 2adf2676dc..fcea264467 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -109,8 +109,13 @@ cli_unittest_SRC := \ $(USER_DIR)/common/crc.c \ $(USER_DIR)/common/printf.c \ $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/common/gps_conversion.c \ $(USER_DIR)/config/feature.c \ + $(USER_DIR)/io/gps.c \ $(USER_DIR)/pg/pg.c \ + $(USER_DIR)/fc/runtime_config.c \ + $(USER_DIR)/drivers/serial.c \ $(USER_DIR)/common/typeconversion.c cli_unittest_DEFINES := \ diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 254a31f112..7637821ddb 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -26,9 +26,12 @@ extern "C" { #include "platform.h" #include "target.h" #include "build/version.h" + #include "io/gps.h" #include "cli/cli.h" #include "cli/settings.h" #include "common/printf.h" + #include "common/maths.h" + #include "common/gps_conversion.h" #include "config/feature.h" #include "drivers/buf_writer.h" #include "drivers/vtx_common.h" @@ -39,7 +42,6 @@ extern "C" { #include "flight/pid.h" #include "flight/servos.h" #include "io/beeper.h" - #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" #include "io/vtx.h" @@ -47,6 +49,7 @@ extern "C" { #include "msp/msp_box.h" #include "osd/osd.h" #include "pg/pg.h" + #include "pg/gps_rescue.h" #include "pg/pg_ids.h" #include "pg/beeper.h" #include "pg/gps.h" @@ -92,6 +95,7 @@ extern "C" { PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0); PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); + PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER_WITH_RESET_FN(int8_t, unitTestData, PG_RESERVED_FOR_TESTING_1, 0); } @@ -201,6 +205,7 @@ TEST(CLIUnittest, TestCliSetStringWriteOnce) // STUBS extern "C" { +int16_t debug[8]; float motor_disarmed[MAX_SUPPORTED_MOTORS]; uint16_t batteryWarningVoltage; @@ -280,9 +285,13 @@ bool parseColor(int, const char *) {return false; } bool resetEEPROM(void) { return true; } void bufWriterFlush(bufWriter_t *) {} void mixerResetDisarmedMotors(void) {} -void gpsEnablePassthrough(struct serialPort_s *) {} -bool gpsIsHealthy(void) { return true; } -baudRate_e getGpsPortActualBaudRateIndex(void) { return BAUD_AUTO; } + +typedef enum { + DUMMY +} pageId_e; + +void dashboardShowFixedPage(pageId_e){} +void dashboardUpdate(timeUs_t) {} bool parseLedStripConfig(int, const char *){return false; } const char rcChannelLetters[] = "AERT12345678abcdefgh"; @@ -302,8 +311,6 @@ baudRate_e lookupBaudRateIndex(uint32_t){return BAUD_9600; } serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e){ return NULL; } serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; } const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) { return NULL; } -void serialSetBaudRate(serialPort_t *, uint32_t) {} -void serialSetMode(serialPort_t *, portMode_e) {} void serialPassthrough(serialPort_t *, serialPort_t *, serialConsumer *, serialConsumer *) {} uint32_t millis(void) { return 0; } uint8_t getBatteryCellCount(void) { return 1; } @@ -320,11 +327,10 @@ uint16_t averageSystemLoadPercent = 0; timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; } uint16_t currentRxIntervalUs = 9000; -armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; } -const char *armingDisableFlagNames[]= { +/*const char *armingDisableFlagNames[]= { "DUMMYDISABLEFLAGNAME" -}; +};*/ void getTaskInfo(taskId_e, taskInfo_t *) {} void getCheckFuncInfo(cfCheckFuncInfo_t *) {} @@ -336,13 +342,13 @@ const char* const buildDate = "Jan 01 2017"; const char * const buildTime = "00:00:00"; const char * const shortGitRevision = "MASTER"; -uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} -uint8_t serialRead(serialPort_t *){return 0;} +//uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} +//uint8_t serialRead(serialPort_t *){return 0;} void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); } -void serialWriteBufShim(void *, const uint8_t *, int) {} +//void serialWriteBufShim(void *, const uint8_t *, int) {} void bufWriterInit(bufWriter_t *, uint8_t *, int, bufWrite_t, void *) { } -void setArmingDisabled(armingDisableFlags_e) {} +//void setArmingDisabled(armingDisableFlags_e) {} void waitForSerialPortToFinishTransmitting(serialPort_t *) {} void systemResetToBootloader(void) {} @@ -353,15 +359,16 @@ void writeUnmodifiedConfigToEEPROM(void) {} void changePidProfile(uint8_t) {} bool serialIsPortAvailable(serialPortIdentifier_e) { return false; } void generateLedConfig(ledConfig_t *, char *, size_t) {} -bool isSerialTransmitBufferEmpty(const serialPort_t *) {return true; } -void serialWrite(serialPort_t *, uint8_t ch) { printf("%c", ch);} +//bool isSerialTransmitBufferEmpty(const serialPort_t *) {return true; } +//void serialWrite(serialPort_t *, uint8_t ch) { printf("%c", ch);} -void serialSetCtrlLineStateCb(serialPort_t *, void (*)(void *, uint16_t ), void *) {} +//void serialSetCtrlLineStateCb(serialPort_t *, void (*)(void *, uint16_t ), void *) {} void serialSetCtrlLineStateDtrPin(serialPort_t *, ioTag_t ) {} void serialSetCtrlLineState(serialPort_t *, uint16_t ) {} -void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {} - +//void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {} +void rescheduleTask(taskId_e, timeDelta_t){} +void schedulerSetNextStateTime(timeDelta_t ){} char *getBoardName(void) { return NULL; } char *getManufacturerId(void) { return NULL; } bool boardInformationIsSet(void) { return true; } diff --git a/src/test/unit/target.h b/src/test/unit/target.h index c8553f10fa..cb35314fe8 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -32,6 +32,8 @@ #define USE_MAG #define USE_BARO #define USE_GPS +#define USE_GPS_NMEA +#define USE_GPS_UBLOX #define USE_DASHBOARD #define USE_SERIALRX #define USE_RX_MSP