diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9c25b298bd..6d119ad547 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3251,8 +3251,8 @@ Commander::reset_posvel_validity() _lpos_probation_time_us = POSVEL_PROBATION_MIN; _lvel_probation_time_us = POSVEL_PROBATION_MIN; - // recheck validity - UpdateEstimateValidity(); + // recheck validity (force update) + estimator_check(true); } bool @@ -3968,7 +3968,7 @@ void Commander::battery_status_check() } } -void Commander::estimator_check() +void Commander::estimator_check(bool force) { // Check if quality checking of position accuracy and consistency is to be performed const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check; @@ -3990,7 +3990,7 @@ void Commander::estimator_check() const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault; // use primary estimator_status - if (_estimator_selector_status_sub.updated()) { + if (_estimator_selector_status_sub.updated() || force) { estimator_selector_status_s estimator_selector_status; if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { @@ -4042,47 +4042,56 @@ void Commander::estimator_check() * if this does not fix the issue we need to stop using a position controlled * mode to prevent flyaway crashes. */ - if (_estimator_status_sub.updated() && run_quality_checks - && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + bool pre_flt_fail_innov_heading = false; + bool pre_flt_fail_innov_vel_horiz = false; + + if (_estimator_status_sub.updated() || force) { estimator_status_s estimator_status; if (_estimator_status_sub.copy(&estimator_status)) { - if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { - _nav_test_failed = false; - _nav_test_passed = false; - } else { - if (!_nav_test_passed) { - // Both test ratios need to pass/fail together to change the nav test status - const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) - && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); + pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading; + pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz; - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); + if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (innovation_pass) { - _time_last_innov_pass = hrt_absolute_time(); + if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _nav_test_failed = false; + _nav_test_passed = false; - // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s); - const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); + } else { + if (!_nav_test_passed) { + // Both test ratios need to pass/fail together to change the nav test status + const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) + && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); - // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds - if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s - && (sufficient_time || sufficient_speed)) { - _nav_test_passed = true; - _nav_test_failed = false; - } + const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); - } else if (innovation_fail) { - _time_last_innov_fail = hrt_absolute_time(); + if (innovation_pass) { + _time_last_innov_pass = hrt_absolute_time(); - if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) { - // if the innovation test has failed continuously, declare the nav as failed - _nav_test_failed = true; - mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t"); - events::send(events::ID("commander_navigation_failure"), events::Log::Emergency, - "Navigation failure! Land and recalibrate the sensors"); + // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed + const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s); + const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); + + // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds + if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s + && (sufficient_time || sufficient_speed)) { + _nav_test_passed = true; + _nav_test_failed = false; + } + + } else if (innovation_fail) { + _time_last_innov_fail = hrt_absolute_time(); + + if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) { + // if the innovation test has failed continuously, declare the nav as failed + _nav_test_failed = true; + mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t"); + events::send(events::ID("commander_navigation_failure"), events::Log::Emergency, + "Navigation failure! Land and recalibrate the sensors"); + } } } } @@ -4093,9 +4102,51 @@ void Commander::estimator_check() // run position and velocity accuracy checks // Check if quality checking of position accuracy and consistency is to be performed if (run_quality_checks) { - UpdateEstimateValidity(); + float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get(); + + // relax local position eph threshold in operator controlled position mode + if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL && + ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) + || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) { + + // Set the allowable position uncertainty based on combination of flight and estimator state + // When we are in a operator demanded position control mode and are solely reliant on optical flow, + // do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift + if (_status_flags.position_reliant_on_optical_flow) { + lpos_eph_threshold_adj = INFINITY; + } + } + + bool xy_valid = lpos.xy_valid && !_nav_test_failed; + bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; + + if (!_armed.armed) { + if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { + xy_valid = false; + } + + if (pre_flt_fail_innov_vel_horiz) { + v_xy_valid = false; + } + } + + const vehicle_global_position_s &gpos = _global_position_sub.get(); + + _status_flags.condition_global_position_valid = + check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, + &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid); + + _status_flags.condition_local_position_valid = + check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp, + &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid); + + _status_flags.condition_local_velocity_valid = + check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, + &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid); } + + // altitude _status_flags.condition_local_altitude_valid = lpos.z_valid && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s)); @@ -4147,7 +4198,7 @@ void Commander::estimator_check() // gps const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid; - if (_vehicle_gps_position_sub.updated()) { + if (_vehicle_gps_position_sub.updated() || force) { vehicle_gps_position_s vehicle_gps_position; if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) { @@ -4179,38 +4230,6 @@ void Commander::estimator_check() } } -void Commander::UpdateEstimateValidity() -{ - const vehicle_local_position_s &lpos = _local_position_sub.get(); - const vehicle_global_position_s &gpos = _global_position_sub.get(); - - float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get(); - - // relax local position eph threshold in operator controlled position mode - if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL && - ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) - || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) { - - // Set the allowable position uncertainty based on combination of flight and estimator state - // When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - if (_status_flags.position_reliant_on_optical_flow) { - lpos_eph_threshold_adj = INFINITY; - } - } - - _status_flags.condition_global_position_valid = - check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, - &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid); - - _status_flags.condition_local_position_valid = - check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp, - &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid); - - _status_flags.condition_local_velocity_valid = - check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, - &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid); -} - void Commander::offboard_control_update() { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a1385045ba..74d44a718d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -146,7 +146,7 @@ private: void esc_status_check(); - void estimator_check(); + void estimator_check(bool force = false); bool handle_command(const vehicle_command_s &cmd); @@ -174,8 +174,6 @@ private: void update_control_mode(); - void UpdateEstimateValidity(); - bool shutdown_if_allowed(); bool stabilization_required(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 534b4acdd3..5687e3df2b 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -82,8 +82,6 @@ void Ekf::reset() _prev_dvel_bias_var.zero(); - _control_status.flags.vehicle_at_rest = true; - resetGpsDriftCheckFilters(); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 579e4eea9d..5cdfadd5ca 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -258,7 +258,12 @@ public: protected: - EstimatorInterface() = default; + EstimatorInterface() + { + _control_status.flags.in_air = true; + _control_status.flags.vehicle_at_rest = false; + }; + virtual ~EstimatorInterface(); virtual bool init(uint64_t timestamp) = 0; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a5767dca0b..71a2de794c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -496,18 +496,6 @@ void EKF2::Run() _preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); - - _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - - // update standby (arming state) flag - const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY); - - if (_standby != standby) { - _standby = standby; - - // reset preflight checks if transitioning in or out of standby arming state - _preflt_checker.reset(); - } } } @@ -813,7 +801,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _estimator_innovations_pub.publish(innovations); // calculate noise filtered velocity innovations which are used for pre-flight checking - if (_standby) { + if (!_ekf.control_status_flags().in_air) { // TODO: move to run before publications _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); @@ -821,6 +809,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); _preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations); + + } else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) { + // reset preflight checks if transitioning back to landed + _preflt_checker.reset(); } } @@ -901,9 +893,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.az = vel_deriv(2); // TODO: better status reporting - lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.xy_valid = _ekf.local_position_is_valid(); lpos.z_valid = !_preflt_checker.hasVertFailed(); - lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.v_xy_valid = _ekf.local_position_is_valid(); lpos.v_z_valid = !_preflt_checker.hasVertFailed(); // Position of local NED origin in GPS / WGS84 frame @@ -1923,22 +1915,19 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) } } + // update stored declination value + if (!_mag_decl_saved) { + float declination_deg; - if (!_armed) { - // update stored declination value - if (!_mag_decl_saved) { - float declination_deg; + if (_ekf.get_mag_decl_deg(&declination_deg)) { + _param_ekf2_mag_decl.update(); - if (_ekf.get_mag_decl_deg(&declination_deg)) { - _param_ekf2_mag_decl.update(); - - if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) { - _param_ekf2_mag_decl.set(declination_deg); - _param_ekf2_mag_decl.commit_no_notification(); - } - - _mag_decl_saved = true; + if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) { + _param_ekf2_mag_decl.set(declination_deg); + _param_ekf2_mag_decl.commit_no_notification(); } + + _mag_decl_saved = true; } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 72c02a0d6f..e5e5de74af 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -267,9 +267,6 @@ private: bool _callback_registered{false}; - bool _armed{false}; - bool _standby{false}; // standby arming state - hrt_abstime _last_status_flag_update{0}; hrt_abstime _last_range_sensor_update{0};