From f17f38197d60e8579186424c98d651c80df4dcac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 27 Aug 2022 09:40:42 +0200 Subject: [PATCH] commander: move estimator checks to arming check --- msg/vehicle_status_flags.msg | 8 +- .../Arming/ArmStateMachine/CMakeLists.txt | 2 +- src/modules/commander/Commander.cpp | 304 --------------- src/modules/commander/Commander.hpp | 33 -- .../HealthAndArmingChecks.hpp | 2 +- .../checks/estimatorCheck.cpp | 364 +++++++++++++++++- .../checks/estimatorCheck.hpp | 56 ++- .../checks/modeCheck.cpp | 15 +- .../commander/ModeUtil/mode_requirements.cpp | 3 +- 9 files changed, 428 insertions(+), 359 deletions(-) diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 4234917eda..fa756273ef 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -7,6 +7,7 @@ uint64 timestamp # time since system start (microseconds) uint32 mode_req_angular_velocity uint32 mode_req_attitude uint32 mode_req_local_position +uint32 mode_req_local_position_relaxed uint32 mode_req_global_position uint32 mode_req_local_alt uint32 mode_req_mission @@ -23,6 +24,7 @@ bool angular_velocity_valid bool attitude_valid bool local_altitude_valid bool local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation +bool local_position_valid_relaxed # Local position with reduced accuracy requirements (e.g. flying with optical flow) bool local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool gps_position_valid @@ -30,12 +32,6 @@ bool home_position_valid # indicates a valid home position (a valid home positi bool escs_error # set to true if one or more ESCs reporting esc_status are offline bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure -bool position_reliant_on_gps -bool position_reliant_on_optical_flow -bool position_reliant_on_vision_position - -bool dead_reckoning - bool offboard_control_signal_lost bool rc_signal_found_once diff --git a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt index 8a82b6de9e..e9f9c54232 100644 --- a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt +++ b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt @@ -39,6 +39,6 @@ target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks) px4_add_functional_gtest(SRC ArmStateMachineTest.cpp - LINKLIBS ArmStateMachine health_and_arming_checks sensor_calibration ArmAuthorization mode_util + LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util ) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c51110db78..f4df954e2a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -787,9 +787,6 @@ Commander::Commander() : // default for vtol is rotary wing _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - _vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME); - _vehicle_gps_position_valid.set_hysteresis_time_from(true, 0); - _param_mav_comp_id = param_find("MAV_COMP_ID"); _param_mav_sys_id = param_find("MAV_SYS_ID"); _param_mav_type = param_find("MAV_TYPE"); @@ -2109,11 +2106,6 @@ Commander::run() _boot_timestamp = hrt_absolute_time(); - // initially set to failed - _last_lpos_fail_time_us = _boot_timestamp; - _last_gpos_fail_time_us = _boot_timestamp; - _last_lvel_fail_time_us = _boot_timestamp; - arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id); while (!should_exit()) { @@ -3209,42 +3201,6 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) } } -bool Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, - const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us, - const bool was_valid) -{ - bool valid = was_valid; - const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s) - || (data_timestamp_us == 0)); - const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); - const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); - - // Check accuracy with hysteresis in both test level and time - if (level_check_pass) { - if (!was_valid) { - // check if probation period has elapsed - if (hrt_elapsed_time(&last_fail_time_us) > 1_s) { - valid = true; - } - } - - } else { - // level check failed - if (was_valid) { - // FAILURE! no longer valid - valid = false; - } - - last_fail_time_us = hrt_absolute_time(); - } - - if (was_valid != valid) { - _status_changed = true; - } - - return valid; -} - void Commander::update_control_mode() { @@ -3745,9 +3701,6 @@ void Commander::battery_status_check() void Commander::estimator_check() { - // Check if quality checking of position accuracy and consistency is to be performed - const bool run_quality_checks = true; - _local_position_sub.update(); _global_position_sub.update(); @@ -3761,263 +3714,6 @@ void Commander::estimator_check() _heading_reset_counter = lpos.heading_reset_counter; } - const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault; - 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()) { - estimator_selector_status_s estimator_selector_status; - - if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { - if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { - _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); - _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance); - } - } - } - - if (_estimator_status_flags_sub.update()) { - const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get(); - - _vehicle_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _vehicle_status_flags.position_reliant_on_gps = gps && !optical_flow && !vision_position; - _vehicle_status_flags.position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; - _vehicle_status_flags.position_reliant_on_vision_position = !gps && !optical_flow && vision_position; - } - - // Check for a magnetomer fault and notify the user - if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) { - mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t"); - events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical, - "Stopping compass use! Land now and calibrate the compass"); - } - - if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) { - mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t"); - events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical, - "GNSS heading not reliable. Land now!"); - } - } - - - /* Check estimator status for signs of bad yaw induced post takeoff navigation failure - * for a short time interval after takeoff. - * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial - * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but - * if this does not fix the issue we need to stop using a position controlled - * mode to prevent flyaway crashes. - */ - bool pre_flt_fail_innov_heading = false; - bool pre_flt_fail_innov_vel_horiz = false; - - if (_estimator_status_sub.updated()) { - - estimator_status_s estimator_status; - - if (_estimator_status_sub.copy(&estimator_status)) { - - 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; - - if (run_quality_checks && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - if (!_arm_state_machine.isArmed()) { - _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); - - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); - - if (innovation_pass) { - _time_last_innov_pass = hrt_absolute_time(); - - // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (_vehicle_status.takeoff_time != 0) - && (hrt_elapsed_time(&_vehicle_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"); - } - } - } - } - } - } - } - - // run position and velocity accuracy checks - // Check if quality checking of position accuracy and consistency is to be performed - if (run_quality_checks) { - float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get(); - - // relax local position eph threshold in operator controlled position mode - if (_commander_state.main_state == commander_state_s::MAIN_STATE_POSCTL && - ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) - || (_vehicle_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 (_vehicle_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 (!_arm_state_machine.isArmed()) { - 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(); - - _vehicle_status_flags.global_position_valid = - check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, - _last_gpos_fail_time_us, _vehicle_status_flags.global_position_valid); - - _vehicle_status_flags.local_position_valid = - check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp, - _last_lpos_fail_time_us, _vehicle_status_flags.local_position_valid); - - _vehicle_status_flags.local_velocity_valid = - check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, - _last_lvel_fail_time_us, _vehicle_status_flags.local_velocity_valid); - } - - - // altitude - _vehicle_status_flags.local_altitude_valid = lpos.z_valid - && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s)); - - - // attitude - vehicle_attitude_s attitude{}; - _vehicle_attitude_sub.copy(&attitude); - const matrix::Quatf q{attitude.q}; - const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f) - && (fabsf(q(1)) <= 1.f) - && (fabsf(q(2)) <= 1.f) - && (fabsf(q(3)) <= 1.f); - const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f); - - const bool attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s) - && norm_in_tolerance && no_element_larger_than_one; - - if (_vehicle_status_flags.attitude_valid && !attitude_valid) { - PX4_ERR("attitude estimate no longer valid"); - } - - _vehicle_status_flags.attitude_valid = attitude_valid; - - - // angular velocity - vehicle_angular_velocity_s angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&angular_velocity); - const bool condition_angular_velocity_time_valid = (angular_velocity.timestamp != 0) - && (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s); - const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) - && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); - const bool angular_velocity_valid = condition_angular_velocity_time_valid - && condition_angular_velocity_finite; - - if (_vehicle_status_flags.angular_velocity_valid && !angular_velocity_valid) { - const char err_str[] {"angular velocity no longer valid"}; - - if (!condition_angular_velocity_time_valid) { - PX4_ERR("%s (timeout)", err_str); - - } else if (!condition_angular_velocity_finite) { - PX4_ERR("%s (non-finite values)", err_str); - } - } - - _vehicle_status_flags.angular_velocity_valid = angular_velocity_valid; - - - // gps - const bool condition_gps_position_was_valid = _vehicle_status_flags.gps_position_valid; - - if (_vehicle_gps_position_sub.updated()) { - sensor_gps_s vehicle_gps_position; - - if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) { - - bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s); - - bool fix = vehicle_gps_position.fix_type >= 2; - bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get(); - bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get(); - bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get(); - bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL); - - _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, hrt_absolute_time()); - _vehicle_status_flags.gps_position_valid = _vehicle_gps_position_valid.get_state(); - - _vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp; - - if (condition_gps_position_was_valid) { - if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) { - mavlink_log_warning(&_mavlink_log_pub, "GPS jamming state critical\t"); - events::send(events::ID("commander_gps_jamming_critical"), {events::Log::Critical, events::LogInternal::Info}, - "GPS jamming state critical"); - } - } - } - - } else { - const hrt_abstime now_us = hrt_absolute_time(); - - if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) { - _vehicle_gps_position_valid.set_state_and_update(false, now_us); - _vehicle_status_flags.gps_position_valid = false; - } - } - - if (condition_gps_position_was_valid && !_vehicle_status_flags.gps_position_valid) { - PX4_DEBUG("GPS no longer valid"); - - // report GPS failure if flying and the global position estimate is still valid - if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) { - mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t"); - events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info}, - "GPS no longer valid"); - } - } } void Commander::manual_control_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4b68a47b02..ec56cf4283 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -70,9 +70,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -84,8 +81,6 @@ #include #include #include -#include -#include #include #include #include @@ -134,10 +129,6 @@ private: void battery_status_check(); - bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, - const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us, - const bool was_valid); - void control_status_leds(bool changed, const uint8_t battery_warning); /** @@ -205,13 +196,8 @@ private: (ParamBool) _param_com_home_en, (ParamBool) _param_com_home_in_air, - (ParamFloat) _param_com_pos_fs_eph, - (ParamFloat) _param_com_pos_fs_epv, /*Not realy used for now*/ - (ParamFloat) _param_com_vel_fs_evh, (ParamInt) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ - (ParamInt) _param_com_pos_fs_delay, - (ParamInt) _param_com_low_bat_act, (ParamFloat) _param_com_bat_act_t, (ParamInt) _param_com_imb_prop_act, @@ -293,20 +279,6 @@ private: ArmStateMachine _arm_state_machine{}; - hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */ - hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */ - hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */ - - /* class variables used to check for navigation failure after takeoff */ - hrt_abstime _time_last_innov_pass{0}; /**< last time velocity and position innovations passed */ - hrt_abstime _time_last_innov_fail{0}; /**< last time velocity and position innovations failed */ - bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */ - bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */ - - static constexpr hrt_abstime GPS_VALID_TIME{3_s}; - Hysteresis _vehicle_gps_position_valid{false}; - hrt_abstime _vehicle_gps_position_timestamp_last{0}; - bool _geofence_loiter_on{false}; bool _geofence_rtl_on{false}; bool _geofence_land_on{false}; @@ -392,15 +364,11 @@ private: uORB::Subscription _action_request_sub {ORB_ID(action_request)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; @@ -414,7 +382,6 @@ private: uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; #endif // BOARD_HAS_POWER_CONTROL - uORB::SubscriptionData _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index c34e57b01e..dcce2f9bff 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -124,7 +124,7 @@ private: &_imu_consistency_checks, &_magnetometer_checks, &_manual_control_checks, - &_mode_checks, + &_mode_checks, // must be after _estimator_checks &_parachute_checks, &_power_checks, &_rc_calibration_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index d5939a6b2a..8eba402998 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -35,21 +35,48 @@ using namespace time_literals; +EstimatorChecks::EstimatorChecks() +{ + _vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME); + _vehicle_gps_position_valid.set_hysteresis_time_from(true, 0); + + // initially set to failed + _last_lpos_fail_time_us = hrt_absolute_time(); + _last_lpos_relaxed_fail_time_us = _last_lpos_fail_time_us; + _last_gpos_fail_time_us = _last_lpos_fail_time_us; + _last_lvel_fail_time_us = _last_lpos_fail_time_us; +} + void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) { - if (_param_sys_mc_est_group.get() != 2) { - return; + sensor_gps_s vehicle_gps_position; + + if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) { + checkGps(context, reporter, vehicle_gps_position); + + } else { + vehicle_gps_position = {}; } - const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; - bool missing_data = false; + vehicle_local_position_s lpos; + if (!_vehicle_local_position_sub.copy(&lpos)) { + lpos = {}; + } + + bool pre_flt_fail_innov_heading = false; + bool pre_flt_fail_innov_vel_horiz = false; + bool missing_data = false; + const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; + + // Change topics to primary estimator instance if (_param_sens_imu_mode.get() == 0) { // multi-ekf estimator_selector_status_s estimator_selector_status; if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance) - && _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + && _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance) + && _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance); if (!instance_changed) { missing_data = true; @@ -64,8 +91,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) estimator_status_s estimator_status; if (_estimator_status_sub.copy(&estimator_status)) { + 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; checkEstimatorStatus(context, reporter, estimator_status, required_groups); + checkEstimatorStatusFlags(context, reporter, estimator_status, lpos); } else { missing_data = true; @@ -87,6 +117,15 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) reporter.setIsPresent(health_component_t::local_position_estimate); checkSensorBias(context, reporter, required_groups); } + + // set mode requirements + const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid; + setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, + reporter.failsafeFlags()); + + if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) { + gpsNoLongerValid(context, reporter); + } } void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter, @@ -163,6 +202,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor // check vertical position innovation test ratio if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) { /* EVENT + * @description * * Test ratio: {1:.3}, limit: {2:.3}. * @@ -181,6 +221,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor // check velocity innovation test ratio if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) { /* EVENT + * @description * * Test ratio: {1:.3}, limit: {2:.3}. * @@ -199,6 +240,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor // check horizontal position innovation test ratio if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) { /* EVENT + * @description * * Test ratio: {1:.3}, limit: {2:.3}. * @@ -217,6 +259,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor // check magnetometer innovation test ratio if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) { /* EVENT + * @description * * Test ratio: {1:.3}, limit: {2:.3}. * @@ -256,6 +299,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { message = "Preflight%s: GPS fix too low"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -267,6 +311,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { message = "Preflight%s: not enough GPS Satellites"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -278,6 +323,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) { message = "Preflight%s: GPS PDOP too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -289,6 +335,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { message = "Preflight%s: GPS Horizontal Pos Error too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -300,6 +347,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { message = "Preflight%s: GPS Vertical Pos Error too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -311,6 +359,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { message = "Preflight%s: GPS Speed Accuracy too low"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -322,6 +371,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { message = "Preflight%s: GPS Horizontal Pos Drift too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -333,6 +383,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { message = "Preflight%s: GPS Vertical Pos Drift too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -344,6 +395,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { message = "Preflight%s: GPS Hor Speed Drift too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -355,6 +407,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { message = "Preflight%s: GPS Vert Speed Drift too high"; /* EVENT + * @description * * This check can be configured via EKF2_GPS_CHECK parameter. * @@ -415,10 +468,11 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) { /* EVENT + * @description * An accelerometer recalibration might help. * * - * Axis {1}: |{2:.8}| > {3:.8} + {4:.8} + * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} * * This check can be configured via EKF2_ABL_LIM parameter. * @@ -448,10 +502,11 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) { /* EVENT + * @description * A Gyro recalibration might help. * * - * Axis {1}: |{2:.8}| > {3:.8} + {4:.8} + * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} * * This check can be configured via EKF2_ABL_GYRLIM parameter. * @@ -471,3 +526,298 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, } } } + +void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter, + const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos) +{ + estimator_status_flags_s estimator_status_flags; + + if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { + + bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning + || estimator_status_flags.cs_inertial_dead_reckoning; + + if (!dead_reckoning) { + // position requirements (update if not dead reckoning) + bool gps = estimator_status_flags.cs_gps; + bool optical_flow = estimator_status_flags.cs_opt_flow; + bool vision_position = estimator_status_flags.cs_ev_pos; + + _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; + } + + // Check for a magnetomer fault and notify the user + if (estimator_status_flags.cs_mag_fault) { + /* EVENT + * @description + * Land and calibrate the compass. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_mag_fault"), + events::Log::Critical, "Stopping compass use"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t"); + } + } + + if (estimator_status_flags.cs_gps_yaw_fault) { + /* EVENT + * @description + * Land now + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_gnss_fault"), + events::Log::Critical, "GNSS heading not reliable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t"); + } + } + } + + const hrt_abstime now = hrt_absolute_time(); + + /* Check estimator status for signs of bad yaw induced post takeoff navigation failure + * for a short time interval after takeoff. + * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial + * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but + * if this does not fix the issue we need to stop using a position controlled + * mode to prevent flyaway crashes. + */ + + if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + if (!context.isArmed()) { + _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); + + const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); + + if (innovation_pass) { + _time_last_innov_pass = now; + + // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed + const bool sufficient_time = context.status().takeoff_time != 0 + && now - context.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 (now - _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 = now; + + if (now - _time_last_innov_pass > 2_s) { + // if the innovation test has failed continuously, declare the nav as failed + _nav_test_failed = true; + /* EVENT + * @description + * Land and recalibrate the sensors. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_nav_failure"), + events::Log::Emergency, "Navigation failure"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t"); + } + } + } + } + } + } +} + +void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const +{ + if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::None, health_component_t::gps, + events::ID("check_estimator_gps_jamming_critical"), + events::Log::Critical, "GPS reports critical jamming state"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports critical jamming state\t"); + } + } +} + +void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) const +{ + PX4_DEBUG("GPS no longer valid"); + + // report GPS failure if armed and the global position estimate is still valid + if (context.isArmed() && reporter.failsafeFlags().global_position_valid) { + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t"); + } + + events::send(events::ID("check_estimator_gps_lost"), {events::Log::Error, events::LogInternal::Info}, + "GPS no longer valid"); + } +} + +void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, + bool pre_flt_fail_innov_vel_horiz, + const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_flags_s &failsafe_flags) +{ + // The following flags correspond to mode requirements, and are reported in the corresponding mode checks + + const hrt_abstime now = hrt_absolute_time(); + + vehicle_global_position_s gpos; + + if (!_vehicle_global_position_sub.copy(&gpos)) { + gpos = {}; + } + + // run position and velocity accuracy checks + // Check if quality checking of position accuracy and consistency is to be performed + float lpos_eph_threshold_relaxed = _param_com_pos_fs_eph.get(); + + // 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 (_position_reliant_on_optical_flow) { + lpos_eph_threshold_relaxed = INFINITY; + } + + bool xy_valid = lpos.xy_valid && !_nav_test_failed; + bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; + + if (!context.isArmed()) { + 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; + } + } + + failsafe_flags.global_position_valid = + checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, + _last_gpos_fail_time_us, failsafe_flags.global_position_valid); + + failsafe_flags.local_position_valid = + checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp, + _last_lpos_fail_time_us, failsafe_flags.local_position_valid); + + failsafe_flags.local_position_valid_relaxed = + checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, + _last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid); + + failsafe_flags.local_velocity_valid = + checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, + _last_lvel_fail_time_us, failsafe_flags.local_velocity_valid); + + + // altitude + failsafe_flags.local_altitude_valid = lpos.z_valid + && (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s)); + + + // attitude + vehicle_attitude_s attitude; + + if (_vehicle_attitude_sub.copy(&attitude)) { + const matrix::Quatf q{attitude.q}; + const float eps = 1e-5f; + const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f + eps) + && (fabsf(q(1)) <= 1.f + eps) + && (fabsf(q(2)) <= 1.f + eps) + && (fabsf(q(3)) <= 1.f + eps); + const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps; + + failsafe_flags.attitude_valid = now - attitude.timestamp < 1_s && norm_in_tolerance && no_element_larger_than_one; + + } else { + failsafe_flags.attitude_valid = false; + } + + // angular velocity + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0 + && now - angular_velocity.timestamp < 1_s; + const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) + && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); + const bool angular_velocity_valid = condition_angular_velocity_time_valid + && condition_angular_velocity_finite; + + if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) { + const char err_str[] {"angular velocity no longer valid"}; + + if (!condition_angular_velocity_time_valid) { + PX4_ERR("%s (timeout)", err_str); + + } else if (!condition_angular_velocity_finite) { + PX4_ERR("%s (non-finite values)", err_str); + } + } + + failsafe_flags.angular_velocity_valid = angular_velocity_valid; + + + // gps + if (vehicle_gps_position.timestamp != 0) { + bool time = now - vehicle_gps_position.timestamp < 1_s; + + bool fix = vehicle_gps_position.fix_type >= 2; + bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get(); + bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get(); + bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get(); + bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL); + + _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now); + failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state(); + + } else { + failsafe_flags.gps_position_valid = false; + } +} + +bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, + const float required_accuracy, + const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us, + const bool was_valid) const +{ + bool valid = was_valid; + const bool data_stale = (now - data_timestamp_us > _param_com_pos_fs_delay.get() * 1_s) + || (data_timestamp_us == 0); + const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); + const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); + + // Check accuracy with hysteresis in both test level and time + if (level_check_pass) { + if (!was_valid) { + // check if probation period has elapsed + if (now - last_fail_time_us > 1_s) { + valid = true; + } + } + + } else { + // level check failed + if (was_valid) { + // FAILURE! no longer valid + valid = false; + } + + last_fail_time_us = now; + } + + return valid; +} + diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 002a2a720a..76edd4d942 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -39,13 +39,21 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; class EstimatorChecks : public HealthAndArmingCheckBase { public: - EstimatorChecks() = default; + EstimatorChecks(); ~EstimatorChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; @@ -54,13 +62,49 @@ private: void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, NavModes required_groups); void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups); + void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status, + const vehicle_local_position_s &lpos); + + void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; + void gpsNoLongerValid(const Context &context, Report &reporter) const; + void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, + const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, + vehicle_status_flags_s &failsafe_flags); + + bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, + const float required_accuracy, + const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us, + const bool was_valid) const; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + + hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec) + hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec) + hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) + hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) + + // variables used to check for navigation failure after takeoff + hrt_abstime _time_last_innov_pass{0}; ///< last time velocity and position innovations passed + hrt_abstime _time_last_innov_fail{0}; ///< last time velocity and position innovations failed + bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed + bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed + + static constexpr hrt_abstime GPS_VALID_TIME{3_s}; + systemlib::Hysteresis _vehicle_gps_position_valid{false}; + + bool _position_reliant_on_optical_flow{false}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_sys_mc_est_group, (ParamInt) _param_sens_imu_mode, (ParamInt) _param_com_arm_mag_str, (ParamFloat) _param_com_arm_ekf_hgt, @@ -68,6 +112,10 @@ private: (ParamFloat) _param_com_arm_ekf_pos, (ParamFloat) _param_com_arm_ekf_yaw, (ParamBool) _param_com_arm_wo_gps, - (ParamBool) _param_sys_has_gps + (ParamBool) _param_sys_has_gps, + (ParamFloat) _param_com_pos_fs_eph, + (ParamFloat) _param_com_pos_fs_epv, + (ParamFloat) _param_com_vel_fs_evh, + (ParamInt) _param_com_pos_fs_delay ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index b76cca5b3d..63619118f0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -64,13 +64,24 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude); } + NavModes local_position_modes = NavModes::None; + if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) { + local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position; + } + + if (!reporter.failsafeFlags().local_position_valid_relaxed + && reporter.failsafeFlags().mode_req_local_position_relaxed != 0) { + local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed; + } + + if (local_position_modes != NavModes::None) { /* EVENT */ - reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_position, health_component_t::system, + reporter.armingCheckFailure(local_position_modes, health_component_t::system, events::ID("check_modes_local_pos"), events::Log::Error, "No valid local position estimate"); - reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_position); + reporter.clearCanRunBits(local_position_modes); } if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) { diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 60d1efcd8b..3cbe06a9b7 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -45,6 +45,7 @@ void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags) flags.mode_req_angular_velocity = 0; flags.mode_req_attitude = 0; flags.mode_req_local_position = 0; + flags.mode_req_local_position_relaxed = 0; flags.mode_req_global_position = 0; flags.mode_req_local_alt = 0; flags.mode_req_mission = 0; @@ -63,7 +64,7 @@ void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags) // NAVIGATION_STATE_POSCTL setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed); if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);