voted_sensors_update: set priority properly in case of late mag subscriptions or sensors enabled param changes

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);