Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: stop auto replacing missing CAN by default, add option to re-enable

This commit is contained in:
Iampete1 2023-06-18 19:19:39 +01:00 committed by Andrew Tridgell
parent 97541f82d0
commit 262b11f0c1
3 changed files with 52 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
libraries/AP_Compass/AP_Compass.cpp
View File

@ -588,6 +588,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass options
// @Description: This sets options to change the behaviour of the compass
// @Bitmask: 0:CalRequireGPS
// @Bitmask: 1: Allow missing DroneCAN compasses to be automaticaly replaced (calibration still required)
// @User: Advanced
AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0),
@ -1464,56 +1465,58 @@ void Compass::_detect_backends(void)
}
#if COMPASS_MAX_UNREG_DEV > 0
// check if there's any uavcan compass in prio slot that's not found
// and replace it if there's a replacement compass
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
|| _get_state(i).registered) {
continue;
}
// There's a UAVCAN compass missing
// Let's check if there's a replacement
for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
// Check if this is a potential replacement mag
if (!is_replacement_mag(detected_devid)) {
if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT)) {
// check if there's any uavcan compass in prio slot that's not found
// and replace it if there's a replacement compass
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
|| _get_state(i).registered) {
continue;
}
// We have found a replacement mag, let's replace the existing one
// with this by setting the priority to zero and calling uavcan probe
gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
_priority_did_stored_list[i].set_and_save(0);
_priority_did_list[i] = 0;
AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
// we also need to remove the id from unreg list
remove_unreg_dev_id(detected_devid);
} else {
// the mag has already been allocated,
// let's begin the replacement
bool found_replacement = false;
for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
if ((uint32_t)_state[k].dev_id == detected_devid) {
if (_state[k].priority <= uint8_t(i)) {
// we are already on higher priority
// nothing to do
break;
}
found_replacement = true;
// reset old priority of replacement mag
_priority_did_stored_list[_state[k].priority].set_and_save(0);
_priority_did_list[_state[k].priority] = 0;
// update new priority
_state[k].priority = i;
}
}
if (!found_replacement) {
// There's a UAVCAN compass missing
// Let's check if there's a replacement
for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
// Check if this is a potential replacement mag
if (!is_replacement_mag(detected_devid)) {
continue;
}
_priority_did_stored_list[i].set_and_save(detected_devid);
_priority_did_list[i] = detected_devid;
// We have found a replacement mag, let's replace the existing one
// with this by setting the priority to zero and calling uavcan probe
gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
_priority_did_stored_list[i].set_and_save(0);
_priority_did_list[i] = 0;
AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
// we also need to remove the id from unreg list
remove_unreg_dev_id(detected_devid);
} else {
// the mag has already been allocated,
// let's begin the replacement
bool found_replacement = false;
for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
if ((uint32_t)_state[k].dev_id == detected_devid) {
if (_state[k].priority <= uint8_t(i)) {
// we are already on higher priority
// nothing to do
break;
}
found_replacement = true;
// reset old priority of replacement mag
_priority_did_stored_list[_state[k].priority].set_and_save(0);
_priority_did_list[_state[k].priority] = 0;
// update new priority
_state[k].priority = i;
}
}
if (!found_replacement) {
continue;
}
_priority_did_stored_list[i].set_and_save(detected_devid);
_priority_did_list[i] = detected_devid;
}
}
}
}

2
libraries/AP_Compass/AP_Compass.h
View File

@ -607,7 +607,9 @@ private:
// bitmask of options
enum class Option : uint16_t {
CAL_REQUIRE_GPS = (1U<<0),
ALLOW_DRONECAN_AUTO_REPLACEMENT = (1U<<1),
};
bool option_set(Option opt) const { return (_options.get() & uint16_t(opt)) != 0; }
AP_Int16 _options;
#if COMPASS_CAL_ENABLED

2
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -70,7 +70,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
}
}
if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
if (option_set(Option::CAL_REQUIRE_GPS)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
return false;