sensors: automatically set initial accel/gyro calibration if stable bias available

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -259,8 +259,8 @@ void VehicleIMU::Run()
 		}
 	}
 
-	if (!parameters_updated) {
-		if (_armed) {
+	if (_param_sens_imu_autocal.get() && !parameters_updated) {
+		if (_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) {
 			if (now_us > _in_flight_calibration_check_timestamp_last + 1_s) {
 				SensorCalibrationUpdate();
 				_in_flight_calibration_check_timestamp_last = now_us;
@@ -792,6 +792,45 @@ void VehicleIMU::SensorCalibrationSaveAccel()
 					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
+				// find appropriate calibration slot if not already set
+				if (_accel_calibration.calibration_index() < 0) {
+					uint32_t cal_device_ids[calibration::Accelerometer::MAX_SENSOR_COUNT] {};
+					bool cal_slot_match = false;
+
+					for (unsigned cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
+						char str[20] {};
+						sprintf(str, "CAL_%s%u_ID", "ACC", cal_index);
+						int32_t device_id_val = 0;
+
+						if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
+							cal_device_ids[cal_index] = device_id_val;
+
+							if (cal_device_ids[cal_index] == _accel_calibration.device_id()) {
+								cal_slot_match = true;
+								_accel_calibration.set_calibration_index(cal_index);
+								break;
+							}
+						}
+					}
+
+					if (!cal_slot_match) {
+						// prefer slot that matches sensor instance
+						int accel_index = _sensor_accel_sub.get_instance();
+
+						if (cal_device_ids[accel_index] == 0) {
+							_accel_calibration.set_calibration_index(accel_index);
+
+						} else {
+							for (int cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
+								if (cal_device_ids[accel_index] == 0) {
+									_accel_calibration.set_calibration_index(cal_index);
+									break;
+								}
+							}
+						}
+					}
+				}
+
 				if (_accel_calibration.ParametersSave()) {
 					param_notify_changes();
 				}
@@ -841,6 +880,45 @@ void VehicleIMU::SensorCalibrationSaveGyro()
 					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
+				// find appropriate calibration slot if not already set
+				if (_gyro_calibration.calibration_index() < 0) {
+					uint32_t cal_device_ids[calibration::Gyroscope::MAX_SENSOR_COUNT] {};
+					bool cal_slot_match = false;
+
+					for (unsigned cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
+						char str[20] {};
+						sprintf(str, "CAL_%s%u_ID", "GYRO", cal_index);
+						int32_t device_id_val = 0;
+
+						if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
+							cal_device_ids[cal_index] = device_id_val;
+
+							if (cal_device_ids[cal_index] == _gyro_calibration.device_id()) {
+								cal_slot_match = true;
+								_gyro_calibration.set_calibration_index(cal_index);
+								break;
+							}
+						}
+					}
+
+					if (!cal_slot_match) {
+						// prefer slot that matches sensor instance
+						int gyro_index = _sensor_gyro_sub.get_instance();
+
+						if (cal_device_ids[gyro_index] == 0) {
+							_gyro_calibration.set_calibration_index(gyro_index);
+
+						} else {
+							for (int cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
+								if (cal_device_ids[gyro_index] == 0) {
+									_gyro_calibration.set_calibration_index(cal_index);
+									break;
+								}
+							}
+						}
+					}
+				}
+
 				if (_gyro_calibration.ParametersSave()) {
 					param_notify_changes();
 				}

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -180,7 +180,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
+		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
 	)
 };
 

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -48,3 +48,15 @@
 * @group Sensors
 */
 PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200);
+
+/**
+ * IMU auto calibration
+ *
+ * Automatically initialize IMU (accel/gyro) calibration from bias estimates if available.
+ *
+ * @boolean
+ *
+ * @category system
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_IMU_AUTOCAL, 1);