diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index 091e37b1da..929aa97e57 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -105,3 +105,15 @@ PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f); * @group Sensors */ PARAM_DEFINE_INT32(SENS_MAG_MODE, 1); + +/** + * Magnetometer auto calibration + * + * Automatically initialize magnetometer calibration from bias estimate if available. + * + * @boolean + * + * @category system + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 0); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 2e9cbea69b..68e2361661 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -183,7 +183,7 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() _calibration_estimator_bias[mag_index] = bias; // set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available - if (!_armed && mag_bias_est.stable[mag_index] + if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { if (_calibration[mag_index].calibration_index() < 0) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 62f4dbfa9a..90486dee74 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -171,7 +171,8 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_mag_comp_typ, (ParamBool) _param_sens_mag_mode, - (ParamFloat) _param_sens_mag_rate + (ParamFloat) _param_sens_mag_rate, + (ParamBool) _param_sens_mag_autocal ) }; }; // namespace sensors