diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7d5642e137..3bdd872461 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -56,7 +56,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART next_fix(AP_GPS::NO_FIX), rate_update_step(0), _last_5hz_time(0), - noReceivedHdop(true) + noReceivedHdop(true), + _cfg_saved(false) { // stop any config strings that are pending gps.send_blob_start(state.instance, NULL, 0); @@ -156,6 +157,8 @@ AP_GPS_UBLOX::read(void) if (need_rate_update) { send_next_rate_update(); + }else if(!_cfg_saved) { //save the configuration sent until now + _save_cfg(); } numc = port->available(); @@ -395,6 +398,10 @@ AP_GPS_UBLOX::_parse_gps(void) { if (_class == CLASS_ACK) { Debug("ACK %u", (unsigned)_msg_id); + + if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) { + _cfg_saved = true; + } return false; } @@ -423,6 +430,7 @@ AP_GPS_UBLOX::_parse_gps(void) _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, &_buffer.nav_settings, sizeof(_buffer.nav_settings)); + _cfg_saved = false; //save configuration } return false; } @@ -485,6 +493,7 @@ AP_GPS_UBLOX::_parse_gps(void) _send_message(CLASS_CFG, MSG_CFG_SBAS, &_buffer.sbas, sizeof(_buffer.sbas)); + _cfg_saved = false; } } @@ -786,6 +795,18 @@ AP_GPS_UBLOX::_configure_gps(void) _send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0); } +/* + * + */ +void +AP_GPS_UBLOX::_save_cfg() +{ + ubx_cfg_cfg save_cfg; + save_cfg.clearMask = 0; + save_cfg.saveMask = SAVE_CFG_ALL; + save_cfg.loadMask = 0; + _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg)); +} /* detect a Ublox GPS. Adds one byte, and returns true if the stream diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 98399b05ea..9600be474e 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -50,6 +50,16 @@ #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7 #define UBX_MSG_TYPES 2 +//Configuration Sub-Sections +#define SAVE_CFG_IO (1<<0) +#define SAVE_CFG_MSG (1<<1) +#define SAVE_CFG_INF (1<<2) +#define SAVE_CFG_NAV (1<<3) +#define SAVE_CFG_RXM (1<<4) +#define SAVE_CFG_RINV (1<<9) +#define SAVE_CFG_ANT (1<<10) +#define SAVE_CFG_ALL (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT) + class AP_GPS_UBLOX : public AP_GPS_Backend { public: @@ -281,6 +291,19 @@ private: } svinfo[UBLOX_MAX_RXM_RAWX_SATS]; }; #endif + + struct PACKED ubx_ack_ack { + uint8_t clsID; + uint8_t msgID; + }; + + + struct PACKED ubx_cfg_cfg { + uint32_t clearMask; + uint32_t saveMask; + uint32_t loadMask; + }; + // Receive buffer union PACKED { ubx_nav_posllh posllh; @@ -303,6 +326,7 @@ private: ubx_rxm_raw rxm_raw; ubx_rxm_rawx rxm_rawx; #endif + ubx_ack_ack ack; uint8_t bytes[]; } _buffer; @@ -321,6 +345,7 @@ private: MSG_DOP = 0x4, MSG_SOL = 0x6, MSG_VELNED = 0x12, + MSG_CFG_CFG = 0x09, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, @@ -376,6 +401,7 @@ private: // processing uint8_t _fix_count; uint8_t _class; + bool _cfg_saved; uint32_t _last_vel_time; uint32_t _last_pos_time; @@ -407,6 +433,7 @@ private: void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size); void send_next_rate_update(void); void _request_version(void); + void _save_cfg(void); void unexpected_message(void); void write_logging_headers(void);