From d3e79c4726d80afd2c903e2edfdcc2f25a40549e Mon Sep 17 00:00:00 2001 From: Dusan Zivkovic Date: Sat, 21 Sep 2019 18:48:46 +0200 Subject: [PATCH] voted_sensors_update: set priority properly in case of late mag subscriptions or sensors enabled param changes --- src/modules/sensors/voted_sensors_update.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 9014febc10..8e37b54cfe 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -260,6 +260,8 @@ void VotedSensorsUpdate::parametersUpdate() /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == driver_device_id) { + if (!_gyro.enabled[driver_index]) { _gyro.priority[driver_index] = 0; } + struct gyro_calibration_s gscale = {}; (void)sprintf(str, "CAL_GYRO%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); @@ -348,6 +350,8 @@ void VotedSensorsUpdate::parametersUpdate() /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == driver_device_id) { + if (!_accel.enabled[driver_index]) { _accel.priority[driver_index] = 0; } + struct accel_calibration_s ascale = {}; (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &ascale.x_offset)); @@ -460,6 +464,10 @@ void VotedSensorsUpdate::parametersUpdate() /* if the calibration is for this device, apply it */ if ((uint32_t)device_id == _mag_device_id[topic_instance]) { + // 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; } + struct mag_calibration_s mscale = {}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &mscale.x_offset)); @@ -768,6 +776,15 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) /* force a scale and offset update the first time we get data */ parametersUpdate(); + + if (!_mag.enabled[uorb_index]) { + /* in case the data on the mag topic comes after the initial parameterUpdate(), we would get here since the sensor + * is enabled by default. The latest parameterUpdate() call would set enabled to false and reset priority to zero + * for disabled sensors, and we shouldn't cal voter.put() in that case + */ + continue; + } + } Vector3f vect(mag_report.x, mag_report.y, mag_report.z);