forked from Archive/PX4-Autopilot
Improve robustness to bad and lost airspeed data (#11846)
This commit is contained in:
Daniel Agar
GitHub
parent
59ebb9af0a
commit
9bad61b86b
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -51,13 +51,16 @@
|
|||
|
||||
// subscriptions
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/iridiumsbd_status.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
using math::constrain;
|
||||
using uORB::Publication;
|
||||
using uORB::Subscription;
|
||||
|
|
@ -118,7 +121,15 @@ private:
|
|||
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
|
||||
|
||||
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
|
||||
(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t
|
||||
(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
|
||||
|
||||
(ParamFloat<px4::params::COM_TAS_FS_INNOV>) _tas_innov_threshold,
|
||||
(ParamFloat<px4::params::COM_TAS_FS_INTEG>) _tas_innov_integ_threshold,
|
||||
(ParamInt<px4::params::COM_TAS_FS_T1>) _tas_use_stop_delay,
|
||||
(ParamInt<px4::params::COM_TAS_FS_T2>) _tas_use_start_delay,
|
||||
(ParamInt<px4::params::COM_ASPD_FS_ACT>) _airspeed_fail_action,
|
||||
(ParamFloat<px4::params::COM_ASPD_STALL>) _airspeed_stall,
|
||||
(ParamInt<px4::params::COM_ASPD_FS_DLY>) _airspeed_rtl_delay
|
||||
|
||||
)
|
||||
|
||||
|
|
@ -140,6 +151,20 @@ private:
|
|||
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 @@ private:
|
|||
|
||||
void estimator_check(bool *status_changed);
|
||||
|
||||
void airspeed_use_check();
|
||||
|
||||
void battery_status_check();
|
||||
|
||||
/**
|
||||
|
|
@ -206,8 +233,10 @@ private:
|
|||
bool _print_avoidance_msg_once{false};
|
||||
|
||||
// Subscriptions
|
||||
Subscription<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
|
||||
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
|
||||
Subscription<sensor_bias_s> _sensor_bias_sub{ORB_ID(sensor_bias)};
|
||||
Subscription<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
Subscription<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue