Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: tidy configuration of UBLOX rates

This commit is contained in:
Peter Barker 2022-01-08 19:00:52 +11:00 committed by Andrew Tridgell
parent 852546f8cf
commit 1943de9016
1 changed files with 38 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -404,126 +404,86 @@ AP_GPS_UBLOX::_request_next_config(void)
void void
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
uint8_t desired_rate; uint8_t desired_rate;
uint32_t config_msg_id;
switch(msg_class) { switch(msg_class) {
case CLASS_NAV: case CLASS_NAV:
switch(msg_id) { switch(msg_id) {
case MSG_POSLLH: case MSG_POSLLH:
desired_rate = havePvtMsg ? 0 : RATE_POSLLH; desired_rate = havePvtMsg ? 0 : RATE_POSLLH;
if(rate == desired_rate) { config_msg_id = CONFIG_RATE_POSLLH;
_unconfigured_messages &= ~CONFIG_RATE_POSLLH;
} else {
_configure_message_rate(msg_class, msg_id, desired_rate);
_unconfigured_messages |= CONFIG_RATE_POSLLH;
_cfg_needs_save = true;
}
break; break;
case MSG_STATUS: case MSG_STATUS:
desired_rate = havePvtMsg ? 0 : RATE_STATUS; desired_rate = havePvtMsg ? 0 : RATE_STATUS;
if(rate == desired_rate) { config_msg_id = CONFIG_RATE_STATUS;
_unconfigured_messages &= ~CONFIG_RATE_STATUS;
} else {
_configure_message_rate(msg_class, msg_id, desired_rate);
_unconfigured_messages |= CONFIG_RATE_STATUS;
_cfg_needs_save = true;
}
break; break;
case MSG_SOL: case MSG_SOL:
desired_rate = havePvtMsg ? 0 : RATE_SOL; desired_rate = havePvtMsg ? 0 : RATE_SOL;
if(rate == desired_rate) { config_msg_id = CONFIG_RATE_SOL;
_unconfigured_messages &= ~CONFIG_RATE_SOL;
} else {
_configure_message_rate(msg_class, msg_id, desired_rate);
_unconfigured_messages |= CONFIG_RATE_SOL;
_cfg_needs_save = true;
}
break; break;
case MSG_PVT: case MSG_PVT:
if(rate == RATE_PVT) { desired_rate = RATE_PVT;
_unconfigured_messages &= ~CONFIG_RATE_PVT; config_msg_id = CONFIG_RATE_PVT;
} else {
_configure_message_rate(msg_class, msg_id, RATE_PVT);
_unconfigured_messages |= CONFIG_RATE_PVT;
_cfg_needs_save = true;
}
break; break;
case MSG_TIMEGPS: case MSG_TIMEGPS:
if(rate == RATE_TIMEGPS) { desired_rate = RATE_TIMEGPS;
_unconfigured_messages &= ~CONFIG_RATE_TIMEGPS; config_msg_id = CONFIG_RATE_TIMEGPS;
} else {
_configure_message_rate(msg_class, msg_id, RATE_TIMEGPS);
_unconfigured_messages |= CONFIG_RATE_TIMEGPS;
_cfg_needs_save = true;
}
break; break;
case MSG_VELNED: case MSG_VELNED:
desired_rate = havePvtMsg ? 0 : RATE_VELNED; desired_rate = havePvtMsg ? 0 : RATE_VELNED;
if(rate == desired_rate) { config_msg_id = CONFIG_RATE_VELNED;
_unconfigured_messages &= ~CONFIG_RATE_VELNED;
} else {
_configure_message_rate(msg_class, msg_id, desired_rate);
_unconfigured_messages |= CONFIG_RATE_VELNED;
_cfg_needs_save = true;
}
break; break;
case MSG_DOP: case MSG_DOP:
if(rate == RATE_DOP) { desired_rate = RATE_DOP;
_unconfigured_messages &= ~CONFIG_RATE_DOP; config_msg_id = CONFIG_RATE_DOP;
} else {
_configure_message_rate(msg_class, msg_id, RATE_DOP);
_unconfigured_messages |= CONFIG_RATE_DOP;
_cfg_needs_save = true;
}
break; break;
default:
return;
} }
break; break;
case CLASS_MON: case CLASS_MON:
switch(msg_id) { switch(msg_id) {
case MSG_MON_HW: case MSG_MON_HW:
if(rate == RATE_HW) { desired_rate = RATE_HW;
_unconfigured_messages &= ~CONFIG_RATE_MON_HW; config_msg_id = CONFIG_RATE_MON_HW;
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW);
_unconfigured_messages |= CONFIG_RATE_MON_HW;
_cfg_needs_save = true;
}
break; break;
case MSG_MON_HW2: case MSG_MON_HW2:
if(rate == RATE_HW2) { desired_rate = RATE_HW2;
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2; config_msg_id = CONFIG_RATE_MON_HW2;
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW2);
_unconfigured_messages |= CONFIG_RATE_MON_HW2;
_cfg_needs_save = true;
}
break; break;
default:
return;
} }
break; break;
#if UBLOX_RXM_RAW_LOGGING #if UBLOX_RXM_RAW_LOGGING
case CLASS_RXM: case CLASS_RXM:
switch(msg_id) { switch(msg_id) {
case MSG_RXM_RAW: case MSG_RXM_RAW:
if(rate == gps._raw_data) { desired_rate = gps._raw_data;
_unconfigured_messages &= ~CONFIG_RATE_RAW; config_msg_id = CONFIG_RATE_RAW;
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
_cfg_needs_save = true;
}
break; break;
case MSG_RXM_RAWX: case MSG_RXM_RAWX:
if(rate == gps._raw_data) { desired_rate = gps._raw_data;
_unconfigured_messages &= ~CONFIG_RATE_RAW; config_msg_id = CONFIG_RATE_RAW;
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
_cfg_needs_save = true;
}
break; break;
default:
return;
} }
break; break;
#endif // UBLOX_RXM_RAW_LOGGING #endif // UBLOX_RXM_RAW_LOGGING
default:
return;
} }
if (rate == desired_rate) {
// coming in at correct rate; mark as configured
_unconfigured_messages &= ~config_msg_id;
return;
}
// coming in at wrong rate; try to configure it
_configure_message_rate(msg_class, msg_id, desired_rate);
_unconfigured_messages |= config_msg_id;
_cfg_needs_save = true;
} }
// Requests the ublox driver to identify what port we are using to communicate // Requests the ublox driver to identify what port we are using to communicate