Airspeed Selector: enable airspeed_validated in control modules (#12887)

* FW attitude controller, FW position controller and VTOL attitude controller subscribe to airspeed_validated topic
* add possibility to switch off the airspeed valid checks
* remove airspeed valid checks from commander
* clean up of VTOL transition logic
* Airspeed Selector: remove dynamic allocation of airspeed validators (depending on number of connected sensors) but do it statically for the maximum number allowed. Check for number of connected sensors not only during start up, but always when vehicle is disarmed.
* Airspeed Selector: change work queue from lp to att_pos_ctrl as this module is safety-critical
* add airspeed selector to px4_fmu-v2 defaults
This commit is contained in:
Silvan Fuhrer
2019-11-21 20:14:26 +01:00
committed by Daniel Agar
parent a46581987c
commit ebdc29bc5f
21 changed files with 381 additions and 571 deletions
-275
View File
@@ -577,11 +577,6 @@ Commander::Commander() :
status_flags.avoidance_system_valid = false;
}
Commander::~Commander()
{
delete[] _airspeed_fault_type;
}
bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
@@ -1710,7 +1705,6 @@ Commander::run()
}
estimator_check(&status_changed);
airspeed_use_check();
/* Update land detector */
if (land_detector_sub.updated()) {
@@ -4018,275 +4012,6 @@ 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();
vehicle_acceleration_s accel{};
_vehicle_acceleration_sub.copy(&accel);
bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
// assume airspeed sensor is good before starting FW flight
bool valid_flight_condition = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) &&
is_fixed_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(accel.xyz[2]) / 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
+1 -24
View File
@@ -77,7 +77,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
{
public:
Commander();
~Commander();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -133,15 +132,9 @@ private:
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(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,
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
@@ -184,20 +177,6 @@ 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) */
bool _geofence_loiter_on{false};
bool _geofence_rtl_on{false};
bool _geofence_warning_action_on{false};
@@ -240,8 +219,6 @@ private:
void offboard_control_update(bool &status_changed);
void airspeed_use_check();
void battery_status_check();
void esc_status_check(const esc_status_s &esc_status);
-89
View File
@@ -848,95 +848,6 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
*/
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);
/**
* User Flight Profile
*