aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS_UBLOX: save cfg only when needed

Browse Source 
added GPS_SAVE_CFG param option 2
This commit is contained in:
Tom Pittenger 2016-02-12 12:32:08 -08:00
parent 55e64b3622
commit 18163bd01f
2 changed files with 25 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -66,6 +66,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_disable_counter(0),
next_fix(AP_GPS::NO_FIX),
_last_5hz_time(0),
_cfg_needs_save(false),
noReceivedHdop(true)
{
// stop any config strings that are pending
@ -212,6 +213,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_POSLLH);
_unconfigured_messages |= CONFIG_RATE_POSLLH;
_cfg_needs_save = true;
}
break;
case MSG_STATUS:
@ -220,6 +222,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_STATUS);
_unconfigured_messages |= CONFIG_RATE_STATUS;
_cfg_needs_save = true;
}
break;
case MSG_SOL:
@ -228,6 +231,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_SOL);
_unconfigured_messages |= CONFIG_RATE_SOL;
_cfg_needs_save = true;
}
break;
case MSG_VELNED:
@ -236,6 +240,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_VELNED);
_unconfigured_messages |= CONFIG_RATE_VELNED;
_cfg_needs_save = true;
}
break;
case MSG_DOP:
@ -244,6 +249,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_DOP);
_unconfigured_messages |= CONFIG_RATE_DOP;
_cfg_needs_save = true;
}
break;
}
@ -256,6 +262,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW);
_unconfigured_messages |= CONFIG_RATE_MON_HW;
_cfg_needs_save = true;
}
break;
case MSG_MON_HW2:
@ -264,6 +271,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, RATE_HW2);
_unconfigured_messages |= CONFIG_RATE_MON_HW2;
_cfg_needs_save = true;
}
break;
}
@ -277,6 +285,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
_cfg_needs_save = true;
}
break;
case MSG_RXM_RAWX:
@ -285,6 +294,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
} else {
_configure_message_rate(msg_class, msg_id, gps._raw_data);
_unconfigured_messages |= CONFIG_RATE_RAW;
_cfg_needs_save = true;
}
break;
}
@ -336,11 +346,14 @@ AP_GPS_UBLOX::read(void)
}
}
if(!_unconfigured_messages && gps._save_config &&
if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed()) {
//save the configuration sent until now
_save_cfg();
if (gps._save_config == 1 ||
(gps._save_config == 2 && _cfg_needs_save)) {
_save_cfg();
}
}
numc = port->available();
@ -583,6 +596,7 @@ AP_GPS_UBLOX::_parse_gps(void)
switch(_buffer.ack.msgID) {
case MSG_CFG_CFG:
_cfg_saved = true;
_cfg_needs_save = false;
break;
case MSG_CFG_GNSS:
_unconfigured_messages &= ~CONFIG_GNSS;
@ -646,6 +660,7 @@ AP_GPS_UBLOX::_parse_gps(void)
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
}
@ -697,6 +712,7 @@ AP_GPS_UBLOX::_parse_gps(void)
if (!memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
_unconfigured_messages |= CONFIG_GNSS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
@ -720,6 +736,7 @@ AP_GPS_UBLOX::_parse_gps(void)
&_buffer.sbas,
sizeof(_buffer.sbas));
_unconfigured_messages |= CONFIG_SBAS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
@ -751,6 +768,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_buffer.nav_rate.timeref != 0) {
_configure_rate();
_unconfigured_messages |= CONFIG_RATE_NAV;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
@ -1082,6 +1100,9 @@ AP_GPS_UBLOX::_save_cfg()
_send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
_last_cfg_sent_time = AP_HAL::millis();
_num_cfg_save_tries++;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO,
"GPS: u-blox %d saving config",
state.instance);
}
/*

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -506,6 +506,8 @@ private:
uint32_t _last_5hz_time;
bool _cfg_needs_save;
bool noReceivedHdop;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);