diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 635376ddcf..5438581988 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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 diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index fd09f18ce2..0def47d3b5 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -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}; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index fd47faadef..8d1da15249 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -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; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index e69483c7c3..295f95c8d3 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -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 diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ab20579b79..d969acd19e 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index bf696eeba6..191c980e09 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -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 */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 7b8f94cc4a..77c440b7b4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2f79a185bf..2d2ed56f9b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -98,16 +98,6 @@ class VtolAttitudeControl : public ModuleBase, public Modul { public: - enum class QuadchuteReason { - TransitionTimeout = 0, - ExternalCommand, - MinimumAltBreached, - LossOfAlt, - LargeAltError, - MaximumPitchExceeded, - MaximumRollExceeded, - }; - VtolAttitudeControl(); ~VtolAttitudeControl() override; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index b5b28448bc..96f1fa592d 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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(_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(_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(_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(_param_vt_fw_qc_r.get())))) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded); - } + if (reason != QuadchuteReason::None) { + _attc->quadchute(reason); } } } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5ae921722c..c3363665f7 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -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;