dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Refactor quadchute logic (#20704) 

* 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
parent 2b1d8c1d8e
commit cfb670fbb3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 336 additions and 237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/modules/vtol_att_control/standard.cpp
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

5
src/modules/vtol_att_control/standard.h
View File

@ -72,10 +72,7 @@ private:
FW_MODE
};
struct {
vtol_mode flight_mode; // indicates in which mode the vehicle is in
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
} _vtol_schedule;
vtol_mode _vtol_mode{vtol_mode::MC_MODE}; /**< vtol flight mode, defined by enum vtol_mode */
float _pusher_throttle{0.0f};
float _reverse_output{0.0f};

75
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -52,8 +52,6 @@ using namespace matrix;
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc)
{
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_schedule.transition_start = 0;
}
void
@ -75,7 +73,7 @@ void Tailsitter::update_vtol_state()
if (_vtol_vehicle_status->vtol_transition_failsafe) {
// Failsafe event, switch to MC mode immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
//reset failsafe when FW is no longer requested
if (!_attc->is_fixed_wing_requested()) {
@ -84,26 +82,25 @@ void Tailsitter::update_vtol_state()
} else if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
switch (_vtol_mode) { // user switchig to MC mode
case vtol_mode::MC_MODE:
break;
case vtol_mode::FW_MODE:
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_BACK;
_transition_start_timestamp = hrt_absolute_time();
break;
case vtol_mode::TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
break;
case vtol_mode::TRANSITION_BACK:
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached pitch angle to switch to MC mode
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
_vtol_mode = vtol_mode::MC_MODE;
}
break;
@ -111,11 +108,11 @@ void Tailsitter::update_vtol_state()
} else { // user switchig to FW mode
switch (_vtol_schedule.flight_mode) {
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
_transition_start_timestamp = hrt_absolute_time();
break;
case vtol_mode::FW_MODE:
@ -123,9 +120,6 @@ void Tailsitter::update_vtol_state()
case vtol_mode::TRANSITION_FRONT_P1: {
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_param_fw_arsp_mode.get() ;
@ -143,15 +137,7 @@ void Tailsitter::update_vtol_state()
transition_to_fw |= can_transition_on_ground();
if (transition_to_fw) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
}
// 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);
}
_vtol_mode = vtol_mode::FW_MODE;
}
break;
@ -159,29 +145,29 @@ void Tailsitter::update_vtol_state()
case vtol_mode::TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_vtol_mode = vtol_mode::FW_MODE;
break;
}
}
// map tailsitter 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;
_flag_was_in_trans_mode = false;
break;
case vtol_mode::FW_MODE:
_vtol_mode = mode::FIXED_WING;
_common_vtol_mode = mode::FIXED_WING;
_flag_was_in_trans_mode = false;
break;
case vtol_mode::TRANSITION_FRONT_P1:
_vtol_mode = mode::TRANSITION_TO_FW;
_common_vtol_mode = mode::TRANSITION_TO_FW;
break;
case vtol_mode::TRANSITION_BACK:
_vtol_mode = mode::TRANSITION_TO_MC;
_common_vtol_mode = mode::TRANSITION_TO_MC;
break;
}
}
@ -189,7 +175,6 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_transition_state()
{
const hrt_abstime now = hrt_absolute_time();
const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
// we need the incoming (virtual) mc attitude setpoints to be recent, otherwise return (means the previous setpoint stays active)
if (_mc_virtual_att_sp->timestamp < (now - 1_s)) {
@ -199,7 +184,7 @@ void Tailsitter::update_transition_state()
if (!_flag_was_in_trans_mode) {
_flag_was_in_trans_mode = true;
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
// calculate rotation axis for transition.
_q_trans_start = Quatf(_v_att->q);
Vector3f z = -_q_trans_start.dcm_z();
@ -223,7 +208,7 @@ void Tailsitter::update_transition_state()
// multirotor frame
_q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
// initial attitude setpoint for the transition should be with wings level
_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
@ -243,32 +228,24 @@ void Tailsitter::update_transition_state()
cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt;
const float tilt = acosf(cos_tilt);
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
// calculate pitching rate - and constrain to at least 0.1s transition time
const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_f_trans_dur.get(), 0.1f);
if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
_time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
// 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);
}
}
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
// calculate pitching rate - and constrain to at least 0.1s transition time
const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
if (tilt > 0.01f) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
_time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
}
@ -336,7 +313,7 @@ void Tailsitter::fill_actuator_outputs()
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW];
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
if (_vtol_mode == vtol_mode::FW_MODE) {
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
@ -371,14 +348,14 @@ void Tailsitter::fill_actuator_outputs()
}
// Landing Gear
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_vtol_mode == vtol_mode::MC_MODE) {
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
} else {
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
}
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;

5
src/modules/vtol_att_control/tailsitter.h
View File

