voted_sensors_update: parameter CAL_*_EN enables sensor with id CAL_*_ID

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