fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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 14:14:25 -05:00
committed by Daniel Agar
parent a46581987c
commit ebdc29bc5f
21 changed files with 381 additions and 571 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tiltrotor.cpp
+22 -15
View File
@@ -137,20 +137,25 @@ void Tiltrotor::update_vtol_state()
break;
case vtol_mode::TRANSITION_FRONT_P1: {
// allow switch if we are not armed for the sake of bench testing
bool transition_to_p2 = can_transition_on_ground();
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached airspeed to switch to fw mode
transition_to_p2 |= !_params->airspeed_disabled &&
_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed &&
time_since_trans_start > _params->front_trans_time_min;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
&& !_params->airspeed_disabled;
// check if airspeed is invalid and transition by time
transition_to_p2 |= _params->airspeed_disabled &&
_tilt_control >= _params_tiltrotor.tilt_transition &&
time_since_trans_start > _params->front_trans_time_openloop;
bool transition_to_p2 = false;
if (time_since_trans_start > _params->front_trans_time_min) {
if (airspeed_triggers_transition) {
transition_to_p2 = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
} else {
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
time_since_trans_start > _params->front_trans_time_openloop;;
}
}
transition_to_p2 |= can_transition_on_ground();
if (transition_to_p2) {
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
@@ -244,18 +249,20 @@ void Tiltrotor::update_transition_state()
_mc_yaw_weight = 1.0f;
// reduce MC controls once the plane has picked up speed
if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
_airspeed_validated->equivalent_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
if (!_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend) {
_mc_roll_weight = 1.0f - (_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
_airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend) {
_mc_roll_weight = 1.0f - (_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
(_params->transition_airspeed - _params->airspeed_blend);
}
// without airspeed do timed weight changes
if (_params->airspeed_disabled
&& time_since_trans_start > _params->front_trans_time_min) {
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) &&
time_since_trans_start > _params->front_trans_time_min) {
_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
(_params->front_trans_time_openloop - _params->front_trans_time_min);
_mc_yaw_weight = _mc_roll_weight;