@ -69,10 +69,7 @@ private:
FW_MODE /**< vtol is in fixed wing mode */
};
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
vtol_mode _vtol_mode{vtol_mode::MC_MODE}; /**< vtol flight mode, defined by enum vtol_mode */
bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition

94
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -55,12 +55,6 @@ using namespace time_literals;
Tiltrotor::Tiltrotor(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;
}
void
@ -79,7 +73,7 @@ void Tiltrotor::update_vtol_state()
if (_vtol_vehicle_status->vtol_transition_failsafe) {
// Failsafe event, switch to MC mode immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
//reset failsafe when FW is no longer requested
if (!_attc->is_fixed_wing_requested()) {
@ -89,23 +83,23 @@ void Tiltrotor::update_vtol_state()
} else if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
switch (_vtol_schedule.flight_mode) {
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
break;
case vtol_mode::FW_MODE:
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_BACK;
_transition_start_timestamp = hrt_absolute_time();
break;
case vtol_mode::TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
break;
case vtol_mode::TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
break;
case vtol_mode::TRANSITION_BACK:
@ -123,11 +117,10 @@ void Tiltrotor::update_vtol_state()
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
}
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
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 (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_mode = vtol_mode::MC_MODE;
}
break;
@ -135,11 +128,11 @@ void Tiltrotor::update_vtol_state()
} else {
switch (_vtol_schedule.flight_mode) {
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
_transition_start_timestamp = hrt_absolute_time();
break;
case vtol_mode::FW_MODE:
@ -147,36 +140,26 @@ void Tiltrotor::update_vtol_state()
case vtol_mode::TRANSITION_FRONT_P1: {
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_param_fw_arsp_mode.get() ;
bool transition_to_p2 = false;
if (time_since_trans_start > getMinimumFrontTransitionTime()) {
if (_time_since_trans_start > getMinimumFrontTransitionTime()) {
if (airspeed_triggers_transition) {
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
} else {
transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
time_since_trans_start > getOpenLoopFrontTransitionTime();
_time_since_trans_start > getOpenLoopFrontTransitionTime();
}
}
transition_to_p2 |= can_transition_on_ground();
if (transition_to_p2) {
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
// 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);
}
_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
_transition_start_timestamp = hrt_absolute_time();
}
break;
@ -186,7 +169,7 @@ void Tiltrotor::update_vtol_state()
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _param_vt_tilt_fw.get()) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_vtol_mode = vtol_mode::FW_MODE;
_tilt_control = _param_vt_tilt_fw.get();
}
@ -194,28 +177,28 @@ void Tiltrotor::update_vtol_state()
case vtol_mode::TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_vtol_mode = vtol_mode::FW_MODE;
break;
}
}
// map tiltrotor 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_FRONT_P1:
case vtol_mode::TRANSITION_FRONT_P2:
_vtol_mode = mode::TRANSITION_TO_FW;
_common_vtol_mode = mode::TRANSITION_TO_FW;
break;
case vtol_mode::TRANSITION_BACK:
_vtol_mode = mode::TRANSITION_TO_MC;
_common_vtol_mode = mode::TRANSITION_TO_MC;
break;
}
}
@ -304,16 +287,15 @@ void Tiltrotor::update_transition_state()
_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
}
const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
// for the first part of the transition all rotors are enabled
// tilt rotors forward up to certain angle
if (_tilt_control <= _param_vt_tilt_trans.get()) {
const float ramped_up_tilt = _param_vt_tilt_mc.get() +
fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
time_since_trans_start / _param_vt_f_trans_dur.get() ;
_time_since_trans_start / _param_vt_f_trans_dur.get() ;
// only allow increasing tilt (tilt in hover can already be non-zero)
_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@ -333,8 +315,8 @@ void Tiltrotor::update_transition_state()
// without airspeed do timed weight changes
if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
time_since_trans_start > getMinimumFrontTransitionTime()) {
_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
_time_since_trans_start > getMinimumFrontTransitionTime()) {
_mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) /
(getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
_mc_yaw_weight = _mc_roll_weight;
}
@ -346,10 +328,10 @@ void Tiltrotor::update_transition_state()
_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_FRONT_P2) {
} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * _time_since_trans_start /
_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
_mc_roll_weight = 0.0f;
@ -366,12 +348,12 @@ void Tiltrotor::update_transition_state()
_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_BACK) {
} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
// tilt rotors back once motors are idle
if (time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
if (_time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
float progress = (_time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
progress = math::constrain(progress, 0.0f, 1.0f);
_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
}
@ -383,14 +365,14 @@ void Tiltrotor::update_transition_state()
_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
}
if (time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
// blend throttle from FW value to 0
_mc_throttle_weight = 1.0f;
const float target_throttle = 0.0f;
const float progress = time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
const float progress = _time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
blendThrottleDuringBacktransition(progress, target_throttle);
} else if (time_since_trans_start < timeUntilMotorsAreUp()) {
} else if (_time_since_trans_start < timeUntilMotorsAreUp()) {
// while we quickly rotate back the motors keep throttle at idle
// turn on all MC motors
@ -402,7 +384,7 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
// slowly ramp up throttle to avoid step inputs
float progress = (time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
float progress = (_time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
progress = math::constrain(progress, 0.0f, 1.0f);
_mc_throttle_weight = moveLinear(0.0f, 1.0f, progress);
}
@ -469,7 +451,7 @@ void Tiltrotor::fill_actuator_outputs()
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
if (_vtol_mode == vtol_mode::FW_MODE) {
// for the legacy mixing system pubish FW throttle on the MC output
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
@ -494,7 +476,7 @@ void Tiltrotor::fill_actuator_outputs()
}
// Landing gear
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_vtol_mode == vtol_mode::MC_MODE) {
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
} else {
@ -504,7 +486,7 @@ void Tiltrotor::fill_actuator_outputs()
// Fixed wing output
fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 0;

6
src/modules/vtol_att_control/tiltrotor.h
View File

@ -75,11 +75,7 @@ private:
* they need to idle otherwise they need too much time to spin up for mc mode.
*/
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
vtol_mode _vtol_mode{vtol_mode::MC_MODE}; /**< vtol flight mode, defined by enum vtol_mode */
float _tilt_control{0.0f}; /**< actuator value for the tilt servo */

4
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -225,6 +225,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical,
"Quadchute triggered, due to maximum roll angle exceeded");
break;
case QuadchuteReason::None:
// should never get in here
return;
}
_vtol_vehicle_status.vtol_transition_failsafe = true;

10
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -98,16 +98,6 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public Modul
{
public:
enum class QuadchuteReason {
TransitionTimeout = 0,
ExternalCommand,
MinimumAltBreached,
LossOfAlt,
LargeAltError,
MaximumPitchExceeded,
MaximumRollExceeded,
};
VtolAttitudeControl();
~VtolAttitudeControl() override;

209
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -54,7 +54,7 @@ using namespace matrix;
VtolType::VtolType(VtolAttitudeControl *att_controller) :
ModuleParams(nullptr),
_attc(att_controller),
_vtol_mode(mode::ROTARY_WING)
_common_vtol_mode(mode::ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
@ -171,6 +171,8 @@ void VtolType::update_transition_state()
_last_loop_ts = t_now;
_throttle_blend_start_ts = t_now;
_time_since_trans_start = (float)(t_now - _transition_start_timestamp) * 1e-6f;
check_quadchute_condition();
}
@ -210,76 +212,167 @@ bool VtolType::can_transition_on_ground()
return !_v_control_mode->flag_armed || _land_detected->landed;
}
void VtolType::check_quadchute_condition()
bool VtolType::isQuadchuteEnabled()
{
return _v_control_mode->flag_armed && !_land_detected->landed;
}
bool VtolType::isMinAltBreached()
{
// fixed-wing minimum altitude
if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
return true;
}
}
return false;
}
bool VtolType::largeAltitudeLoss()
{
// adaptive quadchute
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) {
// are we dropping while requesting significant ascend?
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
(_ra_hrate < -1.0f) &&
(_ra_hrate_sp > 1.0f)) {
return true;
}
}
return false;
}
bool VtolType::largeAltitudeError()
{
// adaptive quadchute
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && !_tecs_running) {
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
if (height_error && height_rate_error) {
return true;
}
}
return false;
}
bool VtolType::isPitchExceeded()
{
// fixed-wing maximum pitch angle
if (_param_vt_fw_qc_p.get() > 0) {
Eulerf euler = Quatf(_v_att->q);
if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
return true;
}
}
return false;
}
bool VtolType::isRollExceeded()
{
// fixed-wing maximum roll angle
if (_param_vt_fw_qc_r.get() > 0) {
Eulerf euler = Quatf(_v_att->q);
if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
return true;
}
}
return false;
}
bool VtolType::isFrontTransitionTimeout()
{
// check front transition timeout
if (_param_vt_trans_timeout.get() > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW) {
if (_time_since_trans_start > _param_vt_trans_timeout.get()) {
// transition timeout occured, abort transition
return true;
}
}
return false;
}
QuadchuteReason VtolType::getQuadchuteReason()
{
if (isMinAltBreached()) {
return QuadchuteReason::MinimumAltBreached;
}
if (largeAltitudeLoss()) {
return QuadchuteReason::LossOfAlt;
}
if (largeAltitudeError()) {
return QuadchuteReason::LargeAltError;
}
if (isPitchExceeded()) {
return QuadchuteReason::MaximumPitchExceeded;
}
if (isRollExceeded()) {
return QuadchuteReason::MaximumRollExceeded;
}
if (isFrontTransitionTimeout()) {
return QuadchuteReason::TransitionTimeout;
}
return QuadchuteReason::None;
}
void VtolType::filterTecsHeightRates()
{
if (_tecs_running) {
// 1 second rolling average
_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
} else {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
}
void VtolType::handleSpecialExternalCommandQuadchute()
{
if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
&& !_quadchute_command_treated) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand);
_attc->quadchute(QuadchuteReason::ExternalCommand);
_quadchute_command_treated = true;
_attc->reset_immediate_transition();
} else {
_quadchute_command_treated = false;
}
}
if (!_tecs_running) {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
void VtolType::check_quadchute_condition()
{
if (_v_control_mode->flag_armed && !_land_detected->landed) {
Eulerf euler = Quatf(_v_att->q);
filterTecsHeightRates();
handleSpecialExternalCommandQuadchute();
// fixed-wing minimum altitude
if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
if (isQuadchuteEnabled()) {
QuadchuteReason reason = getQuadchuteReason();
if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
}
}
// adaptive quadchute
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
// We use tecs for tracking in FW and local_pos_sp during transitions
if (_tecs_running) {
// 1 second rolling average
_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
// are we dropping while requesting significant ascend?
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
(_ra_hrate < -1.0f) &&
(_ra_hrate_sp > 1.0f)) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt);
}
} else {
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
if (height_error && height_rate_error) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError);
}
}
}
// fixed-wing maximum pitch angle
if (_param_vt_fw_qc_p.get() > 0) {
if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
}
}
// fixed-wing maximum roll angle
if (_param_vt_fw_qc_r.get() > 0) {
if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
}
if (reason != QuadchuteReason::None) {
_attc->quadchute(reason);
}
}
}

