From 316a188e8ed3c0c337ff7029cbe973bbef098485 Mon Sep 17 00:00:00 2001 From: Dusan Zivkovic Date: Sat, 21 Sep 2019 19:20:31 +0200 Subject: [PATCH] voted_sensors_update: parameter CAL_*_EN enables sensor with id CAL_*_ID --- src/modules/sensors/voted_sensors_update.cpp | 22 ++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 8e37b54cfe..4aa32339d2 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -222,6 +222,7 @@ void VotedSensorsUpdate::parametersUpdate() /* run through all gyro sensors */ for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { + _gyro.enabled[driver_index] = true; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index); @@ -248,8 +249,6 @@ void VotedSensorsUpdate::parametersUpdate() int32_t device_enabled = 1; failed = failed || (OK != param_get(param_find(str), &device_enabled)); - _gyro.enabled[i] = (device_enabled == 1); - if (failed) { continue; } @@ -260,6 +259,8 @@ void VotedSensorsUpdate::parametersUpdate() /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == driver_device_id) { + _gyro.enabled[driver_index] = (device_enabled == 1); + if (!_gyro.enabled[driver_index]) { _gyro.priority[driver_index] = 0; } struct gyro_calibration_s gscale = {}; @@ -312,6 +313,7 @@ void VotedSensorsUpdate::parametersUpdate() /* run through all accel sensors */ for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) { + _accel.enabled[driver_index] = true; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index); @@ -338,8 +340,6 @@ void VotedSensorsUpdate::parametersUpdate() int32_t device_enabled = 1; failed = failed || (OK != param_get(param_find(str), &device_enabled)); - _accel.enabled[i] = (device_enabled == 1); - if (failed) { continue; } @@ -350,6 +350,8 @@ void VotedSensorsUpdate::parametersUpdate() /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == driver_device_id) { + _accel.enabled[driver_index] = (device_enabled == 1); + if (!_accel.enabled[driver_index]) { _accel.priority[driver_index] = 0; } struct accel_calibration_s ascale = {}; @@ -404,6 +406,14 @@ void VotedSensorsUpdate::parametersUpdate() * Because we store the device id in _mag_device_id, we need to get the id via uorb topic since * the DevHandle method does not work on POSIX. */ + + /* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers, + * and not all uORB instances have to be published yet at the initial call of parametersUpdate() + */ + for (int i = 0; i < MAG_COUNT_MAX; i++) { + _mag.enabled[i] = true; + } + for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX && topic_instance < _mag.subscription_count; ++topic_instance) { @@ -456,14 +466,14 @@ void VotedSensorsUpdate::parametersUpdate() int32_t device_enabled = 1; failed = failed || (OK != param_get(param_find(str), &device_enabled)); - _mag.enabled[i] = (device_enabled == 1); - if (failed) { continue; } /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == _mag_device_id[topic_instance]) { + _mag.enabled[topic_instance] = (device_enabled == 1); + // the mags that were published after the initial parameterUpdate // would be given the priority even if disabled. Reset it to 0 in this case if (!_mag.enabled[topic_instance]) { _mag.priority[topic_instance] = 0; }