sensors: skip selection and failover checks during parameter update cycles

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -127,7 +127,7 @@ void VehicleAirData::AirTemperatureUpdate()
 	}
 }
 
-void VehicleAirData::ParametersUpdate()
+bool VehicleAirData::ParametersUpdate()
 {
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated()) {
@@ -136,7 +136,10 @@ void VehicleAirData::ParametersUpdate()
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
+		return true;
 	}
+
+	return false;
 }
 
 void VehicleAirData::Run()
@@ -145,7 +148,7 @@ void VehicleAirData::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
-	ParametersUpdate();
+	const bool parameter_update = ParametersUpdate();
 
 	SensorCorrectionsUpdate();
 
@@ -203,7 +206,8 @@ void VehicleAirData::Run()
 	_voter.get_best(time_now_us, &best_index);
 
 	if (best_index >= 0) {
-		if (_selected_sensor_sub_index != best_index) {
+		// handle selection change (don't process on same iteration as parameter update)
+		if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
 			// clear all registered callbacks
 			for (auto &sub : _sensor_sub) {
 				sub.unregisterCallback();
@@ -281,8 +285,8 @@ void VehicleAirData::Run()
 		}
 	}
 
-	// check failover and report
+	// check failover and report (save failover report for a cycle where parameters didn't update)
-	if (_last_failover_count != _voter.failover_count()) {
+	if (_last_failover_count != _voter.failover_count() && !parameter_update) {
 		uint32_t flags = _voter.failover_state();
 		int failover_index = _voter.failover_index();
 

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,7 +71,7 @@ public:
 private:
 	void Run() override;
 
-	void ParametersUpdate();
+	bool ParametersUpdate();
 	void SensorCorrectionsUpdate(bool force = false);
 	void AirTemperatureUpdate();
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -79,7 +79,7 @@ void VehicleMagnetometer::Stop()
 	}
 }
 
-void VehicleMagnetometer::ParametersUpdate(bool force)
+bool VehicleMagnetometer::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated() || force) {
@@ -161,7 +161,11 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
 			}
 
 		}
+
+		return true;
 	}
+
+	return false;
 }
 
 void VehicleMagnetometer::UpdateMagBiasEstimate()
@@ -360,7 +364,7 @@ void VehicleMagnetometer::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
-	ParametersUpdate();
+	const bool parameter_update = ParametersUpdate();
 
 	// check vehicle status for changes to armed state
 	if (_vehicle_control_mode_sub.updated()) {
@@ -450,7 +454,8 @@ void VehicleMagnetometer::Run()
 	_voter.get_best(time_now_us, &best_index);
 
 	if (best_index >= 0) {
-		if (_selected_sensor_sub_index != best_index) {
+		// handle selection change (don't process on same iteration as parameter update)
+		if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
 			// clear all registered callbacks
 			for (auto &sub : _sensor_sub) {
 				sub.unregisterCallback();
@@ -488,9 +493,9 @@ void VehicleMagnetometer::Run()
 	}
 
 
-	// check failover and report
 	if (_param_sens_mag_mode.get()) {
-		if (_last_failover_count != _voter.failover_count()) {
+		// check failover and report (save failover report for a cycle where parameters didn't update)
+		if (_last_failover_count != _voter.failover_count() && !parameter_update) {
 			uint32_t flags = _voter.failover_state();
 			int failover_index = _voter.failover_index();
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -79,7 +79,7 @@ public:
 private:
 	void Run() override;
 
-	void ParametersUpdate(bool force = false);
+	bool ParametersUpdate(bool force = false);
 
 	void Publish(uint8_t instance, bool multi = false);
 

src/modules/sensors/voted_sensors_update.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -80,6 +80,8 @@ void VotedSensorsUpdate::initializeSensors()
 
 void VotedSensorsUpdate::parametersUpdate()
 {
+	_parameter_update = true;
+
 	updateParams();
 
 	// run through all IMUs
@@ -188,40 +190,44 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	}
 
 	// find the best sensor
-	int accel_best_index = -1;
+	int accel_best_index = _accel.last_best_vote;
-	int gyro_best_index = -1;
+	int gyro_best_index = _gyro.last_best_vote;
-	_accel.voter.get_best(time_now_us, &accel_best_index);
-	_gyro.voter.get_best(time_now_us, &gyro_best_index);
 
-	if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
+	if (!_parameter_update) {
-		// use sensor_selection to find best
+		// update current accel/gyro selection, skipped on cycles where parameters update
-		if (_sensor_selection_sub.update(&_selection)) {
+		_accel.voter.get_best(time_now_us, &accel_best_index);
-			// reset inconsistency checks against primary
+		_gyro.voter.get_best(time_now_us, &gyro_best_index);
-			for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+
-				_accel_diff[sensor_index].zero();
+		if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
-				_gyro_diff[sensor_index].zero();
+			// use sensor_selection to find best
+			if (_sensor_selection_sub.update(&_selection)) {
+				// reset inconsistency checks against primary
+				for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+					_accel_diff[sensor_index].zero();
+					_gyro_diff[sensor_index].zero();
+				}
 			}
+
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
+					accel_best_index = i;
+				}
+
+				if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
+					gyro_best_index = i;
+				}
+			}
+
+		} else {
+			// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
+			checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
+			checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
 		}
-
-		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-			if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
-				accel_best_index = i;
-			}
-
-			if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
-				gyro_best_index = i;
-			}
-		}
-
-	} else {
-		// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
-		checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
-		checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
 	}
 
 	// write data for the best sensor to output variables
-	if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT)
+	if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0)
-	    && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT)) {
+	    && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) {
 
 		raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
 		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
@@ -423,6 +429,11 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 
 	status.timestamp = hrt_absolute_time();
 	_sensors_status_imu_pub.publish(status);
+
+	if (_parameter_update) {
+		// reset
+		_parameter_update = false;
+	}
 }
 
 void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)

src/modules/sensors/voted_sensors_update.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -182,6 +182,8 @@ private:
 
 	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
 
+	bool _parameter_update{false};
+
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)