Skip to content

Commit

Permalink
Merge pull request #10199 from iNavFlight/mmosca-ublox-satinfo
Browse files Browse the repository at this point in the history 
[GPS] Fix GPS dynamic model setup for M10 GPS units
  • Loading branch information
@mmosca
mmosca authored Jul 7, 2024
2 parents 64da4f4 + 1c0fb3f commit 8887da0
Show file tree
Hide file tree
Showing 4 changed files with 163 additions and 29 deletions.
128 changes: 100 additions & 28 deletions src/main/io/gps_ublox.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
}
}

/*
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);

Expand All @@ -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);

Expand Down Expand Up @@ -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)) {
Expand All @@ -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;
}

Expand Down
32 changes: 32 additions & 0 deletions src/main/io/gps_ublox.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand All @@ -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;
Expand Down
31 changes: 30 additions & 1 deletion src/main/io/gps_ublox_utils.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
1 change: 1 addition & 0 deletions src/main/io/gps_ublox_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down

0 comments on commit 8887da0

Please sign in to comment.