From 94415a45fad398cc6c2125ce409f0e85b25d53bc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 10 Nov 2020 10:20:51 -0500 Subject: [PATCH] ekf2: move estimator status publication to method --- src/modules/ekf2/EKF2.cpp | 83 +++++++++++++++++++++++---------------- src/modules/ekf2/EKF2.hpp | 1 + 2 files changed, 51 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d1bfac2ca0..f2d3bacd59 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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 ×tamp) _estimator_states_pub.publish(states); } +void EKF2::PublishStatus(const hrt_abstime ×tamp) +{ + 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 ×tamp) { static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a7e2507413..fc3bd07bfa 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -142,6 +142,7 @@ private: void PublishOpticalFlowVel(const hrt_abstime ×tamp, const optical_flow_s &optical_flow); void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); + void PublishStatus(const hrt_abstime ×tamp); void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp);