From 9bad61b86b7476967f7e67e18e93c1c093a0c91e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 30 Apr 2019 03:08:23 -0400 Subject: [PATCH] Improve robustness to bad and lost airspeed data (#11846) --- msg/vehicle_status.msg | 8 + src/modules/commander/Commander.cpp | 270 ++++++++++++++++++ src/modules/commander/Commander.hpp | 33 ++- src/modules/commander/commander_params.c | 94 +++++- .../FixedwingAttitudeControl.cpp | 3 +- .../FixedwingPositionControl.cpp | 3 +- 6 files changed, 405 insertions(+), 6 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f197f2a41ad9..9eaabebc0494 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -85,3 +85,11 @@ uint8 failure_detector_status # Bitmask containing FailureDetector status [0, uint32 onboard_control_sensors_present uint32 onboard_control_sensors_enabled uint32 onboard_control_sensors_health + +# airspeed fault and airspeed use status +float32 arspd_check_level # integrated airspeed inconsistency as checked against the COM_TAS_FS_INTEG parameter +bool aspd_check_failing # true when airspeed consistency checks are failing +bool aspd_fault_declared # true when an airspeed fault has been declared +bool aspd_use_inhibit # true if switching to a non-airspeed control mode has been requested +bool aspd_fail_rtl # true if airspeed failure invoked RTL has been requested +float32 load_factor_ratio # ratio of measured to aerodynamic load factor limit. Greater than 1 indicates airspeed low error condition. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3993d5d8efff..3ebfd05a220a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -596,6 +596,8 @@ Commander::~Commander() if (_iridiumsbd_status_sub >= 0) { orb_unsubscribe(_iridiumsbd_status_sub); } + + delete[] _airspeed_fault_type; } bool @@ -1649,6 +1651,7 @@ Commander::run() } estimator_check(&status_changed); + airspeed_use_check(); /* Update land detector */ orb_check(land_detector_sub, &updated); @@ -4095,6 +4098,273 @@ void Commander::battery_status_check() } } +void Commander::airspeed_use_check() +{ + if (_airspeed_fail_action.get() < 1 || _airspeed_fail_action.get() > 4) { + // disabled + return; + } + + _airspeed_sub.update(); + const airspeed_s &airspeed = _airspeed_sub.get(); + + _sensor_bias_sub.update(); + const sensor_bias_s &sensors = _sensor_bias_sub.get(); + + // assume airspeed sensor is good before starting FW flight + bool valid_flight_condition = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && + !status.is_rotary_wing && !land_detector.landed; + bool fault_declared = false; + bool fault_cleared = false; + bool bad_number_fail = !PX4_ISFINITE(airspeed.indicated_airspeed_m_s) || !PX4_ISFINITE(airspeed.true_airspeed_m_s); + + if (!valid_flight_condition) { + + _tas_check_fail = false; + _time_last_tas_pass = hrt_absolute_time(); + _time_last_tas_fail = 0; + + _tas_use_inhibit = false; + _time_tas_good_declared = hrt_absolute_time(); + _time_tas_bad_declared = 0; + + status.aspd_check_failing = false; + status.aspd_fault_declared = false; + status.aspd_use_inhibit = false; + status.aspd_fail_rtl = false; + status.arspd_check_level = 0.0f; + + _time_last_airspeed = hrt_absolute_time(); + _time_last_aspd_innov_check = hrt_absolute_time(); + _load_factor_ratio = 0.5f; + + } else { + + // Check normalised innovation levels with requirement for continuous data and use of hysteresis + // to prevent false triggering. + float dt_s = float(1e-6 * (double)hrt_elapsed_time(&_time_last_aspd_innov_check)); + + if (dt_s < 1.0f) { + if (_estimator_status_sub.get().tas_test_ratio <= _tas_innov_threshold.get()) { + // record pass and reset integrator used to trigger + _time_last_tas_pass = hrt_absolute_time(); + _apsd_innov_integ_state = 0.0f; + + } else { + // integrate exceedance + _apsd_innov_integ_state += dt_s * (_estimator_status_sub.get().tas_test_ratio - _tas_innov_threshold.get()); + } + + status.arspd_check_level = _apsd_innov_integ_state; + + if ((_estimator_status_sub.get().vel_test_ratio < 1.0f) && (_estimator_status_sub.get().mag_test_ratio < 1.0f)) { + // nav velocity data is likely good so airspeed innovations are able to be used + if ((_tas_innov_integ_threshold.get() > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold.get())) { + _time_last_tas_fail = hrt_absolute_time(); + } + } + + if (!_tas_check_fail) { + _tas_check_fail = (hrt_elapsed_time(&_time_last_tas_pass) > TAS_INNOV_FAIL_DELAY); + + } else { + _tas_check_fail = (hrt_elapsed_time(&_time_last_tas_fail) < TAS_INNOV_FAIL_DELAY); + } + } + + _time_last_aspd_innov_check = hrt_absolute_time(); + + + // The vehicle is flying so use the status of the airspeed innovation check '_tas_check_fail' in + // addition to a sanity check using airspeed and load factor and a missing sensor data check. + + // Check if sensor data is missing - assume a minimum 5Hz data rate. + const bool data_missing = (hrt_elapsed_time(&_time_last_airspeed) > 200_ms); + + // Declare data stopped if not received for longer than 1 second + const bool data_stopped = (hrt_elapsed_time(&_time_last_airspeed) > 1_s); + + _time_last_airspeed = hrt_absolute_time(); + + // Check if the airpeed reading is lower than physically possible given the load factor + bool load_factor_ratio_fail = true; + + if (!bad_number_fail) { + float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f); + max_lift_ratio *= max_lift_ratio; + + _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(sensors.accel_z) / CONSTANTS_ONE_G) / max_lift_ratio; + _load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f); + load_factor_ratio_fail = (_load_factor_ratio > 1.1f); + status.load_factor_ratio = _load_factor_ratio; + + // sanity check independent of stall speed and load factor calculation + if (airspeed.indicated_airspeed_m_s <= 0.0f) { + bad_number_fail = true; + } + } + + // Decide if the control loops should be using the airspeed data based on the length of time the + // airspeed data has been declared bad + if (_tas_check_fail || load_factor_ratio_fail || data_missing || bad_number_fail) { + // either load factor or EKF innovation or missing data test failure can declare the airspeed bad + _time_tas_bad_declared = hrt_absolute_time(); + status.aspd_check_failing = true; + + } else if (!_tas_check_fail && !load_factor_ratio_fail && !data_missing && !bad_number_fail) { + // All checks must pass to declare airspeed good + _time_tas_good_declared = hrt_absolute_time(); + status.aspd_check_failing = false; + } + + if (!_tas_use_inhibit) { + // A simultaneous load factor and innovaton check fail makes it more likely that a large + // airspeed measurement fault has developed, so a fault should be declared immediately + const bool both_checks_failed = (_tas_check_fail && load_factor_ratio_fail); + + // Because the innovation, load factor and data missing checks are subject to short duration false positives + // a timeout period is applied. + const bool single_check_fail_timeout = (hrt_elapsed_time(&_time_tas_good_declared) > (_tas_use_stop_delay.get() * 1_s)); + + if (data_stopped || both_checks_failed || single_check_fail_timeout || bad_number_fail) { + + _tas_use_inhibit = true; + fault_declared = true; + + if (data_stopped || data_missing) { + strcpy(_airspeed_fault_type, "MISSING"); + + } else { + strcpy(_airspeed_fault_type, "FAULTY "); + } + } + + } else if (hrt_elapsed_time(&_time_tas_bad_declared) > (_tas_use_start_delay.get() * 1_s)) { + _tas_use_inhibit = false; + fault_cleared = true; + } + } + + // Do actions based on value of COM_ASPD_FS_ACT parameter + status.aspd_fault_declared = false; + status.aspd_use_inhibit = false; + status.aspd_fail_rtl = false; + + switch (_airspeed_fail_action.get()) { + case 4: { // log a message, warn the user, switch to non-airspeed TECS mode, switch to Return mode if not in a pilot controlled mode. + if (fault_declared) { + status.aspd_fault_declared = true; + status.aspd_use_inhibit = true; + + if ((internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) + || (internal_state.main_state == commander_state_s::MAIN_STATE_ACRO) + || (internal_state.main_state == commander_state_s::MAIN_STATE_STAB) + || (internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL) + || (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) + || (internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE)) { + + // don't RTL if pilot is in control + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use", _airspeed_fault_type); + + } else if (hrt_elapsed_time(&_time_tas_good_declared) < (_airspeed_rtl_delay.get() * 1_s)) { + // Wait for timeout and issue message + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use, RTL in %i sec", _airspeed_fault_type, + _airspeed_rtl_delay.get()); + + } else if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, + &internal_state)) { + + // Issue critical message even if already in RTL + status.aspd_fail_rtl = true; + + if (_airspeed_rtl_delay.get() == 0) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use and returning", _airspeed_fault_type); + + } else { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA STILL %s - returning", _airspeed_fault_type); + } + + } else { + status.aspd_fail_rtl = true; + + if (_airspeed_rtl_delay.get() == 0) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use, return failed", _airspeed_fault_type); + + } else { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA STILL %s - return failed", _airspeed_fault_type); + } + } + + } else if (fault_cleared) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD - restarting use"); + } + + // Inhibit airspeed use immediately if a bad number + if (bad_number_fail && !status.aspd_use_inhibit) { + status.aspd_use_inhibit = true; + } + + return; + } + + case 3: { // log a message, warn the user, switch to non-airspeed TECS mode + if (fault_declared) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s - stopping use", _airspeed_fault_type); + status.aspd_fault_declared = true; + status.aspd_use_inhibit = true; + + } else if (fault_cleared) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD - restarting use"); + } + + // Inhibit airspeed use immediately if a bad number + if (bad_number_fail && !status.aspd_use_inhibit) { + status.aspd_use_inhibit = true; + } + + return; + } + + case 2: { // log a message, warn the user + if (fault_declared) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA %s", _airspeed_fault_type); + status.aspd_fault_declared = true; + + } else if (fault_cleared) { + mavlink_log_critical(&mavlink_log_pub, "ASPD DATA GOOD"); + } + + // Inhibit airspeed use immediately if a bad number + if (bad_number_fail && !status.aspd_use_inhibit) { + status.aspd_use_inhibit = true; + } + + return; + } + + case 1: { // log a message + if (fault_declared) { + mavlink_log_info(&mavlink_log_pub, "ASPD DATA %s", _airspeed_fault_type); + status.aspd_fault_declared = true; + + } else if (fault_cleared) { + mavlink_log_info(&mavlink_log_pub, "ASPD DATA GOOD"); + } + + // Inhibit airspeed use immediately if a bad number + if (bad_number_fail && !status.aspd_use_inhibit) { + status.aspd_use_inhibit = true; + } + + return; + } + + default: + // Do nothing + return; + } +} + void Commander::estimator_check(bool *status_changed) { // Check if quality checking of position accuracy and consistency is to be performed diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 36700da6dcff..10b972a0e134 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -51,13 +51,16 @@ // subscriptions #include +#include #include #include #include +#include +#include #include #include #include -#include + using math::constrain; using uORB::Publication; using uORB::Subscription; @@ -118,7 +121,15 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_disarm_land, (ParamInt) _param_com_obs_avoid, - (ParamInt) _param_com_oa_boot_t + (ParamInt) _param_com_oa_boot_t, + + (ParamFloat) _tas_innov_threshold, + (ParamFloat) _tas_innov_integ_threshold, + (ParamInt) _tas_use_stop_delay, + (ParamInt) _tas_use_start_delay, + (ParamInt) _airspeed_fail_action, + (ParamFloat) _airspeed_stall, + (ParamInt) _airspeed_rtl_delay ) @@ -140,6 +151,20 @@ class Commander : public ModuleBase, public ModuleParams 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 */ + /* class variables used to check for airspeed sensor failure */ + bool _tas_check_fail{false}; /**< true when airspeed innovations have failed consistency checks */ + hrt_abstime _time_last_tas_pass{0}; /**< last time innovation checks passed */ + hrt_abstime _time_last_tas_fail{0}; /**< last time innovation checks failed */ + static constexpr hrt_abstime TAS_INNOV_FAIL_DELAY{1_s}; /**< time required for innovation levels to pass or fail (usec) */ + bool _tas_use_inhibit{false}; /**< true when the commander has instructed the control loops to not use airspeed data */ + hrt_abstime _time_tas_good_declared{0}; /**< time TAS use was started (uSec) */ + hrt_abstime _time_tas_bad_declared{0}; /**< time TAS use was stopped (uSec) */ + hrt_abstime _time_last_airspeed{0}; /**< time last airspeed measurement was received (uSec) */ + hrt_abstime _time_last_aspd_innov_check{0}; /**< time airspeed innovation was last checked (uSec) */ + char *_airspeed_fault_type = new char[7]; + float _load_factor_ratio{0.5f}; /**< ratio of maximum load factor predicted by stall speed to measured load factor */ + float _apsd_innov_integ_state{0.0f}; /**< inegral of excess normalised airspeed innovation (sec) */ + FailureDetector _failure_detector; bool _failure_detector_termination_printed{false}; @@ -171,6 +196,8 @@ class Commander : public ModuleBase, public ModuleParams void estimator_check(bool *status_changed); + void airspeed_use_check(); + void battery_status_check(); /** @@ -206,8 +233,10 @@ class Commander : public ModuleBase, public ModuleParams bool _print_avoidance_msg_once{false}; // Subscriptions + Subscription _airspeed_sub{ORB_ID(airspeed)}; Subscription _estimator_status_sub{ORB_ID(estimator_status)}; Subscription _mission_result_sub{ORB_ID(mission_result)}; + Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; Subscription _global_position_sub{ORB_ID(vehicle_global_position)}; Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f27e9f1883d7..c96ee27cfe08 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -637,7 +637,7 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * @value 0 Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. * @value 1 Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. * - * @group Mission + * @group Commander */ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); @@ -816,4 +816,94 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); * @min 0 * @max 200 */ -PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100); \ No newline at end of file +PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100); + +/** + * Airspeed failsafe consistency threshold (Experimental) + * + * This specifies the minimum airspeed test ratio as logged in estimator_status.tas_test_ratio required to trigger a failsafe. Larger values make the check less sensitive, smaller values make it more sensitive. Start with a value of 1.0 when tuning. When estimator_status.tas_test_ratio is > 1.0 it indicates the inconsistency between predicted and measured airspeed is large enough to cause the navigation EKF to reject airspeed measurements. The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the COM_TAS_FS_INTEG parameter. The subsequent failsafe response is controlled by the COM_ASPD_FS_ACT parameter. +* + * @min 0.5 + * @max 3.0 + * @group Commander + * @category Developer + */ +PARAM_DEFINE_FLOAT(COM_TAS_FS_INNOV, 1.0f); + +/** + * Airspeed failsafe consistency delay (Experimental) + * + * This sets the time integral of airspeed test ratio exceedance above COM_TAS_FS_INNOV required to trigger a failsafe. For example if COM_TAS_FS_INNOV is 100 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values make it more sensitive. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter. + * + * @unit s + * @max 30.0 + * @group Commander + * @category Developer + */ +PARAM_DEFINE_FLOAT(COM_TAS_FS_INTEG, -1.0f); + +/** + * Airspeed failsafe stop delay (Experimental) + * + * Delay before stopping use of airspeed sensor if checks indicate sensor is bad. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter. + * + * @unit s + * @group Commander + * @category Developer + * @min 1 + * @max 10 + */ +PARAM_DEFINE_INT32(COM_TAS_FS_T1, 3); + +/** + * Airspeed failsafe start delay (Experimental) + * + * Delay before switching back to using airspeed sensor if checks indicate sensor is good. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter. + * + * @unit s + * @group Commander + * @category Developer + * @min 10 + * @max 1000 + */ +PARAM_DEFINE_INT32(COM_TAS_FS_T2, 100); + +/** + * Airspeed fault detection stall airspeed. (Experimental) + * + * This is the minimum indicated airspeed at which the wing can produce 1g of lift. It is used by the airspeed sensor fault detection and failsafe calculation to detect a significant airspeed low measurement error condition and should be set based on flight test for reliable operation. The failsafe response is controlled by the COM_ASPD_FS_ACT parameter. + * + * @group Commander + * @category Developer + * @unit m/s + */ +PARAM_DEFINE_FLOAT(COM_ASPD_STALL, 10.0f); + +/** + * Airspeed fault detection (Experimental) + * + * Failsafe action when bad airspeed measurements are detected. Ensure the COM_ASPD_STALL parameter is set correctly before use. + * + * @value 0 disabled + * @value 1 log a message + * @value 2 log a message, warn the user + * @value 3 log a message, warn the user, switch to non-airspeed TECS mode + * @value 4 log a message, warn the user, switch to non-airspeed TECS mode, switch to Return mode after COM_ASPD_FS_DLY seconds + * @group Commander + * @category Developer + */ +PARAM_DEFINE_INT32(COM_ASPD_FS_ACT, 0); + +/** + * Airspeed fault detection delay before RTL (Experimental) + * + * RTL delay after bad airspeed measurements are detected if COM_ASPD_FS_ACT is set to 4. Ensure the COM_ASPD_STALL parameter is set correctly before use. The failsafe start and stop delays are controlled by the COM_TAS_FS_T1 and COM_TAS_FS_T2 parameters. Additional protection against persistent airspeed sensor errors can be enabled using the COM_TAS_FS_INNOV parameter, but these addtional checks are more prone to false positives in windy conditions. + * + * @min 0 + * @max 300 + * @unit s + * @group Commander + * @category Developer + */ +PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0); + diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 8c59b5a4bee5..e5062095e685 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -472,7 +472,8 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() { _airspeed_sub.update(); const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s) - && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s); + && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s) + && !_vehicle_status.aspd_use_inhibit; if (!airspeed_valid) { perf_count(_nonfinite_input_perf); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 6b1709ae46eb..735be816a0ad 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -391,7 +391,8 @@ FixedwingPositionControl::airspeed_poll() if (PX4_ISFINITE(as.indicated_airspeed_m_s) && PX4_ISFINITE(as.true_airspeed_m_s) - && (as.indicated_airspeed_m_s > 0.0f)) { + && (as.indicated_airspeed_m_s > 0.0f) + && !_vehicle_status.aspd_use_inhibit) { airspeed_valid = true;