forked from Archive/PX4-Autopilot
save significant IMU bias changes learned by the EKF
* ekf2: make publishing of learned accel biases more robust * ekf2: reset accel bias if calibration updated * msg: add separate accel and gyro calibration counters * ekf2: use separate accel and gyro calibration counters * ekf2: rework logic to reset biases when calibration counters increment * sensors: add saving of learned accel biases * ekf2: generalized saving accel/gyro/mag in flight sensor calibration * boards: holybro kakutef7 disable systemcmds/perf and systemcmds/top to save flash Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
GitHub
parent
5969508fa7
commit
68026eadeb
|
|
@ -41,8 +41,6 @@ CONFIG_SYSTEMCMDS_DMESG=y
|
|||
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||
CONFIG_SYSTEMCMDS_MIXER=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_PWM=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
|
|
|
|||
|
|
@ -13,15 +13,18 @@ float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s)
|
|||
float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s)
|
||||
float32[3] gyro_bias_variance
|
||||
bool gyro_bias_valid
|
||||
bool gyro_bias_stable # true when the gyro bias estimate is stable enough to use for calibration
|
||||
|
||||
uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2)
|
||||
float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2)
|
||||
float32[3] accel_bias_variance
|
||||
bool accel_bias_valid
|
||||
bool accel_bias_stable # true when the accel bias estimate is stable enough to use for calibration
|
||||
|
||||
uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles
|
||||
float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss)
|
||||
float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss)
|
||||
float32[3] mag_bias_variance
|
||||
bool mag_bias_valid
|
||||
bool mag_bias_stable # true when the mag bias estimate is stable enough to use for calibration
|
||||
|
|
|
|||
|
|
@ -16,4 +16,5 @@ uint8 CLIPPING_Y = 2
|
|||
uint8 CLIPPING_Z = 4
|
||||
uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
|
||||
|
||||
uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes.
|
||||
uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
|
||||
uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
|
||||
|
|
|
|||
|
|
@ -84,6 +84,12 @@ public:
|
|||
return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
|
||||
}
|
||||
|
||||
// Compute sensor offset from bias (board frame)
|
||||
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
|
||||
{
|
||||
return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
|
||||
}
|
||||
|
||||
bool ParametersSave();
|
||||
void ParametersUpdate();
|
||||
|
||||
|
|
|
|||
|
|
@ -88,6 +88,12 @@ public:
|
|||
return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
|
||||
}
|
||||
|
||||
// Compute sensor offset from bias (board frame)
|
||||
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
|
||||
{
|
||||
return (_rotation.I() * bias) + _thermal_offset + _offset;
|
||||
}
|
||||
|
||||
bool ParametersSave();
|
||||
void ParametersUpdate();
|
||||
|
||||
|
|
|
|||
|
|
@ -341,20 +341,45 @@ void EKF2::Run()
|
|||
if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
|
||||
_device_id_accel = imu.accel_device_id;
|
||||
_device_id_gyro = imu.gyro_device_id;
|
||||
_imu_calibration_count = imu.calibration_count;
|
||||
_accel_calibration_count = imu.accel_calibration_count;
|
||||
_gyro_calibration_count = imu.gyro_calibration_count;
|
||||
|
||||
} else if ((imu.calibration_count > _imu_calibration_count)
|
||||
|| (imu.accel_device_id != _device_id_accel)
|
||||
|| (imu.gyro_device_id != _device_id_gyro)) {
|
||||
} else {
|
||||
bool reset_actioned = false;
|
||||
|
||||
PX4_DEBUG("%d - resetting IMU bias", _instance);
|
||||
_device_id_accel = imu.accel_device_id;
|
||||
_device_id_gyro = imu.gyro_device_id;
|
||||
if ((imu.accel_calibration_count != _accel_calibration_count)
|
||||
|| (imu.accel_device_id != _device_id_accel)) {
|
||||
|
||||
_ekf.resetImuBias();
|
||||
_imu_calibration_count = imu.calibration_count;
|
||||
PX4_DEBUG("%d - resetting accelerometer bias", _instance);
|
||||
_device_id_accel = imu.accel_device_id;
|
||||
|
||||
SelectImuStatus();
|
||||
_ekf.resetAccelBias();
|
||||
_accel_calibration_count = imu.accel_calibration_count;
|
||||
|
||||
// reset bias learning
|
||||
_accel_cal = {};
|
||||
|
||||
reset_actioned = true;
|
||||
}
|
||||
|
||||
if ((imu.gyro_calibration_count != _gyro_calibration_count)
|
||||
|| (imu.gyro_device_id != _device_id_gyro)) {
|
||||
|
||||
PX4_DEBUG("%d - resetting rate gyro bias", _instance);
|
||||
_device_id_gyro = imu.gyro_device_id;
|
||||
|
||||
_ekf.resetGyroBias();
|
||||
_gyro_calibration_count = imu.gyro_calibration_count;
|
||||
|
||||
// reset bias learning
|
||||
_gyro_cal = {};
|
||||
|
||||
reset_actioned = true;
|
||||
}
|
||||
|
||||
if (reset_actioned) {
|
||||
SelectImuStatus();
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -385,14 +410,26 @@ void EKF2::Run()
|
|||
|
||||
if (_sensor_selection_sub.copy(&sensor_selection)) {
|
||||
if (_device_id_accel != sensor_selection.accel_device_id) {
|
||||
_ekf.resetAccelBias();
|
||||
|
||||
_device_id_accel = sensor_selection.accel_device_id;
|
||||
|
||||
_ekf.resetAccelBias();
|
||||
|
||||
// reset bias learning
|
||||
_accel_cal = {};
|
||||
|
||||
SelectImuStatus();
|
||||
}
|
||||
|
||||
if (_device_id_gyro != sensor_selection.gyro_device_id) {
|
||||
_ekf.resetGyroBias();
|
||||
|
||||
_device_id_gyro = sensor_selection.gyro_device_id;
|
||||
|
||||
_ekf.resetGyroBias();
|
||||
|
||||
// reset bias learning
|
||||
_gyro_cal = {};
|
||||
|
||||
SelectImuStatus();
|
||||
}
|
||||
}
|
||||
|
|
@ -452,6 +489,7 @@ void EKF2::Run()
|
|||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
const bool was_in_air = _ekf.control_status_flags().in_air;
|
||||
_ekf.set_in_air_status(!vehicle_land_detected.landed);
|
||||
|
||||
if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
|
||||
|
|
@ -463,6 +501,13 @@ void EKF2::Run()
|
|||
} else {
|
||||
_ekf.set_gnd_effect_flag(false);
|
||||
}
|
||||
|
||||
// reset learned sensor calibrations on takeoff
|
||||
if (_ekf.control_status_flags().in_air && !was_in_air) {
|
||||
_accel_cal = {};
|
||||
_gyro_cal = {};
|
||||
_mag_cal = {};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -500,7 +545,6 @@ void EKF2::Run()
|
|||
PublishLocalPosition(now);
|
||||
PublishOdometry(now, imu_sample_new);
|
||||
PublishGlobalPosition(now);
|
||||
PublishSensorBias(now);
|
||||
PublishWindEstimate(now);
|
||||
|
||||
// publish status/logging messages
|
||||
|
|
@ -515,7 +559,10 @@ void EKF2::Run()
|
|||
PublishInnovationVariances(now);
|
||||
PublishYawEstimatorStatus(now);
|
||||
|
||||
UpdateAccelCalibration(now);
|
||||
UpdateGyroCalibration(now);
|
||||
UpdateMagCalibration(now);
|
||||
PublishSensorBias(now);
|
||||
|
||||
} else {
|
||||
// ekf no update
|
||||
|
|
@ -1013,7 +1060,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
|
||||
const Vector3f gyro_bias{_ekf.getGyroBias()};
|
||||
const Vector3f accel_bias{_ekf.getAccelBias()};
|
||||
const Vector3f mag_bias{_mag_cal_last_bias};
|
||||
const Vector3f mag_bias{_ekf.getMagBias()};
|
||||
|
||||
// only publish on change
|
||||
if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
|
||||
|
|
@ -1026,8 +1073,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
gyro_bias.copyTo(bias.gyro_bias);
|
||||
bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
|
||||
_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
|
||||
bias.gyro_bias_valid = true;
|
||||
|
||||
bias.gyro_bias_valid = true; // TODO
|
||||
bias.gyro_bias_stable = _gyro_cal.cal_available;
|
||||
_last_gyro_bias_published = gyro_bias;
|
||||
}
|
||||
|
||||
|
|
@ -1036,8 +1083,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
accel_bias.copyTo(bias.accel_bias);
|
||||
bias.accel_bias_limit = _params->acc_bias_lim;
|
||||
_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
|
||||
bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
|
||||
|
||||
bias.accel_bias_valid = true; // TODO
|
||||
bias.accel_bias_stable = _accel_cal.cal_available;
|
||||
_last_accel_bias_published = accel_bias;
|
||||
}
|
||||
|
||||
|
|
@ -1045,9 +1092,9 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
bias.mag_device_id = _device_id_mag;
|
||||
mag_bias.copyTo(bias.mag_bias);
|
||||
bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
|
||||
_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
|
||||
bias.mag_bias_valid = _mag_cal_available;
|
||||
|
||||
_ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance);
|
||||
bias.mag_bias_valid = true; // TODO
|
||||
bias.mag_bias_stable = _mag_cal.cal_available;
|
||||
_last_mag_bias_published = mag_bias;
|
||||
}
|
||||
|
||||
|
|
@ -1646,9 +1693,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
_mag_calibration_count = magnetometer.calibration_count;
|
||||
|
||||
// reset magnetometer bias learning
|
||||
_mag_cal_total_time_us = 0;
|
||||
_mag_cal_last_us = 0;
|
||||
_mag_cal_available = false;
|
||||
_mag_cal = {};
|
||||
}
|
||||
|
||||
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
|
||||
|
|
@ -1725,34 +1770,99 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
}
|
||||
}
|
||||
|
||||
void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp)
|
||||
{
|
||||
// Check if conditions are OK for learning of accelerometer bias values
|
||||
// the EKF is operating in the correct mode and there are no filter faults
|
||||
if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)
|
||||
&& !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
|
||||
|
||||
if (_accel_cal.last_us != 0) {
|
||||
_accel_cal.total_time_us += timestamp - _accel_cal.last_us;
|
||||
|
||||
// Start checking accel bias estimates when we have accumulated sufficient calibration time
|
||||
if (_accel_cal.total_time_us > 30_s) {
|
||||
_accel_cal.last_bias = _ekf.getAccelBias();
|
||||
_accel_cal.last_bias_variance = _ekf.getAccelBiasVariance();
|
||||
_accel_cal.cal_available = true;
|
||||
}
|
||||
}
|
||||
|
||||
_accel_cal.last_us = timestamp;
|
||||
|
||||
} else {
|
||||
// conditions are NOT OK for learning accelerometer bias, reset timestamp
|
||||
// but keep the accumulated calibration time
|
||||
_accel_cal.last_us = 0;
|
||||
|
||||
if (_ekf.fault_status().value != 0) {
|
||||
// if a filter fault has occurred, assume previous learning was invalid and do not
|
||||
// count it towards total learning time.
|
||||
_accel_cal.total_time_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp)
|
||||
{
|
||||
// Check if conditions are OK for learning of gyro bias values
|
||||
// the EKF is operating in the correct mode and there are no filter faults
|
||||
if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)) {
|
||||
|
||||
if (_gyro_cal.last_us != 0) {
|
||||
_gyro_cal.total_time_us += timestamp - _gyro_cal.last_us;
|
||||
|
||||
// Start checking gyro bias estimates when we have accumulated sufficient calibration time
|
||||
if (_gyro_cal.total_time_us > 30_s) {
|
||||
_gyro_cal.last_bias = _ekf.getGyroBias();
|
||||
_gyro_cal.last_bias_variance = _ekf.getGyroBiasVariance();
|
||||
_gyro_cal.cal_available = true;
|
||||
}
|
||||
}
|
||||
|
||||
_gyro_cal.last_us = timestamp;
|
||||
|
||||
} else {
|
||||
// conditions are NOT OK for learning gyro bias, reset timestamp
|
||||
// but keep the accumulated calibration time
|
||||
_gyro_cal.last_us = 0;
|
||||
|
||||
if (_ekf.fault_status().value != 0) {
|
||||
// if a filter fault has occurred, assume previous learning was invalid and do not
|
||||
// count it towards total learning time.
|
||||
_gyro_cal.total_time_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
|
||||
{
|
||||
// Check if conditions are OK for learning of magnetometer bias values
|
||||
// the EKF is operating in the correct mode and there are no filter faults
|
||||
if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) {
|
||||
|
||||
if (_mag_cal_last_us != 0) {
|
||||
_mag_cal_total_time_us += timestamp - _mag_cal_last_us;
|
||||
if (_mag_cal.last_us != 0) {
|
||||
_mag_cal.total_time_us += timestamp - _mag_cal.last_us;
|
||||
|
||||
// Start checking mag bias estimates when we have accumulated sufficient calibration time
|
||||
if (_mag_cal_total_time_us > 30_s) {
|
||||
_mag_cal_last_bias = _ekf.getMagBias();
|
||||
_mag_cal_last_bias_variance = _ekf.getMagBiasVariance();
|
||||
_mag_cal_available = true;
|
||||
if (_mag_cal.total_time_us > 30_s) {
|
||||
_mag_cal.last_bias = _ekf.getMagBias();
|
||||
_mag_cal.last_bias_variance = _ekf.getMagBiasVariance();
|
||||
_mag_cal.cal_available = true;
|
||||
}
|
||||
}
|
||||
|
||||
_mag_cal_last_us = timestamp;
|
||||
_mag_cal.last_us = timestamp;
|
||||
|
||||
} else {
|
||||
// conditions are NOT OK for learning magnetometer bias, reset timestamp
|
||||
// but keep the accumulated calibration time
|
||||
_mag_cal_last_us = 0;
|
||||
_mag_cal.last_us = 0;
|
||||
|
||||
if (_ekf.fault_status().value != 0) {
|
||||
// if a filter fault has occurred, assume previous learning was invalid and do not
|
||||
// count it towards total learning time.
|
||||
_mag_cal_total_time_us = 0;
|
||||
_mag_cal.total_time_us = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -166,8 +166,11 @@ private:
|
|||
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
void UpdateImuStatus();
|
||||
|
||||
void UpdateAccelCalibration(const hrt_abstime ×tamp);
|
||||
void UpdateGyroCalibration(const hrt_abstime ×tamp);
|
||||
void UpdateMagCalibration(const hrt_abstime ×tamp);
|
||||
|
||||
|
||||
/*
|
||||
* Calculate filtered WGS84 height from estimated AMSL height
|
||||
*/
|
||||
|
|
@ -198,17 +201,22 @@ private:
|
|||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
|
||||
// Used to check, save and use learned magnetometer biases
|
||||
hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
|
||||
hrt_abstime _mag_cal_total_time_us{0}; ///< accumulated calibration time since the last save
|
||||
|
||||
Vector3f _mag_cal_last_bias{}; ///< last valid XYZ magnetometer bias estimates (Gauss)
|
||||
Vector3f _mag_cal_last_bias_variance{}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2)
|
||||
bool _mag_cal_available{false}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
|
||||
|
||||
// Used to control saving of mag declination to be used on next startup
|
||||
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
|
||||
|
||||
// Used to check, save and use learned accel/gyro/mag biases
|
||||
struct InFlightCalibration {
|
||||
hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec)
|
||||
hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save
|
||||
Vector3f last_bias{}; ///< last valid XYZ accelerometer bias estimates (Gauss)
|
||||
Vector3f last_bias_variance{}; ///< variances for the last valid accelerometer XYZ bias estimates (m/s**2)**2
|
||||
bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available
|
||||
};
|
||||
|
||||
InFlightCalibration _accel_cal{};
|
||||
InFlightCalibration _gyro_cal{};
|
||||
InFlightCalibration _mag_cal{};
|
||||
|
||||
bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate
|
||||
|
||||
uint64_t _gps_time_usec{0};
|
||||
|
|
@ -216,7 +224,8 @@ private:
|
|||
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
|
||||
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
|
||||
|
||||
uint8_t _imu_calibration_count{0};
|
||||
uint8_t _accel_calibration_count{0};
|
||||
uint8_t _gyro_calibration_count{0};
|
||||
uint8_t _mag_calibration_count{0};
|
||||
|
||||
uint32_t _device_id_accel{0};
|
||||
|
|
@ -227,6 +236,11 @@ private:
|
|||
Vector3f _last_accel_bias_published{};
|
||||
Vector3f _last_gyro_bias_published{};
|
||||
Vector3f _last_mag_bias_published{};
|
||||
|
||||
Vector3f _last_accel_calibration_published{};
|
||||
Vector3f _last_gyro_calibration_published{};
|
||||
Vector3f _last_mag_calibration_published{};
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
|
||||
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
|
||||
|
|
|
|||
|
|
@ -146,6 +146,15 @@ void VehicleIMU::Run()
|
|||
|
||||
ParametersUpdate();
|
||||
|
||||
// check vehicle status for changes to armed state
|
||||
if (_vehicle_control_mode_sub.updated()) {
|
||||
vehicle_control_mode_s vehicle_control_mode;
|
||||
|
||||
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||
_armed = vehicle_control_mode.flag_armed;
|
||||
}
|
||||
}
|
||||
|
||||
if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -210,15 +219,20 @@ void VehicleIMU::Run()
|
|||
|
||||
_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
|
||||
|
||||
return;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// finish if there are no more updates, but didn't publish
|
||||
if (!updated) {
|
||||
return;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) {
|
||||
SensorCalibrationUpdate();
|
||||
_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
bool VehicleIMU::UpdateAccel()
|
||||
|
|
@ -537,7 +551,8 @@ bool VehicleIMU::Publish()
|
|||
delta_angle_corrected.copyTo(imu.delta_angle);
|
||||
delta_velocity_corrected.copyTo(imu.delta_velocity);
|
||||
imu.delta_velocity_clipping = _delta_velocity_clipping;
|
||||
imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
|
||||
imu.accel_calibration_count = _accel_calibration.calibration_count();
|
||||
imu.gyro_calibration_count = _gyro_calibration.calibration_count();
|
||||
imu.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_pub.publish(imu);
|
||||
|
||||
|
|
@ -650,4 +665,178 @@ void VehicleIMU::PrintStatus()
|
|||
_gyro_calibration.PrintStatus();
|
||||
}
|
||||
|
||||
void VehicleIMU::SensorCalibrationUpdate()
|
||||
{
|
||||
// State variance assumed for accelerometer bias storage.
|
||||
// This is a reference variance used to calculate the fraction of learned accelerometer bias that will be used to update the stored value.
|
||||
// Larger values cause a larger fraction of the learned biases to be used.
|
||||
static constexpr float max_var_allowed = 1e-3f;
|
||||
static constexpr float max_var_ratio = 1e2f;
|
||||
|
||||
if (_armed) {
|
||||
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)) {
|
||||
// find corresponding accel bias
|
||||
if (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id) {
|
||||
const Vector3f bias{estimator_sensor_bias.accel_bias};
|
||||
const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance};
|
||||
|
||||
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
|
||||
(estimator_sensor_bias.accel_device_id != 0) &&
|
||||
estimator_sensor_bias.accel_bias_stable &&
|
||||
(bias_variance.max() < max_var_allowed) &&
|
||||
(bias_variance.max() < max_var_ratio * bias_variance.min());
|
||||
|
||||
if (valid) {
|
||||
_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
|
||||
_accel_learned_calibration[i].bias_variance = bias_variance;
|
||||
_accel_learned_calibration[i].valid = true;
|
||||
_accel_cal_available = true;
|
||||
|
||||
} else {
|
||||
_accel_learned_calibration[i].valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// find corresponding gyro calibration
|
||||
if (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id) {
|
||||
const Vector3f bias{estimator_sensor_bias.gyro_bias};
|
||||
const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance};
|
||||
|
||||
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
|
||||
(estimator_sensor_bias.gyro_device_id != 0) &&
|
||||
estimator_sensor_bias.gyro_bias_valid &&
|
||||
estimator_sensor_bias.gyro_bias_stable &&
|
||||
(bias_variance.max() < max_var_allowed) &&
|
||||
(bias_variance.max() < max_var_ratio * bias_variance.min());
|
||||
|
||||
if (valid) {
|
||||
_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
|
||||
_gyro_learned_calibration[i].bias_variance = bias_variance;
|
||||
_gyro_learned_calibration[i].valid = true;
|
||||
_gyro_cal_available = true;
|
||||
|
||||
} else {
|
||||
_gyro_learned_calibration[i].valid = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// not armed and accel cal available
|
||||
if (!_armed && _accel_cal_available) {
|
||||
|
||||
bool initialised = false;
|
||||
Vector3f bias_variance {};
|
||||
Vector3f bias_estimate {};
|
||||
|
||||
// apply all valid saved offsets
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_accel_learned_calibration[i].valid) {
|
||||
if (!initialised) {
|
||||
bias_variance = _accel_learned_calibration[i].bias_variance;
|
||||
bias_estimate = _accel_learned_calibration[i].offset;
|
||||
initialised = true;
|
||||
|
||||
} else {
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
|
||||
const float k1 = bias_variance(axis_index) / sum_of_variances;
|
||||
const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
|
||||
bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index);
|
||||
bias_variance(axis_index) *= k2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (initialised && bias_estimate.longerThan(0.05f)) {
|
||||
const Vector3f accel_cal_orig{_accel_calibration.offset()};
|
||||
Vector3f accel_cal_offset{_accel_calibration.offset()};
|
||||
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
accel_cal_offset(axis_index) += bias_estimate(axis_index);
|
||||
}
|
||||
|
||||
if (_accel_calibration.set_offset(accel_cal_offset)) {
|
||||
|
||||
PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
|
||||
_instance, _accel_calibration.device_id(),
|
||||
(double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2),
|
||||
(double)accel_cal_offset(0), (double)accel_cal_offset(1), (double)accel_cal_offset(2),
|
||||
(double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2));
|
||||
|
||||
_accel_calibration.ParametersSave();
|
||||
}
|
||||
}
|
||||
|
||||
// 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) {
|
||||
bool initialised = false;
|
||||
Vector3f bias_variance {};
|
||||
Vector3f bias_estimate {};
|
||||
|
||||
// apply all valid saved offsets
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_gyro_learned_calibration[i].valid) {
|
||||
if (!initialised) {
|
||||
bias_variance = _gyro_learned_calibration[i].bias_variance;
|
||||
bias_estimate = _gyro_learned_calibration[i].offset;
|
||||
initialised = true;
|
||||
|
||||
} else {
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
|
||||
const float k1 = bias_variance(axis_index) / sum_of_variances;
|
||||
const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
|
||||
bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index);
|
||||
bias_variance(axis_index) *= k2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (initialised && bias_estimate.longerThan(0.05f)) {
|
||||
const Vector3f gyro_cal_orig{_gyro_calibration.offset()};
|
||||
Vector3f gyro_cal_offset{_gyro_calibration.offset()};
|
||||
|
||||
for (int axis_index = 0; axis_index < 3; axis_index++) {
|
||||
gyro_cal_offset(axis_index) += bias_estimate(axis_index);
|
||||
}
|
||||
|
||||
if (_gyro_calibration.set_offset(gyro_cal_offset)) {
|
||||
|
||||
PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
|
||||
_instance, _gyro_calibration.device_id(),
|
||||
(double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2),
|
||||
(double)gyro_cal_offset(0), (double)gyro_cal_offset(1), (double)gyro_cal_offset(2),
|
||||
(double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2));
|
||||
|
||||
_gyro_calibration.ParametersSave();
|
||||
}
|
||||
}
|
||||
|
||||
// reset
|
||||
_gyro_cal_available = false;
|
||||
|
||||
for (auto &learned_cal : _gyro_learned_calibration) {
|
||||
learned_cal = {};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sensors
|
||||
|
|
|
|||
|
|
@ -47,10 +47,13 @@
|
|||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_imu.h>
|
||||
#include <uORB/topics/vehicle_imu_status.h>
|
||||
|
||||
|
|
@ -85,14 +88,21 @@ private:
|
|||
void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
|
||||
void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
|
||||
|
||||
void SensorCalibrationUpdate();
|
||||
|
||||
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)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
// Used to check, save and use learned magnetometer biases
|
||||
uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
|
||||
|
||||
uORB::Subscription _sensor_accel_sub;
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
|
||||
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
|
||||
calibration::Accelerometer _accel_calibration{};
|
||||
calibration::Gyroscope _gyro_calibration{};
|
||||
|
||||
|
|
@ -105,6 +115,8 @@ private:
|
|||
hrt_abstime _gyro_timestamp_sample_last{0};
|
||||
hrt_abstime _gyro_timestamp_last{0};
|
||||
|
||||
hrt_abstime _in_flight_calibration_check_timestamp_last{0};
|
||||
|
||||
math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
|
||||
math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
|
||||
|
||||
|
|
@ -147,6 +159,21 @@ private:
|
|||
|
||||
const uint8_t _instance;
|
||||
|
||||
bool _armed{false};
|
||||
|
||||
bool _accel_cal_available{false};
|
||||
bool _gyro_cal_available{false};
|
||||
|
||||
struct InFlightCalibration {
|
||||
matrix::Vector3f offset{};
|
||||
matrix::Vector3f bias_variance{};
|
||||
bool valid{false};
|
||||
};
|
||||
|
||||
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")};
|
||||
|
||||
|
|
|
|||
|
|
@ -160,8 +160,10 @@ void VehicleMagnetometer::MagCalibrationUpdate()
|
|||
const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
|
||||
|
||||
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
|
||||
&& (estimator_sensor_bias.mag_device_id != 0) && estimator_sensor_bias.mag_bias_valid
|
||||
&& (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
|
||||
&& (estimator_sensor_bias.mag_device_id != 0) &&
|
||||
estimator_sensor_bias.mag_bias_valid &&
|
||||
estimator_sensor_bias.mag_bias_stable &&
|
||||
(bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
|
||||
|
||||
if (valid) {
|
||||
// find corresponding mag calibration
|
||||
|
|
|
|||
Loading…
Reference in New Issue