fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 09:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move estimator status publication to method

Browse Source
This commit is contained in:
Daniel Agar
2020-11-10 10:20:51 -05:00
parent 03388f4656
commit 94415a45fa
2 changed files with 51 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+50 -33
View File
@@ -684,43 +684,15 @@ void EKF2::Run()
filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);
// publish estimator status
estimator_status_s status{};
status.timestamp_sample = imu_sample_new.time_us;
_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
_ekf.get_gps_check_status(&status.gps_check_fail_flags);
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
status.control_mode_flags = control_status.value;
_ekf.get_filter_fault_status(&status.filter_fault_flags);
_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
status.vel_test_ratio, status.pos_test_ratio,
status.hgt_test_ratio, status.tas_test_ratio,
status.hagl_test_ratio, status.beta_test_ratio);
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.getImuVibrationMetrics().copyTo(status.vibe);
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
status.accel_device_id = _device_id_accel;
status.baro_device_id = _device_id_baro;
status.gyro_device_id = _device_id_gyro;
status.mag_device_id = _device_id_mag;
status.timestamp = _replay_mode ? now : hrt_absolute_time();
_estimator_status_pub.publish(status);
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 &&
!status.filter_fault_flags && // there are no filter faults
!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) {
@@ -731,7 +703,7 @@ void EKF2::Run()
_last_magcal_us = now;
}
} else if (status.filter_fault_flags != 0) {
} 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;
@@ -780,7 +752,7 @@ void EKF2::Run()
}
// Check and save the last valid calibration when we are disarmed
if (!_armed && _standby && (status.filter_fault_flags == 0)) {
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);
@@ -800,6 +772,7 @@ void EKF2::Run()
// publish status/logging messages
PublishEkfDriftMetrics(now);
PublishStates(now);
PublishStatus(now);
if (!_mag_decl_saved && _standby) {
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
@@ -1365,6 +1338,50 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
_estimator_states_pub.publish(states);
}
void EKF2::PublishStatus(const hrt_abstime &timestamp)
{
estimator_status_s status{};
status.timestamp_sample = timestamp;
_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
_ekf.get_gps_check_status(&status.gps_check_fail_flags);
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);
status.control_mode_flags = control_status.value;
_ekf.get_filter_fault_status(&status.filter_fault_flags);
_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
status.vel_test_ratio, status.pos_test_ratio,
status.hgt_test_ratio, status.tas_test_ratio,
status.hagl_test_ratio, status.beta_test_ratio);
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.getImuVibrationMetrics().copyTo(status.vibe);
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
status.accel_device_id = _device_id_accel;
status.baro_device_id = _device_id_baro;
status.gyro_device_id = _device_id_gyro;
status.mag_device_id = _device_id_mag;
status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_pub.publish(status);
}
void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
{
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,