diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 356a3adc89..90f258409d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -39,6 +39,8 @@ AP_GPS_UBLOX::init(enum GPS_Engine_Setting nav_setting) // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the // right reporting configuration. + Debug("uBlox nav_setting=%u\n", nav_setting); + _port->flush(); _epoch = TIME_OF_WEEK; @@ -180,6 +182,7 @@ AP_GPS_UBLOX::_parse_gps(void) } if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) { + Debug("Got engine settings %u\n", (unsigned)_buffer.nav_settings.dynModel); if (_nav_setting != GPS_ENGINE_NONE && _buffer.nav_settings.dynModel != _nav_setting) { // we've received the current nav settings, change the engine @@ -261,6 +264,12 @@ AP_GPS_UBLOX::_parse_gps(void) // this ensures we don't use stale data if (_new_position && _new_speed) { _new_speed = _new_position = false; + _fix_count++; + if (_fix_count == 100) { + // ask for nav settings every 20 seconds + Debug("Asking for engine setting\n"); + _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); + } return true; } return false; @@ -353,6 +362,7 @@ AP_GPS_UBLOX::_configure_gps(void) _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // ask for the current navigation settings + Debug("Asking for engine setting\n"); _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index be95d18ef5..824e05f7a5 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -164,6 +164,10 @@ private: uint16_t _payload_length; uint16_t _payload_counter; + // 8 bit count of fix messages processed, used for periodic + // processing + uint8_t _fix_count; + uint8_t _class; // do we have new position information?