Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: correct edge case where checks pass when saved dev_id != detected dev_id

Browse Source
This commit is contained in:
Jonathan Challinger 2018-03-19 16:58:40 -07:00 committed by Andrew Tridgell
parent ee9cc28fda
commit 878e84a015
3 changed files with 21 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Compass/AP_Compass.cpp
View File

@ -471,6 +471,13 @@ Compass::Compass(void)
}
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
// Reset dev_id parameters' cached values to zero. This is here to address a potential
// edge case where a previously-detected compass is no longer present, but its dev_id
// is loaded from the parameter's stored value.
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
_state[i].dev_id = 0;
}
}
// Default init method
@ -1084,6 +1091,7 @@ void
Compass::save_offsets(uint8_t i)
{
_state[i].offset.save(); // save offsets
_state[i].dev_id.set_and_save(_state[i].detected_dev_id);
_state[i].dev_id.save(); // save device id corresponding to these offsets
}
@ -1207,16 +1215,21 @@ bool Compass::configured(uint8_t i)
return false;
}
// backup detected dev_id
int32_t dev_id_orig = _state[i].dev_id;
// exit immediately if dev_id hasn't been detected
if (_state[i].detected_dev_id == 0) {
return false;
}
// back up cached value of dev_id
int32_t dev_id_cache_value = _state[i].dev_id;
// load dev_id from eeprom
_state[i].dev_id.load();
// if different then the device has not been configured
if (_state[i].dev_id != dev_id_orig) {
// restore device id
_state[i].dev_id = dev_id_orig;
// if dev_id loaded from eeprom is different from detected dev id or dev_id loaded from eeprom is different from cached dev_id, compass is unconfigured
if (_state[i].dev_id != _state[i].detected_dev_id || _state[i].dev_id != dev_id_cache_value) {
// restore cached value
_state[i].dev_id = dev_id_cache_value;
// return failure
return false;
}

1
libraries/AP_Compass/AP_Compass.h
View File

@ -431,6 +431,7 @@ private:
// saved to eeprom when offsets are saved allowing ram &
// eeprom values to be compared as consistency check
AP_Int32 dev_id;
int32_t detected_dev_id;
AP_Int8 use_for_yaw;

1
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -130,6 +130,7 @@ uint8_t AP_Compass_Backend::register_compass(void) const
void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
{
_compass._state[instance].dev_id.set_and_notify(dev_id);
_compass._state[instance].detected_dev_id = dev_id;
}
/*