84
src/modules/vtol_att_control/vtol_type.h
View File

@ -84,6 +84,17 @@ enum class VtFwDifthrEnBits : int32_t {
PITCH_BIT = (1 << 2),
};
enum class QuadchuteReason {
None = 0,
TransitionTimeout,
ExternalCommand,
MinimumAltBreached,
LossOfAlt,
LargeAltError,
MaximumPitchExceeded,
MaximumRollExceeded,
};
class VtolAttitudeControl;
class VtolType : public ModuleParams
@ -132,6 +143,73 @@ public:
*/
virtual void waiting_on_tecs() {}
/**
* @brief Indicates if quadchute is enabled.
*
* @return true if enabled
*/
bool isQuadchuteEnabled();
/**
* @brief Evaluates quadchute conditions and returns a reson for quadchute.
*
* @return QuadchuteReason, can be None
*/
QuadchuteReason getQuadchuteReason();
/**
* @brief Indicates if the vehicle is lower than VT_FW_MIN_ALT above the local origin.
*
* @return true if below threshold
*/
bool isMinAltBreached();
/**
* @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly.
* This only applies when TECS is running.
*
* @return true if error larger than threshold
*/
bool largeAltitudeLoss();
/**
* @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR. This only applied when TECS is not running.
*
* @return true if error larger than threshold
*/
bool largeAltitudeError();
/**
* @brief Indicates if the absolute value of the vehicle pitch angle exceeds the threshold defined by VT_FW_QC_P
*
* @return true if exeeded
*/
bool isPitchExceeded();
/**
* @brief Indicates if the absolute value of the vehicle roll angle exceeds the threshold defined by VT_FW_QC_R
*
* @return true if exeeded
*/
bool isRollExceeded();
/**
* @brief Indicates if the front transition duration has exceeded the timeout definded by VT_TRANS_TIMEOUT
*
* @return true if exeeded
*/
bool isFrontTransitionTimeout();
/**
* @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint.
*/
void filterTecsHeightRates();
/**
* @brief Special handling of QuadchuteReason::ReasonExternal
*/
void handleSpecialExternalCommandQuadchute();
/**
* Checks for fixed-wing failsafe condition and issues abort request if needed.
*/
@ -149,7 +227,7 @@ public:
virtual void blendThrottleAfterFrontTransition(float scale) {};
mode get_mode() {return _vtol_mode;}
mode get_mode() {return _common_vtol_mode;}
/**
* @return Minimum front transition time scaled for air density (if available) [s]
@ -172,7 +250,7 @@ public:
protected:
VtolAttitudeControl *_attc;
mode _vtol_mode;
mode _common_vtol_mode;
static constexpr const int num_outputs_max = 8;
@ -210,6 +288,8 @@ protected:
float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition
hrt_abstime _trans_finished_ts = 0;
hrt_abstime _transition_start_timestamp{0};
float _time_since_trans_start{0};
bool _tecs_running = false;
hrt_abstime _tecs_running_ts = 0;