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

Refactor quadchute logic (#20704)

Browse Source 
* moved computation of _time_since_trans_start into base class

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* refactor quadchute logic
- move entired logic into VtolType class
- split into smaller functions

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* Update src/modules/vtol_att_control/vtol_type.h

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Roman Bapst
2022-12-08 11:56:20 +03:00
committed by GitHub
co-authored by Silvan Fuhrer
parent 2b1d8c1d8e
commit cfb670fbb3
10 changed files with 336 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+32 -49
View File
@@ -52,13 +52,6 @@ using namespace matrix;
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc)
{
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
}
void
@@ -78,11 +71,10 @@ void Standard::update_vtol_state()
*/
float mc_weight = _mc_roll_weight;
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (_vtol_vehicle_status->vtol_transition_failsafe) {
// Failsafe event, engage mc motors immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
@@ -94,26 +86,26 @@ void Standard::update_vtol_state()
} else if (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_vtol_mode == vtol_mode::MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
mc_weight = 1.0f;
_reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
} else if (_vtol_mode == vtol_mode::FW_MODE) {
// Regular backtransition
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_TO_MC;
_transition_start_timestamp = hrt_absolute_time();
_reverse_output = _param_vt_b_rev_out.get();
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
mc_weight = 1.0f;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;
@@ -126,33 +118,33 @@ void Standard::update_vtol_state()
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
}
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get();
if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
}
}
} else {
// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
if (_vtol_mode == vtol_mode::MC_MODE || _vtol_mode == vtol_mode::TRANSITION_TO_MC) {
// start transition to fw mode
/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
* unsafe flying state. */
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_TO_FW;
_transition_start_timestamp = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
} else if (_vtol_mode == vtol_mode::FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_vtol_mode = vtol_mode::FW_MODE;
mc_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_param_fw_arsp_mode.get();
const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
bool transition_to_fw = false;
@@ -168,7 +160,7 @@ void Standard::update_vtol_state()
transition_to_fw |= can_transition_on_ground();
if (transition_to_fw) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_vtol_mode = vtol_mode::FW_MODE;
// don't set pusher throttle here as it's being ramped up elsewhere
_trans_finished_ts = hrt_absolute_time();
@@ -182,21 +174,21 @@ void Standard::update_vtol_state()
_mc_throttle_weight = mc_weight;
// map specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
_vtol_mode = mode::ROTARY_WING;
_common_vtol_mode = mode::ROTARY_WING;
break;
case vtol_mode::FW_MODE:
_vtol_mode = mode::FIXED_WING;
_common_vtol_mode = mode::FIXED_WING;
break;
case vtol_mode::TRANSITION_TO_FW:
_vtol_mode = mode::TRANSITION_TO_FW;
_common_vtol_mode = mode::TRANSITION_TO_FW;
break;
case vtol_mode::TRANSITION_TO_MC:
_vtol_mode = mode::TRANSITION_TO_MC;
_common_vtol_mode = mode::TRANSITION_TO_MC;
break;
}
}
@@ -205,7 +197,6 @@ void Standard::update_transition_state()
{
const hrt_abstime now = hrt_absolute_time();
float mc_weight = 1.0f;
const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
VtolType::update_transition_state();
@@ -230,7 +221,7 @@ void Standard::update_transition_state()
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
// just set the final target throttle value
_pusher_throttle = _param_vt_f_trans_thr.get();
@@ -248,14 +239,14 @@ void Standard::update_transition_state()
PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
time_since_trans_start > getMinimumFrontTransitionTime()) {
_time_since_trans_start > getMinimumFrontTransitionTime()) {
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime();
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
}
@@ -266,19 +257,11 @@ void Standard::update_transition_state()
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
// check front transition timeout
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
if (time_since_trans_start > _param_vt_trans_timeout.get()) {
// transition timeout occured, abort transition
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
}
}
// set spoiler and flaps to 0
_flaps_setpoint_with_slewrate.update(0.f, _dt);
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
if (_v_control_mode->flag_control_climb_rate_enabled) {
// control backtransition deceleration using pitch.
@@ -290,15 +273,15 @@ void Standard::update_transition_state()
_pusher_throttle = 0.0f;
if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
if (_time_since_trans_start >= _param_vt_b_rev_del.get()) {
// Handle throttle reversal for active breaking
_pusher_throttle = math::constrain((time_since_trans_start - _param_vt_b_rev_del.get())
_pusher_throttle = math::constrain((_time_since_trans_start - _param_vt_b_rev_del.get())
* _param_vt_psher_slew.get(), 0.0f, _param_vt_b_trans_thr.get());
}
// continually increase mc attitude control as we transition back to mc mode
if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get();
}
}
@@ -334,7 +317,7 @@ void Standard::fill_actuator_outputs()
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
switch (_vtol_schedule.flight_mode) {
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
// MC out = MC in