/*
send the next step of rate updates to the GPS. This reconfigures the
GPS on the fly to have the right message rates. It needs to be
careful to only send a message if there is sufficient buffer space
available on the serial port to avoid it blocking the CPU
*/
void
AP_GPS_UBLOX::send_next_rate_update(void)
{
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
//hal.console->printf_P(PSTR("next_rate: %u\n"), (unsigned)rate_update_step);
switch (rate_update_step++) {
case 0:
_configure_navigation_rate(200);
break;
case 1:
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); // 28+8 bytes
break;
case 2:
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1); // 16+8 bytes
break;
case 3:
_configure_message_rate(CLASS_NAV, MSG_DOP, 1);
break;
case 4:
_configure_message_rate(CLASS_NAV, MSG_SOL, 1); // 52+8 bytes
break;
case 5:
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // 36+8 bytes
break;
#if UBLOX_HW_LOGGING
case 6:
// gather MON_HW at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW, 2); // 64+8 bytes
break;
case 7:
// gather MON_HW2 at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
break;
#endif
#if UBLOX_VERSION_AUTODETECTION
case 8:
_request_version();
break;
#endif
default:
need_rate_update = false;
rate_update_step = 0;
break;
}
}
void
AP_GPS_UBLOX::_request_next_config(void)
{
// don't request config if we shouldn't configure the GPS
if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
return;
}
// Ensure there is enough space for the largest possible outgoing message
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
Debug("Unconfigured messages: %u Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
// check AP_GPS_UBLOX.h for the enum that controls the order.
// This switch statement isn't maintained against the enum in order to reduce code churn
switch (_next_message++) {
case STEP_PVT:
if(!_request_message_rate(CLASS_NAV, MSG_PVT)) {
_next_message--;
}
break;
case STEP_PORT:
_request_port();
break;
case STEP_POLL_SVINFO:
// not required once we know what generation we are on
if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
_next_message--;
}
}
break;
case STEP_POLL_SBAS:
if (gps._sbas_mode != 2) {
_send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
break;
case STEP_POLL_NAV:
if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) {
_next_message--;
}
break;
case STEP_POLL_GNSS:
if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) {
_next_message--;
}
break;
case STEP_POLL_TP5:
#if CONFIGURE_PPS_PIN
if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
_next_message--;
}
#endif
break;
case STEP_NAV_RATE:
if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
_next_message--;
}
break;
case STEP_POSLLH:
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
_next_message--;
}
break;
case STEP_STATUS:
if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
_next_message--;
}
break;
case STEP_SOL:
if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
_next_message--;
}
break;
case STEP_VELNED:
if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
_next_message--;
}
break;
case STEP_DOP:
if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
_next_message--;
}
break;
case STEP_MON_HW:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
_next_message--;
}
break;
case STEP_MON_HW2:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
_next_message--;
}
break;
case STEP_RAW:
#if UBLOX_RXM_RAW_LOGGING
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
_next_message--;
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
break;
case STEP_RAWX:
#if UBLOX_RXM_RAW_LOGGING
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
_next_message--;
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
break;
case STEP_VERSION:
if(!_have_version && !hal.util->get_soft_armed()) {
_request_version();
} else {
_unconfigured_messages &= ~CONFIG_VERSION;
}
// no need to send the initial rates, move to checking only
_next_message = STEP_PVT;
break;
default:
// this case should never be reached, do a full reset if it is hit
_next_message = STEP_PVT;
break;
}
}
void
AP_GPS_UBLOX::_request_next_config(void)
{
// don't request config if we shouldn't configure the GPS
if (gps._auto_config == 0) {
return;
}
// Ensure there is enough space for the largest possible outgoing message
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);
switch (_next_message++) {
case STEP_RATE_NAV:
_configure_rate();
break;
case STEP_RATE_POSLLH:
if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
_next_message--;
}
break;
case STEP_RATE_VELNED:
if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
_next_message--;
}
break;
case STEP_PORT:
_request_port();
break;
case STEP_POLL_SVINFO:
// not required once we know what generation we are on
if(_hardware_generation == 0) {
_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0);
}
break;
case STEP_POLL_SBAS:
_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
break;
case STEP_POLL_NAV:
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
break;
case STEP_POLL_GNSS:
_send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
break;
case STEP_NAV_RATE:
_send_message(CLASS_CFG, MSG_CFG_RATE, NULL, 0);
break;
case STEP_POSLLH:
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
_next_message--;
}
break;
case STEP_STATUS:
if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
_next_message--;
}
break;
case STEP_SOL:
if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
_next_message--;
}
break;
case STEP_VELNED:
if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
_next_message--;
}
break;
case STEP_DOP:
if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
_next_message--;
}
break;
case STEP_MON_HW:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
_next_message--;
}
break;
case STEP_MON_HW2:
if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
_next_message--;
}
break;
case STEP_RAW:
#if UBLOX_RXM_RAW_LOGGING
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
_next_message--;
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
break;
case STEP_RAWX:
#if UBLOX_RXM_RAW_LOGGING
if(gps._raw_data == 0) {
_unconfigured_messages &= ~CONFIG_RATE_RAW;
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
_next_message--;
}
#else
_unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
break;
case STEP_VERSION:
if(!_have_version && !hal.util->get_soft_armed()) {
_request_version();
} else {
_unconfigured_messages &= ~CONFIG_VERSION;
}
// no need to send the initial rates, move to checking only
_next_message = STEP_PORT;
break;
default:
// this case should never be reached, do a full reset if it is hit
_next_message = STEP_RATE_NAV;
break;
}
}
