diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index fbe4a7f74ff..9cc4aa76954 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -288,6 +288,19 @@ static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t co _ack_state = UBX_ACK_WAITING; } +// M10 ublox protocol info: +// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf +static void ubloxSendSetCfgU2(ubx_config_data16_payload_t *kvPairs, uint8_t count) +{ + ubx_config_data16_t cfg = {}; + + ubloxCfgFillU2(&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; +} + // 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 @@ -482,13 +495,38 @@ static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate) */ static void configureRATE(uint16_t measRate) { - send_buffer.message.header.msg_class = CLASS_CFG; - send_buffer.message.header.msg_id = MSG_CFG_RATE; - send_buffer.message.header.length = 6; - send_buffer.message.payload.rate.meas=measRate; - send_buffer.message.payload.rate.nav=1; - send_buffer.message.payload.rate.time=1; - sendConfigMessageUBLOX(); + if(ubloxVersionLT(24, 0)) { + measRate = MAX(50, measRate); + } else { + measRate = MAX(25, measRate); + } + + if (ubloxVersionLTE(23, 1)) { + send_buffer.message.header.msg_class = CLASS_CFG; + send_buffer.message.header.msg_id = MSG_CFG_RATE; + send_buffer.message.header.length = 6; + send_buffer.message.payload.rate.meas = measRate; + send_buffer.message.payload.rate.nav = 1; + send_buffer.message.payload.rate.time = 1; + sendConfigMessageUBLOX(); + } else { // M10+ + // 1 is already default, for TIMEREF. + // The wait the configuration happens, + // it is tricky to wait for multiple commands. + // SendSetCfg could be refactored to support U1, U2, U3 and U4 messages + // at the same time. For now, leave it out. + // + //ubx_config_data8_payload_t rateValues[] = { + // {UBLOX_CFG_RATE_TIMEREF, 1}, // 0 + //}; + //ubloxSendSetCfgBytes(rateValues, 1); + + ubx_config_data16_payload_t rate16Values[] = { + {UBLOX_CFG_RATE_MEAS, measRate}, + {UBLOX_CFG_RATE_NAV, 1} + }; + ubloxSendSetCfgU2(rate16Values, 2); + } } /* @@ -865,25 +903,57 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // Set dynamic model - switch (gpsState.gpsConfig->dynModel) { - case GPS_DYNMODEL_PEDESTRIAN: - configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO); - break; - case GPS_DYNMODEL_AUTOMOTIVE: - configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO); - break; - case GPS_DYNMODEL_AIR_1G: - configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO); - break; - case GPS_DYNMODEL_AIR_2G: // Default to this - default: - configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO); - break; - case GPS_DYNMODEL_AIR_4G: - configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO); - break; + if (ubloxVersionGTE(23, 1)) { + ubx_config_data8_payload_t dynmodelCfg[] = { + {UBLOX_CFG_NAVSPG_DYNMODEL, UBX_DYNMODEL_AIR_2G}, + {UBLOX_CFG_NAVSPG_FIXMODE, UBX_FIXMODE_AUTO} + }; + + switch (gpsState.gpsConfig->dynModel) { + case GPS_DYNMODEL_PEDESTRIAN: + dynmodelCfg[0].value = UBX_DYNMODEL_PEDESTRIAN; + ubloxSendSetCfgBytes(dynmodelCfg, 2); + break; + case GPS_DYNMODEL_AUTOMOTIVE: + dynmodelCfg[0].value = UBX_DYNMODEL_AUTOMOVITE; + ubloxSendSetCfgBytes(dynmodelCfg, 2); + break; + case GPS_DYNMODEL_AIR_1G: + dynmodelCfg[0].value = UBX_DYNMODEL_AIR_1G; + ubloxSendSetCfgBytes(dynmodelCfg, 2); + break; + case GPS_DYNMODEL_AIR_2G: // Default to this + default: + dynmodelCfg[0].value = UBX_DYNMODEL_AIR_2G; + ubloxSendSetCfgBytes(dynmodelCfg, 2); + break; + case GPS_DYNMODEL_AIR_4G: + dynmodelCfg[0].value = UBX_DYNMODEL_AIR_4G; + ubloxSendSetCfgBytes(dynmodelCfg, 2); + break; + } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } else { + switch (gpsState.gpsConfig->dynModel) { + case GPS_DYNMODEL_PEDESTRIAN: + configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO); + break; + case GPS_DYNMODEL_AUTOMOTIVE: + configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO); + break; + case GPS_DYNMODEL_AIR_1G: + configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO); + break; + case GPS_DYNMODEL_AIR_2G: // Default to this + default: + configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO); + break; + case GPS_DYNMODEL_AIR_4G: + configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO); + break; + } + ptWait(_ack_state == UBX_ACK_GOT_ACK); } - ptWait(_ack_state == UBX_ACK_GOT_ACK); gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // Disable NMEA messages @@ -935,7 +1005,7 @@ STATIC_PROTOTHREAD(gpsConfigure) ubloxSendSetCfgBytes(rateValues, 7); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - } else if(ubloxVersionGTE(15,0)) { // M8 and potentially M7, PVT, NAV_SAT, old setting API + } else if(ubloxVersionGTE(15,0)) { // M8, PVT, NAV_SAT, old setting API configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -960,6 +1030,7 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); } else { // Really old stuff, consider upgrading :), ols setting API, no PVT or NAV_SAT or NAV_SIG + // TODO: remove in INAV 9.0.0 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -1006,7 +1077,7 @@ 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 >= UBX_HW_VERSION_UBLOX8) { + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { // TODO: This check can be remove in INAV 9.0.0 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); bool use_VALSET = 0; if (ubloxVersionGTE(23,1)) { @@ -1030,7 +1101,8 @@ STATIC_PROTOTHREAD(gpsConfigure) for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) { - satelites[i].svId = 0xFF; // no used + // Mark satelites as unused + satelites[i].svId = 0xFF; satelites[i].gnssId = 0xFF; } diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 287124581d4..ab0ab930275 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -41,11 +41,18 @@ extern "C" { STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); +#define UBX_DYNMODEL_PORTABLE 0 +#define UBX_DYNMODEL_STATIONARY 2 #define UBX_DYNMODEL_PEDESTRIAN 3 #define UBX_DYNMODEL_AUTOMOVITE 4 +#define UBX_DYNMODEL_SEA 5 #define UBX_DYNMODEL_AIR_1G 6 #define UBX_DYNMODEL_AIR_2G 7 #define UBX_DYNMODEL_AIR_4G 8 +#define UBX_DYNMODEL_WRIST 9 +#define UBX_DYNMODEL_BIKE 10 +#define UBX_DYNMODEL_MOWER 11 +#define UBX_DYNMODEL_ESCOOTER 12 #define UBX_FIXMODE_2D_ONLY 1 #define UBX_FIXMODE_3D_ONLY 2 @@ -75,6 +82,13 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); #define UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1 0x209100c0 // U1 #define UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1 0x209100ac // U1 #define UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1 0x209100b1 // U1 +#define UBLOX_CFG_NAVSPG_FIXMODE 0x20110011 // E1 +#define UBLOX_CFG_NAVSPG_DYNMODEL 0x20110021 // E1 +#define UBLOX_CFG_RATE_MEAS 0x30210001 // U2 +#define UBLOX_CFG_RATE_NAV 0x30210002 // U2 +#define UBLOX_CFG_RATE_TIMEREF 0x30210002 // E1 + + #define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1 #define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1 @@ -272,6 +286,13 @@ typedef struct { uint8_t value; } __attribute__((packed)) ubx_config_data8_payload_t; +typedef struct { + uint32_t key; + uint16_t value; +} __attribute__((packed)) ubx_config_data16_payload_t; + + + #define MAX_CONFIG_SET_VAL_VALUES 32 @@ -284,6 +305,17 @@ typedef struct { } data; } __attribute__((packed)) ubx_config_data8_t; +typedef struct { + ubx_header header; + ubx_config_data_header_v1_t configHeader; + union { + ubx_config_data16_payload_t payload[0]; + uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data16_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes + } data; +} __attribute__((packed)) ubx_config_data16_t; + + + typedef struct { ubx_header header; ubx_payload payload; diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index a2adf1df18b..c9c1682a5f7 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -48,7 +48,36 @@ int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPai for (int i = 0; i < count; ++i) { - cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key); + cfg->data.payload[i].key = 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; +} + +int ubloxCfgFillU2(ubx_config_data16_t *cfg, ubx_config_data16_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_v1_t) + ((sizeof(ubx_config_data16_payload_t) * count)); + cfg->configHeader.layers = 0x1; + cfg->configHeader.transaction = 0; + cfg->configHeader.reserved = 0; + cfg->configHeader.version = 1; + + for (int i = 0; i < count; ++i) { + cfg->data.payload[i].key = kvPairs[i].key; cfg->data.payload[i].value = kvPairs[i].value; } diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h index 502bbaf31a6..996cbe2a68c 100644 --- a/src/main/io/gps_ublox_utils.h +++ b/src/main/io/gps_ublox_utils.h @@ -26,6 +26,7 @@ extern "C" { #endif int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count); +int ubloxCfgFillU2(ubx_config_data16_t *cfg, ubx_config_data16_payload_t *kvPairs, uint8_t count); void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b);