sensors/vehicle_imu: refactor SensorCalibrationUpdate() to separate accel/gyro cal saving

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -103,8 +103,10 @@ void VehicleIMU::Stop()
 	Deinit();
 }
 
-void VehicleIMU::ParametersUpdate(bool force)
+bool VehicleIMU::ParametersUpdate(bool force)
 {
+	bool updated = false;
+
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated() || force) {
 		// clear update
@@ -115,6 +117,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		updateParams();
 
+		updated = true;
+
 		const auto accel_calibration_count = _accel_calibration.calibration_count();
 		const auto gyro_calibration_count = _gyro_calibration.calibration_count();
 		_accel_calibration.ParametersUpdate();
@@ -123,6 +127,7 @@ void VehicleIMU::ParametersUpdate(bool force)
 		if (accel_calibration_count != _accel_calibration.calibration_count()) {
 			// if calibration changed reset any existing learned calibration
 			_accel_cal_available = false;
+			_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
 
 			for (auto &learned_cal : _accel_learned_calibration) {
 				learned_cal = {};
@@ -132,6 +137,7 @@ void VehicleIMU::ParametersUpdate(bool force)
 		if (gyro_calibration_count != _gyro_calibration.calibration_count()) {
 			// if calibration changed reset any existing learned calibration
 			_gyro_cal_available = false;
+			_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
 
 			for (auto &learned_cal : _gyro_learned_calibration) {
 				learned_cal = {};
@@ -155,13 +161,15 @@ void VehicleIMU::ParametersUpdate(bool force)
 			_update_integrator_config = true;
 		}
 	}
+
+	return updated;
 }
 
 void VehicleIMU::Run()
 {
 	const hrt_abstime now_us = hrt_absolute_time();
 
-	ParametersUpdate();
+	const bool parameters_updated = ParametersUpdate();
 
 	if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
 		_sensor_gyro_sub.unregisterCallback();
@@ -251,9 +259,17 @@ void VehicleIMU::Run()
 		}
 	}
 
-	if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) {
-		SensorCalibrationUpdate();
-		_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
+	if (!parameters_updated) {
+		if (_armed) {
+			if (now_us > _in_flight_calibration_check_timestamp_last + 1_s) {
+				SensorCalibrationUpdate();
+				_in_flight_calibration_check_timestamp_last = now_us;
+			}
+
+		} else {
+			SensorCalibrationSaveAccel();
+			SensorCalibrationSaveGyro();
+		}
 	}
 }
 
@@ -703,45 +719,45 @@ void VehicleIMU::PrintStatus()
 
 void VehicleIMU::SensorCalibrationUpdate()
 {
-	if (_armed) {
-		for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
-			estimator_sensor_bias_s estimator_sensor_bias;
+	for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
+		estimator_sensor_bias_s estimator_sensor_bias;
 
-			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)
-			    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
+		if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)
+		    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
 
-				// find corresponding accel bias
-				if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable
-				    && (estimator_sensor_bias.accel_device_id != 0)
-				    && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) {
+			// find corresponding accel bias
+			if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable
+			    && (estimator_sensor_bias.accel_device_id != 0)
+			    && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) {
 
-					const Vector3f bias{estimator_sensor_bias.accel_bias};
+				const Vector3f bias{estimator_sensor_bias.accel_bias};
 
-					_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
-					_accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance};
-					_accel_learned_calibration[i].valid = true;
-					_accel_cal_available = true;
-				}
+				_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
+				_accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance};
+				_accel_learned_calibration[i].valid = true;
+				_accel_cal_available = true;
+			}
 
-				// find corresponding gyro calibration
-				if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable
-				    && (estimator_sensor_bias.gyro_device_id != 0)
-				    && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) {
+			// find corresponding gyro calibration
+			if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable
+			    && (estimator_sensor_bias.gyro_device_id != 0)
+			    && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) {
 
-					const Vector3f bias{estimator_sensor_bias.gyro_bias};
+				const Vector3f bias{estimator_sensor_bias.gyro_bias};
 
-					_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
-					_gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance};
-					_gyro_learned_calibration[i].valid = true;
-					_gyro_cal_available = true;
-				}
+				_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
+				_gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance};
+				_gyro_learned_calibration[i].valid = true;
+				_gyro_cal_available = true;
 			}
 		}
 	}
+}
 
-	// not armed and accel cal available
-	if (!_armed && _accel_cal_available) {
-		const Vector3f accel_cal_orig{_accel_calibration.offset()};
+void VehicleIMU::SensorCalibrationSaveAccel()
+{
+	if (_accel_cal_available) {
+		const Vector3f cal_orig{_accel_calibration.offset()};
 		bool initialised = false;
 		Vector3f offset_estimate{};
 		Vector3f bias_variance{};
@@ -763,14 +779,17 @@ void VehicleIMU::SensorCalibrationUpdate()
 						bias_variance(axis_index) *= k2;
 					}
 				}
+
+				// reset
+				_accel_learned_calibration[i] = {};
 			}
 		}
 
-		if (initialised && (accel_cal_orig - offset_estimate).longerThan(0.05f)) {
+		if (initialised && (cal_orig - offset_estimate).longerThan(0.05f)) {
 			if (_accel_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
-					 _instance, _accel_calibration.device_id(),
-					 (double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2),
+				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
+					 _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
+					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
 				if (_accel_calibration.ParametersSave()) {
@@ -781,16 +800,13 @@ void VehicleIMU::SensorCalibrationUpdate()
 
 		// reset
 		_accel_cal_available = false;
-
-		for (auto &learned_cal : _accel_learned_calibration) {
-			learned_cal = {};
-		}
 	}
+}
 
-
-	// not armed and gyro cal available
-	if (!_armed && _gyro_cal_available) {
-		const Vector3f gyro_cal_orig{_gyro_calibration.offset()};
+void VehicleIMU::SensorCalibrationSaveGyro()
+{
+	if (_gyro_cal_available) {
+		const Vector3f cal_orig{_gyro_calibration.offset()};
 		bool initialised = false;
 		Vector3f offset_estimate{};
 		Vector3f bias_variance{};
@@ -812,14 +828,17 @@ void VehicleIMU::SensorCalibrationUpdate()
 						bias_variance(axis_index) *= k2;
 					}
 				}
+
+				// reset
+				_gyro_learned_calibration[i] = {};
 			}
 		}
 
-		if (initialised && (gyro_cal_orig - offset_estimate).longerThan(0.01f)) {
+		if (initialised && (cal_orig - offset_estimate).longerThan(0.01f)) {
 			if (_gyro_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
-					 _instance, _gyro_calibration.device_id(),
-					 (double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2),
+				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
+					 _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),
+					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
 				if (_gyro_calibration.ParametersSave()) {
@@ -830,10 +849,6 @@ void VehicleIMU::SensorCalibrationUpdate()
 
 		// reset
 		_gyro_cal_available = false;
-
-		for (auto &learned_cal : _gyro_learned_calibration) {
-			learned_cal = {};
-		}
 	}
 }
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -76,7 +76,7 @@ public:
 	void PrintStatus();
 
 private:
-	void ParametersUpdate(bool force = false);
+	bool ParametersUpdate(bool force = false);
 	bool Publish();
 	void Run() override;
 
@@ -88,6 +88,8 @@ private:
 	void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
 
 	void SensorCalibrationUpdate();
+	void SensorCalibrationSaveAccel();
+	void SensorCalibrationSaveGyro();
 
 	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
 	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
@@ -173,7 +175,6 @@ private:
 	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 
-
 	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
 	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};