From effe0fdf1a9635b02e029d0f62e6cc24d6ff6f73 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 8 Jun 2023 10:42:53 +0200 Subject: [PATCH 01/36] Add support for configuring other GNSS constellations --- docs/Settings.md | 30 +++++++++++++ src/main/fc/settings.yaml | 15 +++++++ src/main/io/gps.c | 5 ++- src/main/io/gps.h | 3 ++ src/main/io/gps_ublox.c | 88 +++++++++++++++++++++++++++++++++++++-- 5 files changed, 137 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ff6cdea80d0..52183880c2d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1422,6 +1422,16 @@ Which SBAS mode to be used --- +### gps_ublox_use_beidou + +Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### gps_ublox_use_galileo Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. @@ -1432,6 +1442,26 @@ Enable use of Galileo satellites. This is at the expense of other regional const --- +### gps_ublox_use_glonass + +Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### gps_ublox_use_gzss + +Enable use of GZSS satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### ground_test_mode For developer ground test use. Disables motors, sets heading status = Trusted on FW. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bbb4fbba4f5..45a2905bae2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1537,6 +1537,21 @@ groups: default_value: OFF field: ubloxUseGalileo type: bool + - name: gps_ublox_use_beidou + description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]." + default_value: OFF + field: ubloxUseBeidou + type: bool + - name: gps_ublox_use_gzss + description: "Enable use of GZSS satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]." + default_value: OFF + field: ubloxUseGzss + type: bool + - name: gps_ublox_use_glonass + description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]." + default_value: OFF + field: ubloxUseGlonass + type: bool - name: gps_min_sats description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count." default_value: 6 diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 85418c7c2d3..04cf059ad9c 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -121,7 +121,10 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT, .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT, .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT, - .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT + .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT, + .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT, + .ubloxUseGzss = SETTING_GPS_UBLOX_USE_GZSS_DEFAULT, + .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT, ); void gpsSetState(gpsState_e state) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 92241c95602..f2885b838ad 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -93,6 +93,9 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; gpsDynModel_e dynModel; bool ubloxUseGalileo; + bool ubloxUseBeidou; + bool ubloxUseGzss; + bool ubloxUseGlonass; uint8_t gpsMinSats; } gpsConfig_t; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 330cb2b75fc..8fb15be6488 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -370,6 +370,9 @@ static bool _new_speed; // Need this to determine if Galileo capable only static bool capGalileo; +static bool capBeidou; +static bool capGzss; +static bool capGlonass; // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status' @@ -452,8 +455,11 @@ static const uint8_t default_payload[] = { 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; -#define GNSSID_SBAS 1 -#define GNSSID_GALILEO 2 +#define GNSSID_SBAS 1 +#define GNSSID_GALILEO 2 +#define GNSSID_BEIDOU 3 +#define GNSSID_GZSS 4 +#define GNSSID_GLONASS 5 static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { @@ -491,6 +497,67 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) return 1; } +static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) +{ + if (!capBeidou) { + return 0; + } + + gnss_block->gnssId = GNSSID_BEIDOU; + gnss_block->maxTrkCh = 8; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->ubloxUseBeidou) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + +static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) +{ + if (!capGzss) { + return 0; + } + + gnss_block->gnssId = GNSSID_GZSS; + gnss_block->maxTrkCh = 8; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->ubloxUseGzss) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + +static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) +{ + if (!capGlonass) { + return 0; + } + + gnss_block->gnssId = GNSSID_GLONASS; + gnss_block->maxTrkCh = 8; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->ubloxUseGlonass) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + + static void configureGNSS(void) { int blocksUsed = 0; @@ -507,6 +574,15 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* BeiDou */ + blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + + /* GZSS */ + blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); + + /* GLONASS */ + blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); sendConfigMessageUBLOX(); @@ -696,8 +772,14 @@ static bool gpsParceFrameUBLOX(void) for(int j = 40; j < _payload_length; j += 30) { if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) { capGalileo = true; - break; + } else if (strnstr((const char *)(_buffer.bytes+j), "BDS", 30)) { + capBeidou = true; + } else if (strnstr((const char *)(_buffer.bytes+j), "GZSS", 30)) { + capGzss = true; + } else if (strnstr((const char *)(_buffer.bytes+j), "GLO", 30)) { + capGlonass = true; } + } } } From f70cbbea333a800a1447155c8bf72d2958596a21 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 8 Jun 2023 21:38:22 +0200 Subject: [PATCH 02/36] Initial attempt, not sure it is working yet u-center does not show changes (also does not show changes without these changes) --- docs/Settings.md | 10 --- src/main/fc/settings.yaml | 5 -- src/main/io/gps.c | 1 - src/main/io/gps.h | 1 - src/main/io/gps_ublox.c | 144 +++++++++++++++++++++++++++++--------- 5 files changed, 112 insertions(+), 49 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 52183880c2d..20111d50edf 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1452,16 +1452,6 @@ Enable use of Glonass satellites. This is at the expense of other regional const --- -### gps_ublox_use_gzss - -Enable use of GZSS satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]. - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### ground_test_mode For developer ground test use. Disables motors, sets heading status = Trusted on FW. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 45a2905bae2..28b534a33a7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1542,11 +1542,6 @@ groups: default_value: OFF field: ubloxUseBeidou type: bool - - name: gps_ublox_use_gzss - description: "Enable use of GZSS satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]." - default_value: OFF - field: ubloxUseGzss - type: bool - name: gps_ublox_use_glonass description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]." default_value: OFF diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 04cf059ad9c..4f8037ee62f 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -123,7 +123,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT, .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT, .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT, - .ubloxUseGzss = SETTING_GPS_UBLOX_USE_GZSS_DEFAULT, .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT, ); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index f2885b838ad..9cc6dda0818 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -94,7 +94,6 @@ typedef struct gpsConfig_s { gpsDynModel_e dynModel; bool ubloxUseGalileo; bool ubloxUseBeidou; - bool ubloxUseGzss; bool ubloxUseGlonass; uint8_t gpsMinSats; } gpsConfig_t; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8fb15be6488..2f1433e963a 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -49,6 +49,8 @@ #include "scheduler/protothreads.h" +#include "gps_ublox.h" + #define GPS_CFG_CMD_TIMEOUT_MS 200 #define GPS_VERSION_RETRY_TIMES 2 #define MAX_UBLOX_PAYLOAD_SIZE 256 @@ -140,6 +142,13 @@ typedef struct { ubx_gnss_element_t config[0]; } ubx_gnss_msg_t; +typedef struct { + uint8_t version; + uint8_t layers; + uint8_t reserved; +} ubx_config_data_header_t; + + #define MAX_GNSS 7 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) @@ -160,6 +169,20 @@ typedef struct { uint16_t length; } ubx_header; +typedef struct { + uint32_t key; + uint8_t value; +} ubx_config_data8_payload_t; + +typedef struct { + ubx_header header; + ubx_config_data_header_t configHeader; + union { + ubx_config_data8_payload_t payload[0]; + uint8_t buffer[62]; // 12 key/value pairs + 2 checksum bytes + } data; +} __attribute__((packed)) ubx_config_data8_t; + typedef struct { ubx_header header; ubx_payload payload; @@ -458,8 +481,35 @@ static const uint8_t default_payload[] = { #define GNSSID_SBAS 1 #define GNSSID_GALILEO 2 #define GNSSID_BEIDOU 3 -#define GNSSID_GZSS 4 -#define GNSSID_GLONASS 5 +#define GNSSID_GZSS 5 +#define GNSSID_GLONASS 6 + +static void ubloxCfgValSetBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) +{ + ubx_config_data8_t cfg = { + .header.preamble1 = 0xb5, + .header.preamble2 = 0x62, + .header.msg_class = 0x06, + .header.msg_id = 0x8A, + .header.length = sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_payload_t) * count), + .configHeader.layers = 0x1, + .configHeader.reserved = 0, + .configHeader.version = 0, + }; + uint8_t ck_a, ck_b; + + for (int i = 0; i < count; ++i) { + cfg.data.payload[i].key = kvPairs[i].key; + cfg.data.payload[i].value = kvPairs[i].value; + } + + _update_checksum(&cfg.header.msg_class, sizeof(ubx_header) + sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_t) * count) - 4, &ck_a, &ck_b); + cfg.data.buffer[sizeof(ubx_config_data8_t) * count] = ck_a; + cfg.data.buffer[sizeof(ubx_config_data8_t) * count + 1] = ck_b; + serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8); + _ack_waiting_msg = cfg.header.msg_id; + _ack_state = UBX_ACK_WAITING; +} static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { @@ -485,6 +535,7 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_GALILEO; gnss_block->maxTrkCh = 8; + // E1 = 0x01 gnss_block->sigCfgMask = 1; if (gpsState.gpsConfig->ubloxUseGalileo) { gnss_block->enabled = 1; @@ -505,7 +556,9 @@ static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_BEIDOU; gnss_block->maxTrkCh = 8; - gnss_block->sigCfgMask = 1; + // B1L = 0x01 + // B2L = 0x10 + gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseBeidou) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; @@ -525,14 +578,12 @@ static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_GZSS; gnss_block->maxTrkCh = 8; - gnss_block->sigCfgMask = 1; - if (gpsState.gpsConfig->ubloxUseGzss) { - gnss_block->enabled = 1; - gnss_block->resTrkCh = 4; - } else { - gnss_block->enabled = 0; - gnss_block->resTrkCh = 0; - } + // L1C = 0x01 + // L1S = 0x04 + // L2C = 0x10 + gnss_block->sigCfgMask = 0x01 | 0x04; + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; return 1; } @@ -545,7 +596,9 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_GLONASS; gnss_block->maxTrkCh = 8; - gnss_block->sigCfgMask = 1; + // L1 = 0x01 + // L2 = 0x10 + gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseGlonass) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; @@ -560,32 +613,59 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) static void configureGNSS(void) { - int blocksUsed = 0; - - send_buffer.message.header.msg_class = CLASS_CFG; - send_buffer.message.header.msg_id = MSG_CFG_GNSS; - send_buffer.message.payload.gnss.msgVer = 0; - send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset - send_buffer.message.payload.gnss.numTrkChUse = 32; + if(gpsState.hwVersion < UBX_HW_VERSION_UBLOX9) { + int blocksUsed = 0; + send_buffer.message.header.msg_class = CLASS_CFG; + send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET + send_buffer.message.payload.gnss.msgVer = 0; + send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset + send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max - /* SBAS, always generated */ - blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* SBAS, always generated */ + blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* Galileo */ - blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* Galileo */ + blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* BeiDou */ - blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* BeiDou */ + blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* GZSS */ - blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* GZSS should be enabled when GPS is enabled */ + blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* GLONASS */ - blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* GLONASS */ + blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); - send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; - send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); - sendConfigMessageUBLOX(); + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; + send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed); + sendConfigMessageUBLOX(); + } else { + ubx_config_data8_payload_t gnssConfigValues[] = { + // SBAS + {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, + + // Galileo + {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + + // Beidou + {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, + + // Should be enabled with GPS + {UBLOX_CFG_QZSS_ENA, 1}, + {UBLOX_CFG_QZSS_L1CA_ENA, 1}, + {UBLOX_CFG_QZSS_L1S_ENA, 1}, + + // Glonass + {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, + {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} + }; + + ubloxCfgValSetBytes(gnssConfigValues, 12); + } } static void configureNAV5(uint8_t dynModel, uint8_t fixMode) From 73012cf1cf2385db451c0a7c1fdfb209ca1d5ab2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 8 Jun 2023 21:51:48 +0200 Subject: [PATCH 03/36] Move type declarations to header file --- src/main/io/gps_ublox.c | 292 ----------------------------------- src/main/io/gps_ublox.h | 331 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 331 insertions(+), 292 deletions(-) create mode 100644 src/main/io/gps_ublox.h diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 2f1433e963a..f75e974b837 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -51,31 +51,6 @@ #include "gps_ublox.h" -#define GPS_CFG_CMD_TIMEOUT_MS 200 -#define GPS_VERSION_RETRY_TIMES 2 -#define MAX_UBLOX_PAYLOAD_SIZE 256 -#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE -#define UBLOX_SBAS_MESSAGE_LENGTH 16 - -#define UBX_DYNMODEL_PEDESTRIAN 3 -#define UBX_DYNMODEL_AIR_1G 6 -#define UBX_DYNMODEL_AIR_4G 8 - -#define UBX_FIXMODE_2D_ONLY 1 -#define UBX_FIXMODE_3D_ONLY 2 -#define UBX_FIXMODE_AUTO 3 - -#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0) -#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) -#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) - -#define UBX_HW_VERSION_UNKNOWN 0 -#define UBX_HW_VERSION_UBLOX5 500 -#define UBX_HW_VERSION_UBLOX6 600 -#define UBX_HW_VERSION_UBLOX7 700 -#define UBX_HW_VERSION_UBLOX8 800 -#define UBX_HW_VERSION_UBLOX9 900 -#define UBX_HW_VERSION_UBLOX10 1000 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE // note PRNs last upadted 2020-12-18 @@ -101,273 +76,6 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 }; -// payload types -typedef struct { - uint8_t mode; - uint8_t usage; - uint8_t maxSBAS; - uint8_t scanmode2; - uint32_t scanmode1; -} ubx_sbas; - -typedef struct { - uint8_t class; - uint8_t id; - uint8_t rate; -} ubx_msg; - -typedef struct { - uint16_t meas; - uint16_t nav; - uint16_t time; -} ubx_rate; - -typedef struct { - uint8_t gnssId; - uint8_t resTrkCh; - uint8_t maxTrkCh; - uint8_t reserved1; -// flags - uint8_t enabled; - uint8_t undefined0; - uint8_t sigCfgMask; - uint8_t undefined1; -} ubx_gnss_element_t; - -typedef struct { - uint8_t msgVer; - uint8_t numTrkChHw; - uint8_t numTrkChUse; - uint8_t numConfigBlocks; - ubx_gnss_element_t config[0]; -} ubx_gnss_msg_t; - -typedef struct { - uint8_t version; - uint8_t layers; - uint8_t reserved; -} ubx_config_data_header_t; - - -#define MAX_GNSS 7 -#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) - -typedef union { - uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder - ubx_sbas sbas; - ubx_msg msg; - ubx_rate rate; - ubx_gnss_msg_t gnss; -} ubx_payload; - -// UBX support -typedef struct { - uint8_t preamble1; - uint8_t preamble2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -} ubx_header; - -typedef struct { - uint32_t key; - uint8_t value; -} ubx_config_data8_payload_t; - -typedef struct { - ubx_header header; - ubx_config_data_header_t configHeader; - union { - ubx_config_data8_payload_t payload[0]; - uint8_t buffer[62]; // 12 key/value pairs + 2 checksum bytes - } data; -} __attribute__((packed)) ubx_config_data8_t; - -typedef struct { - ubx_header header; - ubx_payload payload; -} __attribute__((packed)) ubx_message; - -typedef struct { - char swVersion[30]; // Zero-terminated Software Version String - char hwVersion[10]; // Zero-terminated Hardware Version String -} ubx_mon_ver; - -typedef struct { - uint32_t time; // GPS msToW - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; -} ubx_nav_posllh; - -typedef struct { - uint32_t time; // GPS msToW - uint8_t fix_type; - uint8_t fix_status; - uint8_t differential_status; - uint8_t res; - uint32_t time_to_first_fix; - uint32_t uptime; // milliseconds -} ubx_nav_status; - -typedef struct { - uint32_t time; - int32_t time_nsec; - int16_t week; - uint8_t fix_type; - uint8_t fix_status; - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - uint32_t position_accuracy_3d; - int32_t ecef_x_velocity; - int32_t ecef_y_velocity; - int32_t ecef_z_velocity; - uint32_t speed_accuracy; - uint16_t position_DOP; - uint8_t res; - uint8_t satellites; - uint32_t res2; -} ubx_nav_solution; - -typedef struct { - uint32_t time; // GPS msToW - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - uint32_t speed_3d; - uint32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; -} ubx_nav_velned; - -typedef struct { - uint8_t chn; // Channel number, 255 for SVx not assigned to channel - uint8_t svid; // Satellite ID - uint8_t flags; // Bitmask - uint8_t quality; // Bitfield - uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55. - uint8_t elev; // Elevation in integer degrees - int16_t azim; // Azimuth in integer degrees - int32_t prRes; // Pseudo range residual in centimetres -} ubx_nav_svinfo_channel; - -typedef struct { - uint32_t time; // GPS Millisecond time of week - 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 - ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte -} ubx_nav_svinfo; - -typedef struct { - uint32_t time; // GPS msToW - uint32_t tAcc; - int32_t nano; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; -} ubx_nav_timeutc; - -typedef struct { - uint32_t time; // GPS msToW - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - uint8_t valid; - uint32_t tAcc; - int32_t nano; - uint8_t fix_type; - uint8_t fix_status; - uint8_t reserved1; - uint8_t satellites; - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - int32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - uint16_t position_DOP; - uint16_t reserved2; - uint16_t reserved3; -} ubx_nav_pvt; - -typedef struct { - uint8_t class; - uint8_t msg; -} ubx_ack_ack; - -enum { - PREAMBLE1 = 0xB5, - PREAMBLE2 = 0x62, - CLASS_NAV = 0x01, - CLASS_ACK = 0x05, - CLASS_CFG = 0x06, - CLASS_MON = 0x0A, - MSG_CLASS_UBX = 0x01, - MSG_CLASS_NMEA = 0xF0, - MSG_VER = 0x04, - MSG_ACK_NACK = 0x00, - MSG_ACK_ACK = 0x01, - MSG_NMEA_GGA = 0x0, - MSG_NMEA_GLL = 0x1, - MSG_NMEA_GSA = 0x2, - MSG_NMEA_GSV = 0x3, - MSG_NMEA_RMC = 0x4, - MSG_NMEA_VGS = 0x5, - MSG_POSLLH = 0x2, - MSG_STATUS = 0x3, - MSG_SOL = 0x6, - MSG_PVT = 0x7, - MSG_VELNED = 0x12, - MSG_TIMEUTC = 0x21, - MSG_SVINFO = 0x30, - MSG_NAV_SAT = 0x35, - MSG_NAV_SIG = 0x35, - MSG_CFG_PRT = 0x00, - MSG_CFG_RATE = 0x08, - MSG_CFG_SET_RATE = 0x01, - MSG_CFG_NAV_SETTINGS = 0x24, - MSG_CFG_SBAS = 0x16, - MSG_CFG_GNSS = 0x3e -} ubx_protocol_bytes; - -enum { - FIX_NONE = 0, - FIX_DEAD_RECKONING = 1, - FIX_2D = 2, - FIX_3D = 3, - FIX_GPS_DEAD_RECKONING = 4, - FIX_TIME = 5 -} ubs_nav_fix_type; - -enum { - NAV_STATUS_FIX_VALID = 1 -} ubx_nav_status_bits; - -enum { - UBX_ACK_WAITING = 0, - UBX_ACK_GOT_ACK = 1, - UBX_ACK_GOT_NAK = 2 -} ubx_ack_state; // Packet checksum accumulators static uint8_t _ck_a; diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h new file mode 100644 index 00000000000..22ee004c66f --- /dev/null +++ b/src/main/io/gps_ublox.h @@ -0,0 +1,331 @@ +/* + * This file is part of INAV + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define GPS_CFG_CMD_TIMEOUT_MS 200 +#define GPS_VERSION_RETRY_TIMES 2 +#define MAX_UBLOX_PAYLOAD_SIZE 256 +#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE +#define UBLOX_SBAS_MESSAGE_LENGTH 16 + +#define UBX_DYNMODEL_PEDESTRIAN 3 +#define UBX_DYNMODEL_AIR_1G 6 +#define UBX_DYNMODEL_AIR_4G 8 + +#define UBX_FIXMODE_2D_ONLY 1 +#define UBX_FIXMODE_3D_ONLY 2 +#define UBX_FIXMODE_AUTO 3 + +#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0) +#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) +#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) + +#define UBX_HW_VERSION_UNKNOWN 0 +#define UBX_HW_VERSION_UBLOX5 500 +#define UBX_HW_VERSION_UBLOX6 600 +#define UBX_HW_VERSION_UBLOX7 700 +#define UBX_HW_VERSION_UBLOX8 800 +#define UBX_HW_VERSION_UBLOX9 900 +#define UBX_HW_VERSION_UBLOX10 1000 + + +#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 +#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 + +#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 +#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 + +#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 +#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d +#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // default off + +#define UBLOX_CFG_QZSS_ENA 0x10310024 +#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 +#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 + +#define UBLOX_CFG_GLO_ENA 0x10310025 +#define UBLOX_CFG_GLO_L1_ENA 0x10310018 + +// payload types +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_sbas; + +typedef struct { + uint8_t class; + uint8_t id; + uint8_t rate; +} ubx_msg; + +typedef struct { + uint16_t meas; + uint16_t nav; + uint16_t time; +} ubx_rate; + +typedef struct { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t reserved1; +// flags + uint8_t enabled; + uint8_t undefined0; + uint8_t sigCfgMask; + uint8_t undefined1; +} ubx_gnss_element_t; + +typedef struct { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + ubx_gnss_element_t config[0]; +} ubx_gnss_msg_t; + +typedef struct { + uint8_t version; + uint8_t layers; + uint8_t reserved; +} ubx_config_data_header_t; + + +#define MAX_GNSS 7 +#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) + +typedef union { + uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder + ubx_sbas sbas; + ubx_msg msg; + ubx_rate rate; + ubx_gnss_msg_t gnss; +} ubx_payload; + +// UBX support +typedef struct { + uint8_t preamble1; + uint8_t preamble2; + uint8_t msg_class; + uint8_t msg_id; + uint16_t length; +} ubx_header; + +typedef struct { + uint32_t key; + uint8_t value; +} ubx_config_data8_payload_t; + +typedef struct { + ubx_header header; + ubx_config_data_header_t configHeader; + union { + ubx_config_data8_payload_t payload[0]; + uint8_t buffer[62]; // 12 key/value pairs + 2 checksum bytes + } data; +} __attribute__((packed)) ubx_config_data8_t; + +typedef struct { + ubx_header header; + ubx_payload payload; +} __attribute__((packed)) ubx_message; + +typedef struct { + char swVersion[30]; // Zero-terminated Software Version String + char hwVersion[10]; // Zero-terminated Hardware Version String +} ubx_mon_ver; + +typedef struct { + uint32_t time; // GPS msToW + int32_t longitude; + int32_t latitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; +} ubx_nav_posllh; + +typedef struct { + uint32_t time; // GPS msToW + uint8_t fix_type; + uint8_t fix_status; + uint8_t differential_status; + uint8_t res; + uint32_t time_to_first_fix; + uint32_t uptime; // milliseconds +} ubx_nav_status; + +typedef struct { + uint32_t time; + int32_t time_nsec; + int16_t week; + uint8_t fix_type; + uint8_t fix_status; + int32_t ecef_x; + int32_t ecef_y; + int32_t ecef_z; + uint32_t position_accuracy_3d; + int32_t ecef_x_velocity; + int32_t ecef_y_velocity; + int32_t ecef_z_velocity; + uint32_t speed_accuracy; + uint16_t position_DOP; + uint8_t res; + uint8_t satellites; + uint32_t res2; +} ubx_nav_solution; + +typedef struct { + uint32_t time; // GPS msToW + int32_t ned_north; + int32_t ned_east; + int32_t ned_down; + uint32_t speed_3d; + uint32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; +} ubx_nav_velned; + +typedef struct { + uint8_t chn; // Channel number, 255 for SVx not assigned to channel + uint8_t svid; // Satellite ID + uint8_t flags; // Bitmask + uint8_t quality; // Bitfield + uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55. + uint8_t elev; // Elevation in integer degrees + int16_t azim; // Azimuth in integer degrees + int32_t prRes; // Pseudo range residual in centimetres +} ubx_nav_svinfo_channel; + +typedef struct { + uint32_t time; // GPS Millisecond time of week + 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 + ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte +} ubx_nav_svinfo; + +typedef struct { + uint32_t time; // GPS msToW + uint32_t tAcc; + int32_t nano; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +} ubx_nav_timeutc; + +typedef struct { + uint32_t time; // GPS msToW + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; + uint32_t tAcc; + int32_t nano; + uint8_t fix_type; + uint8_t fix_status; + uint8_t reserved1; + uint8_t satellites; + int32_t longitude; + int32_t latitude; + int32_t altitude_ellipsoid; + int32_t altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; + int32_t ned_north; + int32_t ned_east; + int32_t ned_down; + int32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + uint32_t heading_accuracy; + uint16_t position_DOP; + uint16_t reserved2; + uint16_t reserved3; +} ubx_nav_pvt; + +typedef struct { + uint8_t class; + uint8_t msg; +} ubx_ack_ack; + +enum { + PREAMBLE1 = 0xB5, + PREAMBLE2 = 0x62, + CLASS_NAV = 0x01, + CLASS_ACK = 0x05, + CLASS_CFG = 0x06, + CLASS_MON = 0x0A, + MSG_CLASS_UBX = 0x01, + MSG_CLASS_NMEA = 0xF0, + MSG_VER = 0x04, + MSG_ACK_NACK = 0x00, + MSG_ACK_ACK = 0x01, + MSG_NMEA_GGA = 0x0, + MSG_NMEA_GLL = 0x1, + MSG_NMEA_GSA = 0x2, + MSG_NMEA_GSV = 0x3, + MSG_NMEA_RMC = 0x4, + MSG_NMEA_VGS = 0x5, + MSG_POSLLH = 0x2, + MSG_STATUS = 0x3, + MSG_SOL = 0x6, + MSG_PVT = 0x7, + MSG_VELNED = 0x12, + MSG_TIMEUTC = 0x21, + MSG_SVINFO = 0x30, + MSG_NAV_SAT = 0x35, + MSG_NAV_SIG = 0x35, + MSG_CFG_PRT = 0x00, + MSG_CFG_RATE = 0x08, + MSG_CFG_SET_RATE = 0x01, + MSG_CFG_NAV_SETTINGS = 0x24, + MSG_CFG_SBAS = 0x16, + MSG_CFG_GNSS = 0x3e +} ubx_protocol_bytes; + +enum { + FIX_NONE = 0, + FIX_DEAD_RECKONING = 1, + FIX_2D = 2, + FIX_3D = 3, + FIX_GPS_DEAD_RECKONING = 4, + FIX_TIME = 5 +} ubs_nav_fix_type; + +enum { + NAV_STATUS_FIX_VALID = 1 +} ubx_nav_status_bits; + +enum { + UBX_ACK_WAITING = 0, + UBX_ACK_GOT_ACK = 1, + UBX_ACK_GOT_NAK = 2 +} ubx_ack_state; + From b242488c8b315d10f55c5764790624176db061fc Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 8 Jun 2023 23:28:41 +0200 Subject: [PATCH 04/36] Changes to avoid issues in c++ unit tests --- src/main/io/gps_ublox.c | 45 +++++++++++++++++------------ src/main/io/gps_ublox.h | 15 ++++++++-- src/test/unit/CMakeLists.txt | 3 ++ src/test/unit/gps_ublox_unittest.cc | 25 ++++++++++++++++ 4 files changed, 68 insertions(+), 20 deletions(-) create mode 100644 src/test/unit/gps_ublox_unittest.cc diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index f75e974b837..cf1b29cea01 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -192,28 +192,37 @@ static const uint8_t default_payload[] = { #define GNSSID_GZSS 5 #define GNSSID_GLONASS 6 -static void ubloxCfgValSetBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) + +void ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count) { - ubx_config_data8_t cfg = { - .header.preamble1 = 0xb5, - .header.preamble2 = 0x62, - .header.msg_class = 0x06, - .header.msg_id = 0x8A, - .header.length = sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_payload_t) * count), - .configHeader.layers = 0x1, - .configHeader.reserved = 0, - .configHeader.version = 0, - }; uint8_t ck_a, ck_b; + cfg->header.preamble1 = 0xb5; + cfg->header.preamble2 = 0x62; + cfg->header.msg_class = 0x06; + cfg->header.msg_id = 0x8A; + cfg->header.length = sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_payload_t) * count); + cfg->configHeader.layers = 0x1; + cfg->configHeader.reserved = 0; + cfg->configHeader.version = 0; + for (int i = 0; i < count; ++i) { - cfg.data.payload[i].key = kvPairs[i].key; - cfg.data.payload[i].value = kvPairs[i].value; + cfg->data.payload[i].key = kvPairs[i].key; + cfg->data.payload[i].value = kvPairs[i].value; } - _update_checksum(&cfg.header.msg_class, sizeof(ubx_header) + sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_t) * count) - 4, &ck_a, &ck_b); - cfg.data.buffer[sizeof(ubx_config_data8_t) * count] = ck_a; - cfg.data.buffer[sizeof(ubx_config_data8_t) * count + 1] = ck_b; + _update_checksum(&cfg->header.msg_class, sizeof(ubx_header) + sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_t) * count) - 4, &ck_a, &ck_b); + cfg->data.buffer[sizeof(ubx_config_data8_t) * count] = ck_a; + cfg->data.buffer[sizeof(ubx_config_data8_t) * count + 1] = ck_b; +} + +// ublox info: https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf +static void ubloxCfgValSetBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) +{ + ubx_config_data8_t cfg = {}; + + ubloxCfgFillBytes(&cfg, kvPairs, count); + serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8); _ack_waiting_msg = cfg.header.msg_id; _ack_state = UBX_ACK_WAITING; @@ -387,12 +396,12 @@ static void configureNAV5(uint8_t dynModel, uint8_t fixMode) sendConfigMessageUBLOX(); } -static void configureMSG(uint8_t class, uint8_t id, uint8_t rate) +static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate) { send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_SET_RATE; send_buffer.message.header.length = 3; - send_buffer.message.payload.msg.class = class; + send_buffer.message.payload.msg.msg_class = msg_class; send_buffer.message.payload.msg.id = id; send_buffer.message.payload.msg.rate = rate; sendConfigMessageUBLOX(); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 22ee004c66f..420bff6621f 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -17,6 +17,12 @@ #pragma once +#include + +#ifdef __cplusplus +extern "C" { +#endif + #define GPS_CFG_CMD_TIMEOUT_MS 200 #define GPS_VERSION_RETRY_TIMES 2 #define MAX_UBLOX_PAYLOAD_SIZE 256 @@ -71,7 +77,7 @@ typedef struct { } ubx_sbas; typedef struct { - uint8_t class; + uint8_t msg_class; uint8_t id; uint8_t rate; } ubx_msg; @@ -271,7 +277,7 @@ typedef struct { } ubx_nav_pvt; typedef struct { - uint8_t class; + uint8_t msg_class; uint8_t msg; } ubx_ack_ack; @@ -329,3 +335,8 @@ enum { UBX_ACK_GOT_NAK = 2 } ubx_ack_state; +void ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); + +#ifdef __cplusplus +} +#endif diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index abb9389fbb3..e51b43fc32a 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -37,6 +37,9 @@ set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displayport_msp_osd.c" "common/typeconversion.c") set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT) +set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox.c") +set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST USE_GPS USE_GPS_PROTO_UBLOX) + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc new file mode 100644 index 00000000000..8be18c05158 --- /dev/null +++ b/src/test/unit/gps_ublox_unittest.cc @@ -0,0 +1,25 @@ +#include "gtest/gtest.h" +#include "unittest_macros.h" + +#include +#include + +extern "C" { +#include "io/gps_ublox.h" +}; + + +TEST(GPSUbloxTest, TestUbloxCfgFillBytes) +{ + ubx_config_data8_t cfg = {}; + ubx_config_data8_payload_t kvPairs = { + { 0x10, 0x1}, + { 0x42, 0x69} + }; + + ubloxCfgFillBytes(&cfg, &kvPairs, 2); + + // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); + // std::cout << "'" << buf << "'" << std::endl; + //EXPECT_FALSE(strcmp(buf, " 123.45")); +} \ No newline at end of file From 96a86443e3993d0bb0f624355fd33b62a98d9de8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 00:02:20 +0200 Subject: [PATCH 05/36] split gps_ublox to facilitate unit tests --- src/main/CMakeLists.txt | 1 + src/main/io/gps_ublox.c | 39 +++++++---------------------- src/main/io/gps_ublox.h | 25 +++++++++--------- src/test/unit/CMakeLists.txt | 2 +- src/test/unit/gps_ublox_unittest.cc | 9 +++---- 5 files changed, 27 insertions(+), 49 deletions(-) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 2a311443544..e67ea9629db 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -499,6 +499,7 @@ main_sources(COMMON_SRC io/gps.c io/gps.h io/gps_ublox.c + io/gps_ublox_utils.c io/gps_nmea.c io/gps_msp.c io/gps_fake.c diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index cf1b29cea01..df5e2c70bf4 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -50,6 +50,7 @@ #include "scheduler/protothreads.h" #include "gps_ublox.h" +#include "gps_ublox_utils.h" // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE @@ -76,6 +77,13 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 }; +ubx_ack_state_t ubx_ack_state; + +ubx_protocol_bytes_t ubx_protocol_bytes; + +ubs_nav_fix_type_t ubs_nav_fix_type; + +ubx_nav_status_bits_t ubx_nav_status_bits; // Packet checksum accumulators static uint8_t _ck_a; @@ -138,14 +146,6 @@ static union { uint8_t bytes[UBLOX_BUFFER_SIZE]; } _buffer; -void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) -{ - while (len--) { - *ck_a += *data; - *ck_b += *ck_a; - data++; - } -} static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) { @@ -161,7 +161,7 @@ static void sendConfigMessageUBLOX(void) uint8_t ck_a=0, ck_b=0; send_buffer.message.header.preamble1=PREAMBLE1; send_buffer.message.header.preamble2=PREAMBLE2; - _update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b); + ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b); send_buffer.bytes[send_buffer.message.header.length+6] = ck_a; send_buffer.bytes[send_buffer.message.header.length+7] = ck_b; serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8); @@ -193,28 +193,7 @@ static const uint8_t default_payload[] = { #define GNSSID_GLONASS 6 -void ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count) -{ - uint8_t ck_a, ck_b; - - cfg->header.preamble1 = 0xb5; - cfg->header.preamble2 = 0x62; - cfg->header.msg_class = 0x06; - cfg->header.msg_id = 0x8A; - cfg->header.length = sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_payload_t) * count); - cfg->configHeader.layers = 0x1; - cfg->configHeader.reserved = 0; - cfg->configHeader.version = 0; - - for (int i = 0; i < count; ++i) { - cfg->data.payload[i].key = kvPairs[i].key; - cfg->data.payload[i].value = kvPairs[i].value; - } - _update_checksum(&cfg->header.msg_class, sizeof(ubx_header) + sizeof(ubx_config_data_header_t) + (sizeof(ubx_config_data8_t) * count) - 4, &ck_a, &ck_b); - cfg->data.buffer[sizeof(ubx_config_data8_t) * count] = ck_a; - cfg->data.buffer[sizeof(ubx_config_data8_t) * count + 1] = ck_b; -} // ublox info: https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf static void ubloxCfgValSetBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 420bff6621f..cbacf0ef6b2 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -281,7 +281,13 @@ typedef struct { uint8_t msg; } ubx_ack_ack; -enum { +typedef enum { + UBX_ACK_WAITING = 0, + UBX_ACK_GOT_ACK = 1, + UBX_ACK_GOT_NAK = 2 +} ubx_ack_state_t; + +typedef enum { PREAMBLE1 = 0xB5, PREAMBLE2 = 0x62, CLASS_NAV = 0x01, @@ -314,28 +320,23 @@ enum { MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_SBAS = 0x16, MSG_CFG_GNSS = 0x3e -} ubx_protocol_bytes; +} ubx_protocol_bytes_t; -enum { +typedef enum { FIX_NONE = 0, FIX_DEAD_RECKONING = 1, FIX_2D = 2, FIX_3D = 3, FIX_GPS_DEAD_RECKONING = 4, FIX_TIME = 5 -} ubs_nav_fix_type; +} ubs_nav_fix_type_t; -enum { +typedef enum { NAV_STATUS_FIX_VALID = 1 -} ubx_nav_status_bits; +} ubx_nav_status_bits_t; + -enum { - UBX_ACK_WAITING = 0, - UBX_ACK_GOT_ACK = 1, - UBX_ACK_GOT_NAK = 2 -} ubx_ack_state; -void ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); #ifdef __cplusplus } diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index e51b43fc32a..6b52a8a6566 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -37,7 +37,7 @@ set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displayport_msp_osd.c" "common/typeconversion.c") set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT) -set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox.c") +set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c") set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST USE_GPS USE_GPS_PROTO_UBLOX) function(unit_test src) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 8be18c05158..f34091008e1 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -4,20 +4,17 @@ #include #include -extern "C" { -#include "io/gps_ublox.h" -}; - +#include "io/gps_ublox_utils.h" TEST(GPSUbloxTest, TestUbloxCfgFillBytes) { ubx_config_data8_t cfg = {}; - ubx_config_data8_payload_t kvPairs = { + ubx_config_data8_payload_t kvPairs[] = { { 0x10, 0x1}, { 0x42, 0x69} }; - ubloxCfgFillBytes(&cfg, &kvPairs, 2); + ubloxCfgFillBytes(&cfg, kvPairs, 2); // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); // std::cout << "'" << buf << "'" << std::endl; From 7cec2be023c2250fa0c2cf68e002aca46328dd1f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 00:10:25 +0200 Subject: [PATCH 06/36] Cleanup --- src/main/io/gps_ublox.c | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index df5e2c70bf4..88a445fe204 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -77,14 +77,6 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 }; -ubx_ack_state_t ubx_ack_state; - -ubx_protocol_bytes_t ubx_protocol_bytes; - -ubs_nav_fix_type_t ubs_nav_fix_type; - -ubx_nav_status_bits_t ubx_nav_status_bits; - // Packet checksum accumulators static uint8_t _ck_a; static uint8_t _ck_b; From f984f055192f566a05e3c0c11771cfaa4c13b98a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 00:33:40 +0200 Subject: [PATCH 07/36] small changes to unit test --- src/test/unit/gps_ublox_unittest.cc | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index f34091008e1..c3c08d5c85c 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -3,6 +3,7 @@ #include #include +#include #include "io/gps_ublox_utils.h" @@ -10,12 +11,24 @@ TEST(GPSUbloxTest, TestUbloxCfgFillBytes) { ubx_config_data8_t cfg = {}; ubx_config_data8_payload_t kvPairs[] = { - { 0x10, 0x1}, + { 0x04, 0x20}, { 0x42, 0x69} }; ubloxCfgFillBytes(&cfg, kvPairs, 2); + printf("%02x %02x %02x %02x %04x\n", cfg.header.preamble1, cfg.header.preamble2, cfg.header.msg_class, cfg.header.msg_id, cfg.header.length); + + printf("%02x %02x %02x\n", cfg.configHeader.version, cfg.configHeader.layers, cfg.configHeader.reserved); + + for(int i =0; i < 2; ++i) { + printf("%08x %02x\n", cfg.data.payload[i].key, cfg.data.payload[i].value); + } + + uint8_t *chksums = (uint8_t *)&cfg.data.payload[2]; + + printf("%02x %02x\n", chksums[0], chksums[1]); + // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); // std::cout << "'" << buf << "'" << std::endl; //EXPECT_FALSE(strcmp(buf, " 123.45")); From 94e7c079973008591439ebe120945de8395e1054 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 00:51:02 +0200 Subject: [PATCH 08/36] Add first assert --- src/main/io/gps_ublox.h | 5 ++++- src/test/unit/gps_ublox_unittest.cc | 20 ++++++++++++++++---- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index cbacf0ef6b2..3bc33cf41e9 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -140,12 +140,15 @@ typedef struct { uint8_t value; } ubx_config_data8_payload_t; + +#define MAX_CONFIG_SET_VAL_VALUES 32 + typedef struct { ubx_header header; ubx_config_data_header_t configHeader; union { ubx_config_data8_payload_t payload[0]; - uint8_t buffer[62]; // 12 key/value pairs + 2 checksum bytes + uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes } data; } __attribute__((packed)) ubx_config_data8_t; diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index c3c08d5c85c..3d1417d9790 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -11,21 +11,33 @@ TEST(GPSUbloxTest, TestUbloxCfgFillBytes) { ubx_config_data8_t cfg = {}; ubx_config_data8_payload_t kvPairs[] = { + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, + { 0x04, 0x20}, + { 0x42, 0x69}, { 0x04, 0x20}, { 0x42, 0x69} }; - ubloxCfgFillBytes(&cfg, kvPairs, 2); + int valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 12); + + EXPECT_TRUE(valuesAdded == 12); printf("%02x %02x %02x %02x %04x\n", cfg.header.preamble1, cfg.header.preamble2, cfg.header.msg_class, cfg.header.msg_id, cfg.header.length); printf("%02x %02x %02x\n", cfg.configHeader.version, cfg.configHeader.layers, cfg.configHeader.reserved); - for(int i =0; i < 2; ++i) { - printf("%08x %02x\n", cfg.data.payload[i].key, cfg.data.payload[i].value); + for(int i =0; i < valuesAdded; ++i) { + printf("%i: %08x %02x\n", i+1, cfg.data.payload[i].key, cfg.data.payload[i].value); } - uint8_t *chksums = (uint8_t *)&cfg.data.payload[2]; + uint8_t *chksums = (uint8_t *)&cfg.data.payload[valuesAdded]; printf("%02x %02x\n", chksums[0], chksums[1]); From 0ddcadb30324770d715662f4d05b45ebc058bafd Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 12:22:29 +0200 Subject: [PATCH 09/36] Passing check agains packet generated in u-center2 --- src/main/io/gps_ublox.h | 5 ++-- src/test/unit/gps_ublox_unittest.cc | 43 +++++++++++++++++++++++------ 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 3bc33cf41e9..720a2e507d2 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -111,8 +111,9 @@ typedef struct { typedef struct { uint8_t version; uint8_t layers; + uint8_t transation; uint8_t reserved; -} ubx_config_data_header_t; +} __attribute__((packed)) ubx_config_data_header_t; #define MAX_GNSS 7 @@ -138,7 +139,7 @@ typedef struct { typedef struct { uint32_t key; uint8_t value; -} ubx_config_data8_payload_t; +} __attribute__((packed)) ubx_config_data8_payload_t; #define MAX_CONFIG_SET_VAL_VALUES 32 diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 3d1417d9790..25dffa704b4 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -7,11 +7,34 @@ #include "io/gps_ublox_utils.h" +void dumpCfg(const ubx_config_data8_t *cfg, int valuesAdded) +{ + printf("%02x %02x %02x %02x %04x\n", cfg->header.preamble1, cfg->header.preamble2, cfg->header.msg_class, cfg->header.msg_id, cfg->header.length); + + printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transation, cfg->configHeader.reserved); + + for(int i =0; i < valuesAdded; ++i) { + printf("%i: %08x %02x\n", i+1, cfg->data.payload[i].key, cfg->data.payload[i].value); + } + + uint8_t *chksums = (uint8_t *)&cfg->data.payload[valuesAdded]; + + printf("%02x %02x\n", chksums[0], chksums[1]); +} + +void dumpMemory(uint8_t *mem, int size) +{ + for(int i =0; i < size; ++i) { + printf("%02x ", mem[i]); + } + printf("\n"); +} + TEST(GPSUbloxTest, TestUbloxCfgFillBytes) { ubx_config_data8_t cfg = {}; ubx_config_data8_payload_t kvPairs[] = { - { 0x04, 0x20}, + { 0x10310025, 0x1}, { 0x42, 0x69}, { 0x04, 0x20}, { 0x42, 0x69}, @@ -29,17 +52,19 @@ TEST(GPSUbloxTest, TestUbloxCfgFillBytes) EXPECT_TRUE(valuesAdded == 12); - printf("%02x %02x %02x %02x %04x\n", cfg.header.preamble1, cfg.header.preamble2, cfg.header.msg_class, cfg.header.msg_id, cfg.header.length); + dumpCfg(&cfg, valuesAdded); - printf("%02x %02x %02x\n", cfg.configHeader.version, cfg.configHeader.layers, cfg.configHeader.reserved); + valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 1); + EXPECT_TRUE(1 == valuesAdded); - for(int i =0; i < valuesAdded; ++i) { - printf("%i: %08x %02x\n", i+1, cfg.data.payload[i].key, cfg.data.payload[i].value); - } + // Set glonass enabled, from u-center 2 + const uint8_t expected[] = {0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x25, 0x00, 0x31, 0x10, 0x01, 0x02, 0xA7}; + EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17)); - uint8_t *chksums = (uint8_t *)&cfg.data.payload[valuesAdded]; - - printf("%02x %02x\n", chksums[0], chksums[1]); + printf("Expected:\n"); + dumpMemory((uint8_t *)expected, 17); + printf("Actual:\n"); + dumpMemory((uint8_t *)&cfg, 17); // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); // std::cout << "'" << buf << "'" << std::endl; From f87f657a8b91a0d5af805e6e5ea86dc183ec1453 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 12:23:53 +0200 Subject: [PATCH 10/36] Add missing files. Split out from main ublox file for easier unit testing --- src/main/io/gps_ublox_utils.c | 44 +++++++++++++++++++++++++++++++++++ src/main/io/gps_ublox_utils.h | 34 +++++++++++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 src/main/io/gps_ublox_utils.c create mode 100644 src/main/io/gps_ublox_utils.h diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c new file mode 100644 index 00000000000..e1025764060 --- /dev/null +++ b/src/main/io/gps_ublox_utils.c @@ -0,0 +1,44 @@ +#include +#include + +#include "gps_ublox_utils.h" + +void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) +{ + while (len--) { + *ck_a += *data; + *ck_b += *ck_a; + data++; + } +} + +int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count) +{ + if (count > MAX_CONFIG_SET_VAL_VALUES) + count = MAX_CONFIG_SET_VAL_VALUES; + + cfg->header.preamble1 = 0xb5; + cfg->header.preamble2 = 0x62; + cfg->header.msg_class = 0x06; + cfg->header.msg_id = 0x8A; + cfg->header.length = sizeof(ubx_config_data_header_t) + ((sizeof(ubx_config_data8_payload_t) * count)); + cfg->configHeader.layers = 0x1; + cfg->configHeader.transation = 0; + cfg->configHeader.reserved = 0; + cfg->configHeader.version = 1; + + + for (int i = 0; i < count; ++i) { + cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key); + cfg->data.payload[i].value = kvPairs[i].value; + } + + uint8_t *buf = (uint8_t *)cfg; + uint8_t ck_a, ck_b; + ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b); + buf[cfg->header.length + 6] = ck_a; + buf[cfg->header.length + 7] = ck_b; + + return count; +} + diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h new file mode 100644 index 00000000000..e7ae0caac90 --- /dev/null +++ b/src/main/io/gps_ublox_utils.h @@ -0,0 +1,34 @@ +/* + * This file is part of INAV + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +#include "gps_ublox.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); + +void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b); + +#ifdef __cplusplus +} +#endif From 9512600aea9515cf5f50d848e7aabb2f777c05d8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 12:26:48 +0200 Subject: [PATCH 11/36] remove header that is not needed --- src/main/io/gps_ublox_utils.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index e1025764060..3d53d255bf5 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -1,5 +1,4 @@ #include -#include #include "gps_ublox_utils.h" From 2cdba8350016d57f7e69bbbb602e7afb0afab059 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 9 Jun 2023 16:44:37 +0200 Subject: [PATCH 12/36] Debugging changes --- src/main/io/gps_ublox.c | 25 +++++++++++++------------ src/main/io/gps_ublox.h | 12 +++++++++--- src/main/io/gps_ublox_utils.c | 4 ++-- 3 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 88a445fe204..9ec0cab77ca 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -330,29 +330,30 @@ static void configureGNSS(void) } else { ubx_config_data8_payload_t gnssConfigValues[] = { // SBAS - {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, - {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, + //{UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, + //{UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, // Galileo {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, - {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + //{UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, // Beidou - {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, + //{UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + //{UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + //{UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, // Should be enabled with GPS - {UBLOX_CFG_QZSS_ENA, 1}, - {UBLOX_CFG_QZSS_L1CA_ENA, 1}, - {UBLOX_CFG_QZSS_L1S_ENA, 1}, + //{UBLOX_CFG_QZSS_ENA, 1}, + //{UBLOX_CFG_QZSS_L1CA_ENA, 1}, + //{UBLOX_CFG_QZSS_L1S_ENA, 1}, // Glonass - {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, - {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} + //{UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, + //{UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} }; - ubloxCfgValSetBytes(gnssConfigValues, 12); + //ubloxCfgValSetBytes(gnssConfigValues, 12); + ubloxCfgValSetBytes(gnssConfigValues, 1); } } diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 720a2e507d2..b49029842d4 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -111,9 +111,15 @@ typedef struct { typedef struct { uint8_t version; uint8_t layers; - uint8_t transation; uint8_t reserved; -} __attribute__((packed)) ubx_config_data_header_t; +} __attribute__((packed)) ubx_config_data_header_v0_t; + +typedef struct { + uint8_t version; + uint8_t layers; + uint8_t transaction; + uint8_t reserved; +} __attribute__((packed)) ubx_config_data_header_v1_t; #define MAX_GNSS 7 @@ -146,7 +152,7 @@ typedef struct { typedef struct { ubx_header header; - ubx_config_data_header_t configHeader; + ubx_config_data_header_v1_t configHeader; union { ubx_config_data8_payload_t payload[0]; uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index 3d53d255bf5..1297bb0d194 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -20,9 +20,9 @@ int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPai cfg->header.preamble2 = 0x62; cfg->header.msg_class = 0x06; cfg->header.msg_id = 0x8A; - cfg->header.length = sizeof(ubx_config_data_header_t) + ((sizeof(ubx_config_data8_payload_t) * count)); + cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count)); cfg->configHeader.layers = 0x1; - cfg->configHeader.transation = 0; + cfg->configHeader.transaction = 0; cfg->configHeader.reserved = 0; cfg->configHeader.version = 1; From e950ba8b719b2b6815c94442e089bd965090f7f6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 20:21:49 +0200 Subject: [PATCH 13/36] Re-enable gss full bulk gnss config --- src/main/io/gps_ublox.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 36f0fae4336..7b7f807fa35 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -330,26 +330,26 @@ static void configureGNSS(void) } else { ubx_config_data8_payload_t gnssConfigValues[] = { // SBAS - //{UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, - //{UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, + {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, // Galileo {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, - //{UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, // Beidou - //{UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - //{UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - //{UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, + {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, // Should be enabled with GPS - //{UBLOX_CFG_QZSS_ENA, 1}, - //{UBLOX_CFG_QZSS_L1CA_ENA, 1}, - //{UBLOX_CFG_QZSS_L1S_ENA, 1}, + {UBLOX_CFG_QZSS_ENA, 1}, + {UBLOX_CFG_QZSS_L1CA_ENA, 1}, + {UBLOX_CFG_QZSS_L1S_ENA, 1}, // Glonass - //{UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, - //{UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} + {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, + {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} }; //ubloxCfgValSetBytes(gnssConfigValues, 12); From 3bc96dd65aea18b088d2d637d1797dfc0eb8c507 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 20:22:53 +0200 Subject: [PATCH 14/36] Disable beidous and gzss config for UBLOX8 and older --- src/main/io/gps_ublox.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 7b7f807fa35..5e807a5ab8b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -314,15 +314,14 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* BeiDou */ - blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + //blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); /* GZSS should be enabled when GPS is enabled */ - blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); + //blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); /* GLONASS */ - blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + //blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed); From 2de339eae90f625597c661bf29e6655e7453420e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 22:46:10 +0200 Subject: [PATCH 15/36] Add support for configuring more GNSS constelations in M9+ Also fix #9123 which affects M8+. --- src/main/fc/cli.c | 6 ++++ src/main/io/gps.c | 49 +++++++++++++++++++++++++++ src/main/io/gps.h | 4 +++ src/main/io/gps_ublox.c | 62 +++++++++++++++++++++++++++++----- src/main/io/gps_ublox.h | 74 ++++++++++++++++++++++++++++++++--------- 5 files changed, 170 insertions(+), 25 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1708c796c7b..a997e0be4ca 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3468,6 +3468,12 @@ static void cliStatus(char *cmdline) cliPrintLinefeed(); #endif + if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) { + cliPrint("GPS: "); + cliPrintf("HW Version: %s Baud: %d", getGpsHwVersion(), getGpsBaudrate()); + cliPrintLinefeed(); + } + // If we are blocked by PWM init - provide more information if (getPwmInitError() != PWM_INIT_ERROR_NONE) { cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage()); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 966f7fca0da..be08f28cc99 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -51,6 +51,7 @@ #include "io/serial.h" #include "io/gps.h" #include "io/gps_private.h" +#include "io/gps_ublox.h" #include "navigation/navigation.h" @@ -127,6 +128,54 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT ); + +bool getGpsGnssSettingsStatus(void) +{ + return gpsState.gnssSettingsSuccess; +} + +int getGpsBaudrate(void) +{ + switch(gpsState.baudrateIndex) + { + case GPS_BAUDRATE_115200: + return 115200; + case GPS_BAUDRATE_57600: + return 57600; + case GPS_BAUDRATE_38400: + return 38400; + case GPS_BAUDRATE_19200: + return 19200; + case GPS_BAUDRATE_9600: + return 9600; + case GPS_BAUDRATE_230400: + return 230400; + default: + return 0; + } +} + +const char *getGpsHwVersion(void) +{ + switch(gpsState.hwVersion) + { + case UBX_HW_VERSION_UBLOX5: + return "UBLOX5"; + case UBX_HW_VERSION_UBLOX6: + return "UBLOX6"; + case UBX_HW_VERSION_UBLOX7: + return "UBLOX7"; + case UBX_HW_VERSION_UBLOX8: + return "UBLOX8"; + case UBX_HW_VERSION_UBLOX9: + return "UBLOX9"; + case UBX_HW_VERSION_UBLOX10: + return "UBLOX10"; + default: + return "Unknown"; + } +} + void gpsSetState(gpsState_e state) { gpsState.state = state; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 35f21d9702e..3dbc4c59976 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -168,6 +168,10 @@ struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); void mspGPSReceiveNewData(const uint8_t * bufferPtr); +const char *getGpsHwVersion(void); +int getGpsBaudrate(void); +bool getGpsGnssSettingsStatus(void); + #if defined(USE_GPS_FAKE) void gpsFakeSet( gpsFixType_e fixType, diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 5e807a5ab8b..179d01876c0 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -42,6 +42,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/serial.h" #include "io/gps.h" @@ -188,7 +189,7 @@ static const uint8_t default_payload[] = { // ublox info: https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf -static void ubloxCfgValSetBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) +static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) { ubx_config_data8_t cfg = {}; @@ -236,6 +237,7 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) return 1; } +/* TODO: look for m8 docs static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) { if (!capBeidou) { @@ -297,7 +299,36 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) return 1; } +*/ +static void configureGNSS9(void) +{ + ubx_config_data8_payload_t gnssConfigValues[] = { + // SBAS + {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, + + // Galileo + {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, + + // Beidou + {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, + {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, + + // Should be enabled with GPS + {UBLOX_CFG_QZSS_ENA, 1}, + {UBLOX_CFG_QZSS_L1CA_ENA, 1}, + {UBLOX_CFG_QZSS_L1S_ENA, 1}, + + // Glonass + {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, + {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} + }; + + ubloxSendSetCfgBytes(gnssConfigValues, 12); +} static void configureGNSS(void) { @@ -351,8 +382,7 @@ static void configureGNSS(void) {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} }; - //ubloxCfgValSetBytes(gnssConfigValues, 12); - ubloxCfgValSetBytes(gnssConfigValues, 1); + ubloxSendSetCfgBytes(gnssConfigValues, 12); } } @@ -743,12 +773,13 @@ STATIC_PROTOTHREAD(gpsConfigure) if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { - configureRATE(200); // 5Hz + configureRATE(hz2rate(SETTING_GPS_UBLOX_NAV_HZ_DEFAULT)); // 5Hz + gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; } ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error - configureRATE(200); // 5Hz + configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); } } @@ -781,12 +812,12 @@ STATIC_PROTOTHREAD(gpsConfigure) configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { - configureRATE(200); // 5Hz + configureRATE(hz2rate(5)); // 5Hz } ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error - configureRATE(200); // 5Hz + configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); } } @@ -829,10 +860,23 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); // Configure GNSS for M8N and later - if (gpsState.hwVersion >= 80000) { + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - configureGNSS(); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { + configureGNSS9(); + } else { + configureGNSS(); + } ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + + if(_ack_state == UBX_ACK_GOT_NAK) { + gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; + gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; + gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; + gpsState.gnssSettingsSuccess = false; + } else { + gpsState.gnssSettingsSuccess = true; + } } ptEnd(0); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index b49029842d4..4f46edf23fc 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -50,22 +50,64 @@ extern "C" { #define UBX_HW_VERSION_UBLOX10 1000 -#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 -#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 - -#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 -#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 - -#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 -#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d -#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // default off - -#define UBLOX_CFG_QZSS_ENA 0x10310024 -#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 -#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 - -#define UBLOX_CFG_GLO_ENA 0x10310025 -#define UBLOX_CFG_GLO_L1_ENA 0x10310018 +#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1 +#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1 + +#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1 +#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1 + +#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1 +#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1 +#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off + +#define UBLOX_CFG_QZSS_ENA 0x10310024 // U1 +#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1 +#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1 + +#define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations +#define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off + +#define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8 +#define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs +#define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120 +#define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121 +#define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122 +#define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123 +#define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124 +#define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125 +#define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126 +#define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127 +#define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128 +#define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129 +#define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130 +#define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131 +#define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132 +#define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133 +#define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134 +#define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135 +#define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136 +#define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137 +#define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138 +#define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139 +#define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140 +#define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141 +#define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142 +#define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143 +#define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144 +#define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145 +#define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146 +#define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147 +#define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148 +#define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149 +#define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150 +#define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151 +#define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152 +#define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153 +#define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154 +#define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155 +#define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156 +#define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157 +#define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158 // payload types typedef struct { From 3a4d475d1556f1f58957e8780d13cc0dab37fe74 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 22:53:40 +0200 Subject: [PATCH 16/36] Add file header --- src/main/io/gps_ublox_utils.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index 1297bb0d194..ac1bd285630 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -1,3 +1,21 @@ +/* + * This file is part of INAV + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #include #include "gps_ublox_utils.h" From d56509bdf4a86ed9e1881460324a1c1014ceebfc Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 22:54:59 +0200 Subject: [PATCH 17/36] Add file header --- src/test/unit/gps_ublox_unittest.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 25dffa704b4..6113ac292c3 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -1,3 +1,21 @@ +/* + * This file is part of INAV. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + #include "gtest/gtest.h" #include "unittest_macros.h" From eb723d6e6994be7b2c2b28b01b0212c97ebd2ef0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 22:56:29 +0200 Subject: [PATCH 18/36] remove left over debug code --- src/main/io/gps_ublox.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 179d01876c0..6bbd34240b0 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -873,9 +873,6 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; - gpsState.gnssSettingsSuccess = false; - } else { - gpsState.gnssSettingsSuccess = true; } } From 3b2118bab4b5e061e42ac00dc3c377072c5b44a3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 23:01:54 +0200 Subject: [PATCH 19/36] remove more debug code --- src/main/io/gps.c | 5 ----- src/main/io/gps.h | 1 - 2 files changed, 6 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index be08f28cc99..6a208bf4853 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -129,11 +129,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, ); -bool getGpsGnssSettingsStatus(void) -{ - return gpsState.gnssSettingsSuccess; -} - int getGpsBaudrate(void) { switch(gpsState.baudrateIndex) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 3dbc4c59976..8045197d480 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -170,7 +170,6 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr); const char *getGpsHwVersion(void); int getGpsBaudrate(void); -bool getGpsGnssSettingsStatus(void); #if defined(USE_GPS_FAKE) void gpsFakeSet( From 7da7b89db046cd6d59d0602ce1771a4acb6a6c9f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 23:21:24 +0200 Subject: [PATCH 20/36] Fix typo --- src/test/unit/gps_ublox_unittest.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 6113ac292c3..bc20b340251 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -29,7 +29,7 @@ void dumpCfg(const ubx_config_data8_t *cfg, int valuesAdded) { printf("%02x %02x %02x %02x %04x\n", cfg->header.preamble1, cfg->header.preamble2, cfg->header.msg_class, cfg->header.msg_id, cfg->header.length); - printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transation, cfg->configHeader.reserved); + printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transaction, cfg->configHeader.reserved); for(int i =0; i < valuesAdded; ++i) { printf("%i: %08x %02x\n", i+1, cfg->data.payload[i].key, cfg->data.payload[i].value); From 552cc2c949c183243a223e1dcfbc1e679026cc7e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 14 Jun 2023 23:36:33 +0200 Subject: [PATCH 21/36] skip brew update for a faster build --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7343892fb99..fa8bb6a0e54 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -79,7 +79,7 @@ jobs: - uses: actions/checkout@v3 - name: Install dependencies run: | - brew update + #brew update brew install cmake ninja ruby - name: Setup environment From 716bcbe85de3d83c0598b1d001717f2b88d2cf8b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 00:01:12 +0200 Subject: [PATCH 22/36] Remove redundant definition from CMakeList.txt --- src/test/unit/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 6b52a8a6566..ebfd3b78d4f 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -38,7 +38,7 @@ set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displa set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT) set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c") -set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST USE_GPS USE_GPS_PROTO_UBLOX) +set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST) function(unit_test src) get_filename_component(basename ${src} NAME) From e3bd1df57d9f1a9a441d5acda0e997f5291823d2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 10:25:22 +0200 Subject: [PATCH 23/36] Ensure ck_a and ck_b are initialized before calculating checksum --- src/main/io/gps_ublox_utils.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index ac1bd285630..97c5bf5cd9a 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -22,6 +22,7 @@ void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) { + *ck_a = *ck_b = 0; while (len--) { *ck_a += *data; *ck_b += *ck_a; From ef7984f69526e2c2c5ec8057fa783c7dc63ed449 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 13:27:46 +0200 Subject: [PATCH 24/36] Use 5Hz as fallback value in all cases. Only use new GNSS setting model for M10 and newer gpss S --- src/main/io/gps_ublox.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 6bbd34240b0..ee4a6824ebd 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -301,7 +301,7 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) } */ -static void configureGNSS9(void) +static void configureGNSS10(void) { ubx_config_data8_payload_t gnssConfigValues[] = { // SBAS @@ -773,7 +773,7 @@ STATIC_PROTOTHREAD(gpsConfigure) if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { - configureRATE(hz2rate(SETTING_GPS_UBLOX_NAV_HZ_DEFAULT)); // 5Hz + configureRATE(hz2rate(5)); // 5Hz gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; } ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); @@ -862,8 +862,8 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { - configureGNSS9(); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10) { + configureGNSS10(); } else { configureGNSS(); } From 0fda8d9cf072714d8d736808b9f998098245a23e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 16:50:08 +0200 Subject: [PATCH 25/36] Refaactor GNSS capabilities detection for M8+ devices Add GPS and GNSS information to status on the CLI --- src/main/fc/cli.c | 7 ++ src/main/io/gps_ublox.c | 147 +++++++++++++++++++++++++++++++++++++--- src/main/io/gps_ublox.h | 31 ++++++++- 3 files changed, 172 insertions(+), 13 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a997e0be4ca..09e736ddb8b 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -89,6 +89,7 @@ bool cliMode = false; #include "io/beeper.h" #include "io/flashfs.h" #include "io/gps.h" +#include "io/gps_ublox.h" #include "io/ledstrip.h" #include "io/osd.h" #include "io/serial.h" @@ -3472,6 +3473,12 @@ static void cliStatus(char *cmdline) cliPrint("GPS: "); cliPrintf("HW Version: %s Baud: %d", getGpsHwVersion(), getGpsBaudrate()); cliPrintLinefeed(); + cliPrintLine(" GNSS Capabilities:"); + cliPrintLine(" GPS"); + cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasGalileo(), gpsUbloxGalileoDefault(), gpsUbloxGalileoEnabled()); + cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasBeidou(), gpsUbloxBeidouDefault(), gpsUbloxBeidouEnabled()); + cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasGlonass(), gpsUbloxGlonassDefault(), gpsUbloxGlonassEnabled()); + cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss()); } // If we are blocked by PWM init - provide more information diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index ee4a6824ebd..6e4f709815c 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -101,10 +101,18 @@ static bool _new_position; static bool _new_speed; // Need this to determine if Galileo capable only -static bool capGalileo; -static bool capBeidou; -static bool capGzss; -static bool capGlonass; +static struct { + bool capGalileo; + bool capBeidou; + bool capGlonass; + int capMaxGnss; + bool galileoDefault; + bool beidouDefault; + bool glonassDefault; + bool galileoEnabled; + bool beidouEnabled; + bool glonassEnabled; +} ubx_capabilities = { }; // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status' @@ -136,9 +144,71 @@ static union { ubx_mon_ver ver; ubx_nav_timeutc timeutc; ubx_ack_ack ack; + ubx_mon_gnss gnss; uint8_t bytes[UBLOX_BUFFER_SIZE]; } _buffer; +const char *gpsUbloxHasGalileo(void) +{ + if (ubx_capabilities.capGalileo) { + return "Galileo"; + } + + return "No Galileo"; +} + +const char *gpsUbloxHasBeidou(void) +{ + if (ubx_capabilities.capBeidou) { + return "BeiDou"; + } + + return "No BeiDou"; +} + +const char *gpsUbloxHasGlonass(void) +{ + if (ubx_capabilities.capGlonass) { + return "Glonass"; + } + + return "No Glonass"; +} + +bool gpsUbloxGalileoDefault(void) +{ + return ubx_capabilities.galileoDefault; +} + +bool gpsUbloxBeidouDefault(void) +{ + return ubx_capabilities.beidouDefault; +} + +bool gpsUbloxGlonassDefault(void) +{ + return ubx_capabilities.glonassDefault; +} + +bool gpsUbloxGalileoEnabled(void) +{ + return ubx_capabilities.galileoEnabled; +} + +bool gpsUbloxBeidouEnabled(void) +{ + return ubx_capabilities.beidouEnabled; +} + +bool gpsUbloxGlonassEnabled(void) +{ + return ubx_capabilities.glonassEnabled; +} + +uint8_t gpsUbloxMaxGnss(void) +{ + return ubx_capabilities.capMaxGnss; +} static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) { @@ -172,6 +242,15 @@ static void pollVersion(void) sendConfigMessageUBLOX(); } +static void pollGnssCapabilities(void) +{ + send_buffer.message.header.msg_class = CLASS_MON; + send_buffer.message.header.msg_id = MSG_MON_GNSS; + send_buffer.message.header.length = 0; + sendConfigMessageUBLOX(); +} + + static const uint8_t default_payload[] = { 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code) 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and @@ -218,7 +297,7 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) { - if (!capGalileo) { + if (!ubx_capabilities.capGalileo) { return 0; } @@ -240,7 +319,7 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) /* TODO: look for m8 docs static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) { - if (!capBeidou) { + if (!ubx_capabilities.capBeidou) { return 0; } @@ -262,7 +341,7 @@ static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) { - if (!capGzss) { + if (!ubx_capabilities.capGzss) { return 0; } @@ -280,7 +359,7 @@ static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) { - if (!capGlonass) { + if (!ubx_capabilities.capGlonass) { return 0; } @@ -564,7 +643,8 @@ static bool gpsParceFrameUBLOX(void) case MSG_VER: if (_class == CLASS_MON) { gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); - if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) { + /* + if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8 && gpsState.hwVersion < UBX_HW_VERSION_UBLOX10) && (_buffer.ver.swVersion[9] > '2')) { // check extensions; // after hw + sw vers; each is 30 bytes for(int j = 40; j < _payload_length; j += 30) { @@ -572,13 +652,48 @@ static bool gpsParceFrameUBLOX(void) capGalileo = true; } else if (strnstr((const char *)(_buffer.bytes+j), "BDS", 30)) { capBeidou = true; - } else if (strnstr((const char *)(_buffer.bytes+j), "GZSS", 30)) { - capGzss = true; } else if (strnstr((const char *)(_buffer.bytes+j), "GLO", 30)) { capGlonass = true; } + } + } + */ + } + break; + case MSG_MON_GNSS: // M9 / M10? + if(_class == CLASS_MON) { + if (_buffer.gnss.version == 0) { + if (_buffer.gnss.supported & UBX_MON_GNSS_GALILEO_MASK) { + ubx_capabilities.capGalileo = true; + } + if (_buffer.gnss.supported & UBX_MON_GNSS_BEIDOU_MASK) { + ubx_capabilities.capBeidou = true; + } + if (_buffer.gnss.supported & UBX_MON_GNSS_GLONASS_MASK) { + ubx_capabilities.capGlonass = true; + } + + if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_GALILEO_MASK) { + ubx_capabilities.galileoDefault = true; + } + if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK) { + ubx_capabilities.beidouDefault = true; + } + if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_GLONASS_MASK) { + ubx_capabilities.glonassDefault = true; + } + if (_buffer.gnss.enabled & UBX_MON_GNSS_GALILEO_MASK) { + ubx_capabilities.galileoEnabled = true; + } + if (_buffer.gnss.enabled & UBX_MON_GNSS_BEIDOU_MASK) { + ubx_capabilities.beidouEnabled = true; } + if (_buffer.gnss.enabled & UBX_MON_GNSS_GLONASS_MASK) { + ubx_capabilities.glonassEnabled = true; + } + + ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent; } } break; @@ -950,6 +1065,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + gpsState.autoConfigStep = 0; + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + + // Configure GPS ptSpawn(gpsConfigure); } @@ -961,6 +1084,8 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) while (1) { ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(); + + pollGnssCapabilities(); } ptEnd(0); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 4f46edf23fc..1391bab03bc 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -18,6 +18,7 @@ #pragma once #include +#include #ifdef __cplusplus extern "C" { @@ -328,6 +329,22 @@ typedef struct { uint16_t reserved3; } ubx_nav_pvt; +#define UBX_MON_GNSS_GPS_MASK (1 << 0) +#define UBX_MON_GNSS_GLONASS_MASK (1 << 1) +#define UBX_MON_GNSS_BEIDOU_MASK (1 << 2) +#define UBX_MON_GNSS_GALILEO_MASK (1 << 3) + +typedef struct { + uint8_t version; + uint8_t supported; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t defaultGnss; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t enabled; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo + uint8_t maxConcurrent; + uint8_t reserverd1; + uint8_t reserverd2; + uint8_t reserverd3; +} ubx_mon_gnss; + typedef struct { uint8_t msg_class; uint8_t msg; @@ -371,7 +388,8 @@ typedef enum { MSG_CFG_SET_RATE = 0x01, MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_SBAS = 0x16, - MSG_CFG_GNSS = 0x3e + MSG_CFG_GNSS = 0x3e, + MSG_MON_GNSS = 0x28 } ubx_protocol_bytes_t; typedef enum { @@ -387,7 +405,16 @@ typedef enum { NAV_STATUS_FIX_VALID = 1 } ubx_nav_status_bits_t; - +const char *gpsUbloxHasGalileo(void); +const char *gpsUbloxHasBeidou(void); +const char *gpsUbloxHasGlonass(void); +uint8_t gpsUbloxMaxGnss(void); +bool gpsUbloxGalileoDefault(void); +bool gpsUbloxBeidouDefault(void); +bool gpsUbloxGlonassDefault(void); +bool gpsUbloxGalileoEnabled(void); +bool gpsUbloxBeidouEnabled(void); +bool gpsUbloxGlonassEnabled(void); #ifdef __cplusplus From 26f69a2070feca7340fe048e61763a0f0f106c23 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 17:28:16 +0200 Subject: [PATCH 26/36] Change format of GPS status --- src/main/fc/cli.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 09e736ddb8b..fa80e4931c8 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3474,10 +3474,11 @@ static void cliStatus(char *cmdline) cliPrintf("HW Version: %s Baud: %d", getGpsHwVersion(), getGpsBaudrate()); cliPrintLinefeed(); cliPrintLine(" GNSS Capabilities:"); - cliPrintLine(" GPS"); - cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasGalileo(), gpsUbloxGalileoDefault(), gpsUbloxGalileoEnabled()); - cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasBeidou(), gpsUbloxBeidouDefault(), gpsUbloxBeidouEnabled()); - cliPrintLinef(" %s (default: %d): %d", gpsUbloxHasGlonass(), gpsUbloxGlonassDefault(), gpsUbloxGlonassEnabled()); + cliPrintLine(" GNSS Provider active/default"); + cliPrintLine (" GPS 1/1"); + cliPrintLinef(" %s %d/%d", gpsUbloxHasGalileo(), gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault()); + cliPrintLinef(" %s %d/%d", gpsUbloxHasBeidou(), gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault()); + cliPrintLinef(" %s %d/%d", gpsUbloxHasGlonass(), gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault()); cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss()); } From 68571a625c549b2241490e5ba49c59a8acbf05ec Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 18:14:25 +0200 Subject: [PATCH 27/36] Simplify capabilities parsing --- src/main/io/gps_ublox.c | 51 ++++++++++++++--------------------------- 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 6e4f709815c..11b1694ec3f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -264,9 +264,6 @@ static const uint8_t default_payload[] = { #define GNSSID_GZSS 5 #define GNSSID_GLONASS 6 - - - // ublox info: https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) { @@ -316,7 +313,6 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) return 1; } -/* TODO: look for m8 docs static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) { if (!ubx_capabilities.capBeidou) { @@ -339,6 +335,7 @@ static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) return 1; } +/* static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) { if (!ubx_capabilities.capGzss) { @@ -356,6 +353,7 @@ static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) return 1; } +*/ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) { @@ -378,7 +376,6 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) return 1; } -*/ static void configureGNSS10(void) { @@ -424,14 +421,15 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* BeiDou */ - //blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); /* GZSS should be enabled when GPS is enabled */ //blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); /* GLONASS */ - //blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed); @@ -663,35 +661,17 @@ static bool gpsParceFrameUBLOX(void) case MSG_MON_GNSS: // M9 / M10? if(_class == CLASS_MON) { if (_buffer.gnss.version == 0) { - if (_buffer.gnss.supported & UBX_MON_GNSS_GALILEO_MASK) { - ubx_capabilities.capGalileo = true; - } - if (_buffer.gnss.supported & UBX_MON_GNSS_BEIDOU_MASK) { - ubx_capabilities.capBeidou = true; - } - if (_buffer.gnss.supported & UBX_MON_GNSS_GLONASS_MASK) { - ubx_capabilities.capGlonass = true; - } + ubx_capabilities.capGalileo = _buffer.gnss.supported & UBX_MON_GNSS_GALILEO_MASK; + ubx_capabilities.capBeidou =_buffer.gnss.supported & UBX_MON_GNSS_BEIDOU_MASK; + ubx_capabilities.capGlonass =_buffer.gnss.supported & UBX_MON_GNSS_GLONASS_MASK ; - if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_GALILEO_MASK) { - ubx_capabilities.galileoDefault = true; - } - if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK) { - ubx_capabilities.beidouDefault = true; - } - if (_buffer.gnss.defaultGnss & UBX_MON_GNSS_GLONASS_MASK) { - ubx_capabilities.glonassDefault = true; - } + ubx_capabilities.galileoDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_GALILEO_MASK; + ubx_capabilities.beidouDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK; + ubx_capabilities.glonassDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_GLONASS_MASK; - if (_buffer.gnss.enabled & UBX_MON_GNSS_GALILEO_MASK) { - ubx_capabilities.galileoEnabled = true; - } - if (_buffer.gnss.enabled & UBX_MON_GNSS_BEIDOU_MASK) { - ubx_capabilities.beidouEnabled = true; - } - if (_buffer.gnss.enabled & UBX_MON_GNSS_GLONASS_MASK) { - ubx_capabilities.glonassEnabled = true; - } + ubx_capabilities.galileoEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_GALILEO_MASK; + ubx_capabilities.beidouEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_BEIDOU_MASK; + ubx_capabilities.glonassEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_GLONASS_MASK; ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent; } @@ -991,6 +971,9 @@ STATIC_PROTOTHREAD(gpsConfigure) } } + pollGnssCapabilities(); + //ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptEnd(0); } From 7cd80769709eb0ce7826daeb90c1ae95045fb737 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 15 Jun 2023 18:30:10 +0200 Subject: [PATCH 28/36] Make sure we poll for gnss capabilities after config --- src/main/io/gps_ublox.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 11b1694ec3f..44e1fbb07ae 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -972,7 +972,7 @@ STATIC_PROTOTHREAD(gpsConfigure) } pollGnssCapabilities(); - //ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); ptEnd(0); } @@ -1053,8 +1053,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) pollGnssCapabilities(); gpsState.autoConfigStep++; ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); - + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); // Configure GPS ptSpawn(gpsConfigure); From a2df1a6ea46d6cae29df6e4d9ac5c6bf15606cb2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 16 Jun 2023 11:05:21 +0200 Subject: [PATCH 29/36] Update pdf links to for M9 and M10 interface descriptions --- src/main/io/gps_ublox.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 44e1fbb07ae..a4db1eda473 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -264,7 +264,8 @@ static const uint8_t default_payload[] = { #define GNSSID_GZSS 5 #define GNSSID_GLONASS 6 -// ublox info: https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf +// M10 ublox protocol info: +// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count) { ubx_config_data8_t cfg = {}; @@ -276,6 +277,8 @@ static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t co _ack_state = UBX_ACK_WAITING; } +// Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration +// https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { gnss_block->gnssId = GNSSID_SBAS; From 58799bbbab822705f553c59415b29bd6acc350d1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 16 Jun 2023 13:02:19 +0200 Subject: [PATCH 30/36] Fix old school detection, for backwards compatibility. Add protocol vesion parsing --- src/main/io/gps_private.h | 2 ++ src/main/io/gps_ublox.c | 36 +++++++++++++++++++++++++++++------- 2 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 6e9abc72ffe..3fda3701248 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -43,6 +43,8 @@ typedef struct { serialPort_t * gpsPort; // Serial GPS only uint32_t hwVersion; + uint8_t swVersionMajor; + uint8_t swVersionMinor; gpsState_e state; gpsBaudRate_e baudrateIndex; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index a4db1eda473..80780ba22ad 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -278,6 +278,7 @@ static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t co } // Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration +// https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf // https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { @@ -519,6 +520,22 @@ static void configureSBAS(void) sendConfigMessageUBLOX(); } +static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) +{ + gpsState.swVersionMajor = 0; + gpsState.swVersionMinor = 0; + + if (!strncmp(proto, "PROTOVER=", 9)) { + proto+=9; + bufferLength-=9; + + float ver = atof(proto); + + gpsState.swVersionMajor = (uint8_t)ver; + gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f); + } +} + static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) { // ublox_5 hwVersion 00040005 @@ -644,21 +661,26 @@ static bool gpsParceFrameUBLOX(void) case MSG_VER: if (_class == CLASS_MON) { gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); - /* if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8 && gpsState.hwVersion < UBX_HW_VERSION_UBLOX10) && (_buffer.ver.swVersion[9] > '2')) { // check extensions; // after hw + sw vers; each is 30 bytes for(int j = 40; j < _payload_length; j += 30) { + // Example content: GPS;GAL;BDS;GLO if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) { - capGalileo = true; - } else if (strnstr((const char *)(_buffer.bytes+j), "BDS", 30)) { - capBeidou = true; - } else if (strnstr((const char *)(_buffer.bytes+j), "GLO", 30)) { - capGlonass = true; + ubx_capabilities.capGalileo = true; + } + if (strnstr((const char *)(_buffer.bytes+j), "BDS", 30)) { + ubx_capabilities.capBeidou = true; + } + if (strnstr((const char *)(_buffer.bytes+j), "GLO", 30)) { + ubx_capabilities.capGlonass = true; + } + + if(!strncmp((const char *)(_buffer.bytes+j), "PROTOVER=", 9)) { + gpsDecodeProtocolVersion((const char *)(_buffer.bytes+j), 30); } } } - */ } break; case MSG_MON_GNSS: // M9 / M10? From 9b39cdf3afa82a13f2c5ae92c309a47dce9e55c7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 16 Jun 2023 22:01:42 +0200 Subject: [PATCH 31/36] Parse protocol version in ublox messages The documentation often refers to messages being obsolete by protocol version, and not by GPS model. --- src/main/fc/cli.c | 2 +- src/main/io/gps.c | 10 +++++ src/main/io/gps.h | 3 ++ src/main/io/gps_private.h | 1 + src/main/io/gps_ublox.c | 92 ++++++++++++++++++++++++--------------- src/main/io/gps_ublox.h | 3 +- 6 files changed, 74 insertions(+), 37 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index fa80e4931c8..9145c1ed3de 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3471,7 +3471,7 @@ static void cliStatus(char *cmdline) if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) { cliPrint("GPS: "); - cliPrintf("HW Version: %s Baud: %d", getGpsHwVersion(), getGpsBaudrate()); + cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); cliPrintLinefeed(); cliPrintLine(" GNSS Capabilities:"); cliPrintLine(" GNSS Provider active/default"); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 6a208bf4853..fa225110b46 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -171,6 +171,16 @@ const char *getGpsHwVersion(void) } } +uint8_t getGpsProtoMajorVersion(void) +{ + return gpsState.swVersionMajor; +} + +uint8_t getGpsProtoMinorVersion(void) +{ + return gpsState.swVersionMinor; +} + void gpsSetState(gpsState_e state) { gpsState.state = state; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 8045197d480..0e0780dea3e 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -169,6 +169,9 @@ void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); void mspGPSReceiveNewData(const uint8_t * bufferPtr); const char *getGpsHwVersion(void); +uint8_t getGpsProtoMajorVersion(void); +uint8_t getGpsProtoMinorVersion(void); + int getGpsBaudrate(void); #if defined(USE_GPS_FAKE) diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 3fda3701248..42dca0669d6 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -56,6 +56,7 @@ typedef struct { timeMs_t lastMessageMs; timeMs_t timeoutMs; timeMs_t baseTimeoutMs; + timeMs_t lastCapaPoolMs; } gpsReceiverData_t; extern gpsReceiverData_t gpsState; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 80780ba22ad..eee0bc7e915 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -298,14 +298,17 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) { - if (!ubx_capabilities.capGalileo) { + if (!ubx_capabilities.capGalileo || gpsState.swVersionMajor < 18) { return 0; } gnss_block->gnssId = GNSSID_GALILEO; gnss_block->maxTrkCh = 8; - // E1 = 0x01 - gnss_block->sigCfgMask = 1; + // sigCfgMask + // 0x01 = Galileo E1 (not supported for protocol versions less than 18.00) + // 0x10 = Galileo E5a // off by default + // 0x20 = Galileo E5b // off by default + gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseGalileo) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; @@ -325,9 +328,11 @@ static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_BEIDOU; gnss_block->maxTrkCh = 8; - // B1L = 0x01 - // B2L = 0x10 - gnss_block->sigCfgMask = 0x01; + // sigCfgMask + // 0x01 = BeiDou B1I + // 0x10 = BeiDou B2I // off by default + // 0x80 = BeiDou B2A // off by default + gnss_block->sigCfgMask = 0x01 | 0x10; if (gpsState.gpsConfig->ubloxUseBeidou) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; @@ -367,8 +372,8 @@ static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) gnss_block->gnssId = GNSSID_GLONASS; gnss_block->maxTrkCh = 8; - // L1 = 0x01 - // L2 = 0x10 + // 0x01 = GLONASS L1 + // 0x10 = GLONASS L2 // off by default gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseGlonass) { gnss_block->enabled = 1; @@ -522,12 +527,9 @@ static void configureSBAS(void) static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) { - gpsState.swVersionMajor = 0; - gpsState.swVersionMinor = 0; - - if (!strncmp(proto, "PROTOVER=", 9)) { - proto+=9; - bufferLength-=9; + if (!strncmp(proto, "PROTVER=", 8)) { + proto+=8; + bufferLength-=8; float ver = atof(proto); @@ -661,23 +663,37 @@ static bool gpsParceFrameUBLOX(void) case MSG_VER: if (_class == CLASS_MON) { gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); - if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8 && gpsState.hwVersion < UBX_HW_VERSION_UBLOX10) && (_buffer.ver.swVersion[9] > '2')) { - // check extensions; - // after hw + sw vers; each is 30 bytes - for(int j = 40; j < _payload_length; j += 30) { - // Example content: GPS;GAL;BDS;GLO - if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) { - ubx_capabilities.capGalileo = true; + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { + if (_buffer.ver.swVersion[9] > '2') { + // check extensions; + // after hw + sw vers; each is 30 bytes + bool found = false; + for (int j = 40; j < _payload_length && !found; j += 30) + { + // Example content: GPS;GAL;BDS;GLO + if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30)) + { + ubx_capabilities.capGalileo = true; + found = true; + } + if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30)) + { + ubx_capabilities.capBeidou = true; + found = true; + } + if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30)) + { + ubx_capabilities.capGlonass = true; + found = true; + } } - if (strnstr((const char *)(_buffer.bytes+j), "BDS", 30)) { - ubx_capabilities.capBeidou = true; - } - if (strnstr((const char *)(_buffer.bytes+j), "GLO", 30)) { - ubx_capabilities.capGlonass = true; - } - - if(!strncmp((const char *)(_buffer.bytes+j), "PROTOVER=", 9)) { - gpsDecodeProtocolVersion((const char *)(_buffer.bytes+j), 30); + } + for (int j = 40; j < _payload_length; j += 30) { + if (strnstr((const char *)(_buffer.bytes + j), "PROTVER=", 30)) { + gpsState.swVersionMajor = 3; + gpsState.swVersionMinor = 3; + gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); + break; } } } @@ -982,7 +998,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10) { + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); } else { configureGNSS(); @@ -996,9 +1012,6 @@ STATIC_PROTOTHREAD(gpsConfigure) } } - pollGnssCapabilities(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - ptEnd(0); } @@ -1092,7 +1105,16 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(); - pollGnssCapabilities(); + if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider || GPS_UBLOX7PLUS) && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { + pollVersion(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } + + if((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { + pollGnssCapabilities(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + gpsState.lastCapaPoolMs = millis(); + } } ptEnd(0); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 1391bab03bc..967e1ddd93b 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -25,10 +25,11 @@ extern "C" { #endif #define GPS_CFG_CMD_TIMEOUT_MS 200 -#define GPS_VERSION_RETRY_TIMES 2 +#define GPS_VERSION_RETRY_TIMES 3 #define MAX_UBLOX_PAYLOAD_SIZE 256 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE #define UBLOX_SBAS_MESSAGE_LENGTH 16 +#define GPS_CAPA_INTERVAL 5000 #define UBX_DYNMODEL_PEDESTRIAN 3 #define UBX_DYNMODEL_AIR_1G 6 From c09167837f619e27c66468f43df9192a07baa2b8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 16 Jun 2023 22:06:15 +0200 Subject: [PATCH 32/36] fix sigCfgMsk --- src/main/io/gps_ublox.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index eee0bc7e915..e056ff3d53c 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -332,7 +332,7 @@ static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) // 0x01 = BeiDou B1I // 0x10 = BeiDou B2I // off by default // 0x80 = BeiDou B2A // off by default - gnss_block->sigCfgMask = 0x01 | 0x10; + gnss_block->sigCfgMask = 0x01; if (gpsState.gpsConfig->ubloxUseBeidou) { gnss_block->enabled = 1; gnss_block->resTrkCh = 4; From 6feb1e4b7f172f25f3199ce08f7655b911fd4d10 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 17 Jun 2023 00:33:42 +0200 Subject: [PATCH 33/36] Simplify capabilities detection and consolidate gnss info into 3 bitmask --- src/main/fc/cli.c | 14 ++-- src/main/io/gps_private.h | 1 + src/main/io/gps_ublox.c | 162 +++++++++++++------------------------- src/main/io/gps_ublox.h | 14 +++- 4 files changed, 74 insertions(+), 117 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9145c1ed3de..ae80f0095df 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3473,13 +3473,17 @@ static void cliStatus(char *cmdline) cliPrint("GPS: "); cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); cliPrintLinefeed(); - cliPrintLine(" GNSS Capabilities:"); + cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate()); cliPrintLine(" GNSS Provider active/default"); - cliPrintLine (" GPS 1/1"); - cliPrintLinef(" %s %d/%d", gpsUbloxHasGalileo(), gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault()); - cliPrintLinef(" %s %d/%d", gpsUbloxHasBeidou(), gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault()); - cliPrintLinef(" %s %d/%d", gpsUbloxHasGlonass(), gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault()); + cliPrintLine(" GPS 1/1"); + if(gpsUbloxHasGalileo()) + cliPrintLinef(" Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault()); + if(gpsUbloxHasBeidou()) + cliPrintLinef(" BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault()); + if(gpsUbloxHasGlonass()) + cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault()); cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss()); + cliPrintLinef(" lastUpdate: %d", gpsUbloxCapLastUpdate()); } // If we are blocked by PWM init - provide more information diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 42dca0669d6..765318d1fce 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -57,6 +57,7 @@ typedef struct { timeMs_t timeoutMs; timeMs_t baseTimeoutMs; timeMs_t lastCapaPoolMs; + timeMs_t lastCapaUpdMs; } gpsReceiverData_t; extern gpsReceiverData_t gpsState; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index e056ff3d53c..c7a0989d83f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -102,16 +102,10 @@ static bool _new_speed; // Need this to determine if Galileo capable only static struct { - bool capGalileo; - bool capBeidou; - bool capGlonass; + uint8_t supported; int capMaxGnss; - bool galileoDefault; - bool beidouDefault; - bool glonassDefault; - bool galileoEnabled; - bool beidouEnabled; - bool glonassEnabled; + uint8_t defaultGnss; + uint8_t enabledGnss; } ubx_capabilities = { }; // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. @@ -148,61 +142,49 @@ static union { uint8_t bytes[UBLOX_BUFFER_SIZE]; } _buffer; -const char *gpsUbloxHasGalileo(void) +bool gpsUbloxHasGalileo(void) { - if (ubx_capabilities.capGalileo) { - return "Galileo"; - } - - return "No Galileo"; + return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); } -const char *gpsUbloxHasBeidou(void) +bool gpsUbloxHasBeidou(void) { - if (ubx_capabilities.capBeidou) { - return "BeiDou"; - } - - return "No BeiDou"; + return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK; } -const char *gpsUbloxHasGlonass(void) +bool gpsUbloxHasGlonass(void) { - if (ubx_capabilities.capGlonass) { - return "Glonass"; - } - - return "No Glonass"; + return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK; } bool gpsUbloxGalileoDefault(void) { - return ubx_capabilities.galileoDefault; + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK; } bool gpsUbloxBeidouDefault(void) { - return ubx_capabilities.beidouDefault; + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK; } bool gpsUbloxGlonassDefault(void) { - return ubx_capabilities.glonassDefault; + return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK; } bool gpsUbloxGalileoEnabled(void) { - return ubx_capabilities.galileoEnabled; + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK; } bool gpsUbloxBeidouEnabled(void) { - return ubx_capabilities.beidouEnabled; + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK; } bool gpsUbloxGlonassEnabled(void) { - return ubx_capabilities.glonassEnabled; + return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK; } uint8_t gpsUbloxMaxGnss(void) @@ -210,6 +192,11 @@ uint8_t gpsUbloxMaxGnss(void) return ubx_capabilities.capMaxGnss; } +timeMs_t gpsUbloxCapLastUpdate(void) +{ + return gpsState.lastCapaUpdMs; +} + static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType) { if (fixValid && ubloxFixType == FIX_2D) @@ -298,7 +285,7 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) { - if (!ubx_capabilities.capGalileo || gpsState.swVersionMajor < 18) { + if (!gpsUbloxHasGalileo()) { return 0; } @@ -322,7 +309,7 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block) { - if (!ubx_capabilities.capBeidou) { + if (!gpsUbloxHasBeidou()) { return 0; } @@ -366,7 +353,7 @@ static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block) static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block) { - if (!ubx_capabilities.capGlonass) { + if(!gpsUbloxHasGlonass()) { return 0; } @@ -417,7 +404,6 @@ static void configureGNSS10(void) static void configureGNSS(void) { - if(gpsState.hwVersion < UBX_HW_VERSION_UBLOX9) { int blocksUsed = 0; send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET @@ -430,46 +416,16 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); - + /* BeiDou */ blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* GZSS should be enabled when GPS is enabled */ - //blocksUsed += configureGNSS_GZSS(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* GLONASS */ blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed); sendConfigMessageUBLOX(); - } else { - ubx_config_data8_payload_t gnssConfigValues[] = { - // SBAS - {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, - {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, - - // Galileo - {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, - {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, - - // Beidou - {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou}, - {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0}, - - // Should be enabled with GPS - {UBLOX_CFG_QZSS_ENA, 1}, - {UBLOX_CFG_QZSS_L1CA_ENA, 1}, - {UBLOX_CFG_QZSS_L1S_ENA, 1}, - - // Glonass - {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass}, - {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass} - }; - - ubloxSendSetCfgBytes(gnssConfigValues, 12); - } } static void configureNAV5(uint8_t dynModel, uint8_t fixMode) @@ -527,9 +483,8 @@ static void configureSBAS(void) static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) { - if (!strncmp(proto, "PROTVER=", 8)) { + if (bufferLength > 13 && !strncmp(proto, "PROTVER=", 8)) { proto+=8; - bufferLength-=8; float ver = atof(proto); @@ -664,7 +619,7 @@ static bool gpsParceFrameUBLOX(void) if (_class == CLASS_MON) { gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { - if (_buffer.ver.swVersion[9] > '2') { + if (_buffer.ver.swVersion[9] > '2' || true) { // check extensions; // after hw + sw vers; each is 30 bytes bool found = false; @@ -673,25 +628,23 @@ static bool gpsParceFrameUBLOX(void) // Example content: GPS;GAL;BDS;GLO if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30)) { - ubx_capabilities.capGalileo = true; + ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK; found = true; } if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30)) { - ubx_capabilities.capBeidou = true; + ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK; found = true; } if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30)) { - ubx_capabilities.capGlonass = true; + ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK; found = true; } } } for (int j = 40; j < _payload_length; j += 30) { if (strnstr((const char *)(_buffer.bytes + j), "PROTVER=", 30)) { - gpsState.swVersionMajor = 3; - gpsState.swVersionMinor = 3; gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); break; } @@ -699,22 +652,14 @@ static bool gpsParceFrameUBLOX(void) } } break; - case MSG_MON_GNSS: // M9 / M10? + case MSG_MON_GNSS: if(_class == CLASS_MON) { if (_buffer.gnss.version == 0) { - ubx_capabilities.capGalileo = _buffer.gnss.supported & UBX_MON_GNSS_GALILEO_MASK; - ubx_capabilities.capBeidou =_buffer.gnss.supported & UBX_MON_GNSS_BEIDOU_MASK; - ubx_capabilities.capGlonass =_buffer.gnss.supported & UBX_MON_GNSS_GLONASS_MASK ; - - ubx_capabilities.galileoDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_GALILEO_MASK; - ubx_capabilities.beidouDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK; - ubx_capabilities.glonassDefault = _buffer.gnss.defaultGnss & UBX_MON_GNSS_GLONASS_MASK; - - ubx_capabilities.galileoEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_GALILEO_MASK; - ubx_capabilities.beidouEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_BEIDOU_MASK; - ubx_capabilities.glonassEnabled = _buffer.gnss.enabled & UBX_MON_GNSS_GLONASS_MASK; - + ubx_capabilities.supported = _buffer.gnss.supported; + ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss; + ubx_capabilities.enabledGnss = _buffer.gnss.enabled; ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent; + gpsState.lastCapaUpdMs = millis(); } } break; @@ -998,7 +943,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 /*|| (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)*/) { configureGNSS10(); } else { configureGNSS(); @@ -1071,28 +1016,29 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Configure GPS module if enabled - if (gpsState.gpsConfig->autoConfig) { - // Reset protocol timeout - gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); + // Reset protocol timeout + gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); - // Attempt to detect GPS hw version - gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; - gpsState.autoConfigStep = 0; + // Attempt to detect GPS hw version + gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; + gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); - gpsState.autoConfigStep = 0; - do { - pollGnssCapabilities(); - gpsState.autoConfigStep++; - ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + gpsState.autoConfigStep = 0; + ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + // Configure GPS module if enabled + if (gpsState.gpsConfig->autoConfig) { // Configure GPS ptSpawn(gpsConfigure); } diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 967e1ddd93b..00b42eeb2b7 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -20,11 +20,13 @@ #include #include +#include "common/time.h" + #ifdef __cplusplus extern "C" { #endif -#define GPS_CFG_CMD_TIMEOUT_MS 200 +#define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 #define MAX_UBLOX_PAYLOAD_SIZE 256 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE @@ -406,13 +408,17 @@ typedef enum { NAV_STATUS_FIX_VALID = 1 } ubx_nav_status_bits_t; -const char *gpsUbloxHasGalileo(void); -const char *gpsUbloxHasBeidou(void); -const char *gpsUbloxHasGlonass(void); uint8_t gpsUbloxMaxGnss(void); +timeMs_t gpsUbloxCapLastUpdate(void); + +bool gpsUbloxHasGalileo(void); +bool gpsUbloxHasBeidou(void); +bool gpsUbloxHasGlonass(void); + bool gpsUbloxGalileoDefault(void); bool gpsUbloxBeidouDefault(void); bool gpsUbloxGlonassDefault(void); + bool gpsUbloxGalileoEnabled(void); bool gpsUbloxBeidouEnabled(void); bool gpsUbloxGlonassEnabled(void); From 1205b69ecb731e16987914a86ffb1df7407c72a3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 17 Jun 2023 00:38:10 +0200 Subject: [PATCH 34/36] remove debug info and fix a typo --- src/main/fc/cli.c | 3 ++- src/main/io/gps_ublox.c | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ae80f0095df..1c1f2b25802 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3473,7 +3473,8 @@ static void cliStatus(char *cmdline) cliPrint("GPS: "); cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); cliPrintLinefeed(); - cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate()); + //cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate()); + cliPrintLinef(" GNSS Capabilities:"); cliPrintLine(" GNSS Provider active/default"); cliPrintLine(" GPS 1/1"); if(gpsUbloxHasGalileo()) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index c7a0989d83f..48d1ef5ec94 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -943,7 +943,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 /*|| (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)*/) { + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); } else { configureGNSS(); @@ -1051,7 +1051,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(); - if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider || GPS_UBLOX7PLUS) && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { + if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { pollVersion(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); } From ca6910a808df695fc59a5d260c46248312296c9d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 17 Jun 2023 00:46:57 +0200 Subject: [PATCH 35/36] Retry version polling on the same interval as capabilities --- src/main/io/gps_ublox.c | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 48d1ef5ec94..9b678a36aff 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1051,15 +1051,19 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(); - if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) { - pollVersion(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - } + if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { + if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { + gpsState.lastCapaPoolMs = millis(); + + if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) + { + pollVersion(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } - if((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { - pollGnssCapabilities(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - gpsState.lastCapaPoolMs = millis(); + pollGnssCapabilities(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } } } From 49238881c959c70fbe6de9ea25a6a7e17390b9d2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 17 Jun 2023 00:53:44 +0200 Subject: [PATCH 36/36] Remove debug info --- src/main/fc/cli.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1c1f2b25802..a14aac38a28 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3484,7 +3484,6 @@ static void cliStatus(char *cmdline) if(gpsUbloxHasGlonass()) cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault()); cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss()); - cliPrintLinef(" lastUpdate: %d", gpsUbloxCapLastUpdate()); } // If we are blocked by PWM init - provide more information