mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: prevent saving of device IDs when not calibrated
this fixes an issue with the following sequence: - new board (or board with FORMAT_VERSION reset) starts up with only internal compasses - internal compasses are detected and devids saved - an external compass is added and the board is rebooted - the external compass will not be the first compass - user then calibrates and flies, but has internal as primary this can lead to a very bad experience for new users. At least one vehicle has crashed due to this sequence. The fix is to not save device IDs during the Compass::init() if we have never been calibrated. This means that when an external compass is added it will come up as the first compass. This also removes the saving of the extra device ID. It was never intended that these be saved (there is a comment to that effect in the code), but actually they were saved.
This commit is contained in:
parent
aaeee2e7dc
commit
9fc9238c53
|
@ -697,6 +697,25 @@ void Compass::init()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
on init() if any devid is set then we set suppress_devid_save to
|
||||||
|
false. This is used to determine if we save device ids during
|
||||||
|
the init process.
|
||||||
|
*/
|
||||||
|
suppress_devid_save = true;
|
||||||
|
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if (_state._priv_instance[i].dev_id != 0) {
|
||||||
|
suppress_devid_save = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
#if COMPASS_MAX_INSTANCES > 1
|
||||||
|
if (_priority_did_stored_list._priv_instance[i] != 0) {
|
||||||
|
suppress_devid_save = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
// convert to new custom rotation method
|
// convert to new custom rotation method
|
||||||
// PARAMETER_CONVERSION - Added: Nov-2021
|
// PARAMETER_CONVERSION - Added: Nov-2021
|
||||||
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
|
||||||
|
@ -741,6 +760,7 @@ void Compass::init()
|
||||||
|
|
||||||
// Load priority list from storage, the changes to priority list
|
// Load priority list from storage, the changes to priority list
|
||||||
// by user only take effect post reboot, after this
|
// by user only take effect post reboot, after this
|
||||||
|
if (!suppress_devid_save) {
|
||||||
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
if (_priority_did_stored_list[i] != 0) {
|
if (_priority_did_stored_list[i] != 0) {
|
||||||
_priority_did_list[i] = _priority_did_stored_list[i];
|
_priority_did_list[i] = _priority_did_stored_list[i];
|
||||||
|
@ -762,6 +782,7 @@ void Compass::init()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
#endif // COMPASS_MAX_INSTANCES
|
#endif // COMPASS_MAX_INSTANCES
|
||||||
|
|
||||||
// cache expected dev ids for use during runtime detection
|
// cache expected dev ids for use during runtime detection
|
||||||
|
@ -777,7 +798,7 @@ void Compass::init()
|
||||||
// cache the extra devices detected in last boot
|
// cache the extra devices detected in last boot
|
||||||
// for detecting replacement mag
|
// for detecting replacement mag
|
||||||
_previously_unreg_mag[i] = extra_dev_id[i];
|
_previously_unreg_mag[i] = extra_dev_id[i];
|
||||||
extra_dev_id[i].set_and_save(0);
|
extra_dev_id[i].set(0);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -786,7 +807,9 @@ void Compass::init()
|
||||||
// which are set() but not saved() during normal runtime,
|
// which are set() but not saved() during normal runtime,
|
||||||
// do not move this call without ensuring that is not happening
|
// do not move this call without ensuring that is not happening
|
||||||
// read comments under set_and_save_ifchanged for details
|
// read comments under set_and_save_ifchanged for details
|
||||||
|
if (!suppress_devid_save) {
|
||||||
_reorder_compass_params();
|
_reorder_compass_params();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (_compass_count == 0) {
|
if (_compass_count == 0) {
|
||||||
|
@ -794,16 +817,6 @@ void Compass::init()
|
||||||
_detect_backends();
|
_detect_backends();
|
||||||
}
|
}
|
||||||
|
|
||||||
#if COMPASS_MAX_UNREG_DEV
|
|
||||||
// We store the list of unregistered mags detected here,
|
|
||||||
// We don't do this during runtime, as we don't want to detect
|
|
||||||
// compasses connected by user as a replacement while the system
|
|
||||||
// is running
|
|
||||||
for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
|
|
||||||
extra_dev_id[i].save();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (_compass_count != 0) {
|
if (_compass_count != 0) {
|
||||||
// get initial health status
|
// get initial health status
|
||||||
hal.scheduler->delay(100);
|
hal.scheduler->delay(100);
|
||||||
|
@ -826,6 +839,7 @@ void Compass::init()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
init_done = true;
|
init_done = true;
|
||||||
|
suppress_devid_save = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
|
#if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
|
||||||
|
@ -846,7 +860,11 @@ Compass::Priority Compass::_update_priority_list(int32_t dev_id)
|
||||||
// We are not in priority list, let's add at first empty
|
// We are not in priority list, let's add at first empty
|
||||||
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
if (_priority_did_stored_list[i] == 0) {
|
if (_priority_did_stored_list[i] == 0) {
|
||||||
|
if (suppress_devid_save) {
|
||||||
|
_priority_did_stored_list[i].set(dev_id);
|
||||||
|
} else {
|
||||||
_priority_did_stored_list[i].set_and_save(dev_id);
|
_priority_did_stored_list[i].set_and_save(dev_id);
|
||||||
|
}
|
||||||
_priority_did_list[i] = dev_id;
|
_priority_did_list[i] = dev_id;
|
||||||
if (i >= _compass_count) {
|
if (i >= _compass_count) {
|
||||||
_compass_count = uint8_t(i)+1;
|
_compass_count = uint8_t(i)+1;
|
||||||
|
@ -1463,7 +1481,7 @@ void Compass::_detect_backends(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
#if COMPASS_MAX_UNREG_DEV > 0
|
#if COMPASS_MAX_UNREG_DEV > 0
|
||||||
if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT)) {
|
if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT) && !suppress_devid_save) {
|
||||||
// check if there's any uavcan compass in prio slot that's not found
|
// check if there's any uavcan compass in prio slot that's not found
|
||||||
// and replace it if there's a replacement compass
|
// and replace it if there's a replacement compass
|
||||||
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
@ -1570,7 +1588,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
|
||||||
#if COMPASS_MAX_UNREG_DEV > 0
|
#if COMPASS_MAX_UNREG_DEV > 0
|
||||||
for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
|
for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
|
||||||
if ((uint32_t)extra_dev_id[i] == devid) {
|
if ((uint32_t)extra_dev_id[i] == devid) {
|
||||||
extra_dev_id[i].set_and_save(0);
|
extra_dev_id[i].set(0);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -2153,7 +2171,6 @@ void Compass::force_save_calibration(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// singleton instance
|
// singleton instance
|
||||||
Compass *Compass::_singleton;
|
Compass *Compass::_singleton;
|
||||||
|
|
||||||
|
|
|
@ -649,6 +649,8 @@ private:
|
||||||
#endif
|
#endif
|
||||||
bool init_done;
|
bool init_done;
|
||||||
|
|
||||||
|
bool suppress_devid_save;
|
||||||
|
|
||||||
uint8_t _first_usable; // first compass usable based on COMPASSx_USE param
|
uint8_t _first_usable; // first compass usable based on COMPASSx_USE param
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue