dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: if multi-mag disabled (EKF2_MULTI_MAG <= 1) properly re-enable sensors hub selection (SENS_MAG_MODE) 

- this prevents potential misconfiguration if trying to disable ekf2 multi mag
This commit is contained in:
Daniel Agar 2022-06-27 12:34:58 -04:00
parent 8ccd8fbed1
commit e5be0e776e
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/ekf2/EKF2.cpp
View File

@ -2097,17 +2097,29 @@ int EKF2::task_spawn(int argc, char *argv[])
}
int32_t sens_mag_mode = 1;
param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE");
param_get(param_sens_mag_mode, &sens_mag_mode);
if (sens_mag_mode == 0) {
param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
const param_t param_ekf2_mult_mag = param_find("EKF2_MULTI_MAG");
param_get(param_ekf2_mult_mag, &mag_instances);
// Mags (1 - 4 supported)
if (mag_instances < 1 || mag_instances > 4) {
if (mag_instances > 4) {
const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited);
param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
param_set_no_notification(param_ekf2_mult_mag, &mag_instances_limited);
mag_instances = mag_instances_limited;
} else if (mag_instances <= 1) {
// properly disable multi-magnetometer at sensors hub level
PX4_WARN("EKF2_MULTI_MAG disabled, resetting SENS_MAG_MODE");
// re-enable at sensors level
sens_mag_mode = 1;
param_set(param_sens_mag_mode, &sens_mag_mode);
mag_instances = 1;
}
} else {