zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf2: move mag calibration to UpdateMagCalibration()

release/1.12
Daniel Agar 4 years ago
parent
96178a6fb0
commit
f4f3ba47a2
2 changed files with 110 additions and 122 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 225
      src/modules/ekf2/EKF2.cpp
  2. 7
      src/modules/ekf2/EKF2.hpp

225
src/modules/ekf2/EKF2.cpp
Unescape View File

@ -212,43 +212,19 @@ int EKF2::print_status() @@ -212,43 +212,19 @@ int EKF2::print_status()
template<typename Param>
void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index)
{
if (_valid_cal_available[axis_index]) {
// calculate weighting using ratio of variances and update stored bias values
const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() +
_last_valid_variance[axis_index]), 0.0f, _param_ekf2_magb_k.get());
const float mag_bias_saved = mag_bias_param.get();
// calculate weighting using ratio of variances and update stored bias values
const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() +
_last_valid_variance[axis_index]), 0.0f, _param_ekf2_magb_k.get());
const float mag_bias_saved = mag_bias_param.get();
_last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved;
_last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved;
mag_bias_param.set(_last_valid_mag_cal[axis_index]);
mag_bias_param.set(_last_valid_mag_cal[axis_index]);
// save new parameters unless in multi-instance mode
if (!_multi_mode) {
mag_bias_param.commit_no_notification();
}
_valid_cal_available[axis_index] = false;
}
}
template<typename Param>
bool EKF2::update_mag_decl(Param &mag_decl_param)
{
// update stored declination value
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {
mag_decl_param.set(declination_deg);
if (!_multi_mode) {
mag_decl_param.commit_no_notification();
}
return true;
// save new parameters unless in multi-instance mode
if (!_multi_mode) {
mag_bias_param.commit_no_notification();
}
return false;
}
void EKF2::Run()
@ -680,88 +656,6 @@ void EKF2::Run() @@ -680,88 +656,6 @@ void EKF2::Run()
}
if (ekf_updated) {
filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);
uint16_t filter_fault_flags;
_ekf.get_filter_fault_status(&filter_fault_flags);
{
/* Check and save learned magnetometer bias estimates */
// Check if conditions are OK for learning of magnetometer bias values
if (!_landed && _armed &&
!filter_fault_flags && // there are no filter faults
control_status.flags.mag_3D) { // the EKF is operating in the correct mode
if (_last_magcal_us == 0) {
_last_magcal_us = now;
} else {
_total_cal_time_us += now - _last_magcal_us;
_last_magcal_us = now;
}
} else if (filter_fault_flags != 0) {
// if a filter fault has occurred, assume previous learning was invalid and do not
// count it towards total learning time.
_total_cal_time_us = 0;
for (bool &cal_available : _valid_cal_available) {
cal_available = false;
}
} else {
// conditions are NOT OK for learning magnetometer bias, reset timestamp
// but keep the accumulated calibration time
_last_magcal_us = now;
}
// Start checking mag bias estimates when we have accumulated sufficient calibration time
if (_total_cal_time_us > 30_s) {
// we have sufficient accumulated valid flight time to form a reliable bias estimate
// check that the state variance for each axis is within a range indicating filter convergence
const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get();
const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get();
// Declare all bias estimates invalid if any variances are out of range
bool all_estimates_invalid = false;
float states[24];
_ekf.getStateAtFusionHorizonAsVector().copyTo(states);
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
if (covariances[axis_index + 19] < min_var_allowed
|| covariances[axis_index + 19] > max_var_allowed) {
all_estimates_invalid = true;
}
}
// Store valid estimates and their associated variances
if (!all_estimates_invalid) {
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
_last_valid_mag_cal[axis_index] = states[axis_index + 19];
_valid_cal_available[axis_index] = true;
_last_valid_variance[axis_index] = covariances[axis_index + 19];
}
}
}
// Check and save the last valid calibration when we are disarmed
if (!_armed && _standby && (filter_fault_flags == 0)) {
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
update_mag_bias(_param_ekf2_magbias_z, 2);
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
}
}
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now);
@ -777,9 +671,7 @@ void EKF2::Run() @@ -777,9 +671,7 @@ void EKF2::Run()
PublishInnovationVariances(now);
PublishYawEstimatorStatus(now);
if (!_mag_decl_saved && _standby) {
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
}
UpdateMagCalibration(now);
}
if (new_optical_flow_data_received) {
@ -1470,6 +1362,103 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) @@ -1470,6 +1362,103 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
{
/* Check and save learned magnetometer bias estimates */
filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);
fault_status_u fault_status;
_ekf.get_filter_fault_status(&fault_status.value);
// Check if conditions are OK for learning of magnetometer bias values
if (!_landed && _armed &&
!fault_status.value && // there are no filter faults
control_status.flags.mag_3D) { // the EKF is operating in the correct mode
if (_last_magcal_us == 0) {
_last_magcal_us = timestamp;
} else {
_total_cal_time_us += timestamp - _last_magcal_us;
_last_magcal_us = timestamp;
}
// Start checking mag bias estimates when we have accumulated sufficient calibration time
if (_total_cal_time_us > 30_s) {
// we have sufficient accumulated valid flight time to form a reliable bias estimate
// check that the state variance for each axis is within a range indicating filter convergence
const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get();
const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get();
// Declare all bias estimates invalid if any variances are out of range
bool all_estimates_invalid = false;
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
if (covariances[axis_index + 19] < min_var_allowed
|| covariances[axis_index + 19] > max_var_allowed) {
all_estimates_invalid = true;
}
}
// Store valid estimates and their associated variances
if (!all_estimates_invalid) {
float states[24];
_ekf.getStateAtFusionHorizonAsVector().copyTo(states);
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
_last_valid_mag_cal[axis_index] = states[axis_index + 19];
_last_valid_variance[axis_index] = covariances[axis_index + 19];
}
_valid_cal_available = true;
}
}
} else if (fault_status.value != 0) {
// if a filter fault has occurred, assume previous learning was invalid and do not
// count it towards total learning time.
_total_cal_time_us = 0;
_valid_cal_available = false;
} else {
// conditions are NOT OK for learning magnetometer bias, reset timestamp
// but keep the accumulated calibration time
_last_magcal_us = timestamp;
}
// Check and save the last valid calibration when we are disarmed
if (!_armed) {
if (_valid_cal_available) {
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
update_mag_bias(_param_ekf2_magbias_z, 2);
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
_valid_cal_available = false;
}
// update stored declination value
if (!_mag_decl_saved) {
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {
_param_ekf2_mag_decl.set(declination_deg);
_mag_decl_saved = true;
if (!_multi_mode) {
_param_ekf2_mag_decl.commit_no_notification();
}
}
}
}
}
int EKF2::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");

7
src/modules/ekf2/EKF2.hpp
Unescape View File

@ -131,9 +131,6 @@ private: @@ -131,9 +131,6 @@ private:
template<typename Param>
void update_mag_bias(Param &mag_bias_param, int axis_index);
template<typename Param>
bool update_mag_decl(Param &mag_decl_param);
void PublishAttitude(const hrt_abstime &timestamp);
void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp);
@ -149,6 +146,8 @@ private: @@ -149,6 +146,8 @@ private:
void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateMagCalibration(const hrt_abstime &timestamp);
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
@ -177,8 +176,8 @@ private: @@ -177,8 +176,8 @@ private:
hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save
float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (Gauss)
bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2)
bool _valid_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

Write Preview
Loading…
Cancel
Save