diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 2e96304024..3727fab8b8 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -420,3 +420,9 @@ Standard::waiting_on_tecs() // keep thrust from transition _v_att_sp->thrust_body[0] = _pusher_throttle; }; + +void Standard::blendThrottleAfterFrontTransition(float scale) +{ + const float tecs_throttle = _v_att_sp->thrust_body[0]; + _v_att_sp->thrust_body[0] = scale * tecs_throttle + (1.0f - scale) * _pusher_throttle; +} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 065b634296..baecb4375d 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -63,6 +63,7 @@ public: void update_mc_state() override; void fill_actuator_outputs() override; void waiting_on_tecs() override; + void blendThrottleAfterFrontTransition(float scale) override; private: diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ff90df1f67..fd462935f7 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -46,6 +46,9 @@ using namespace matrix; using namespace time_literals; #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 1.0f +#define BACKTRANS_THROTTLE_UPRAMP_DUR_S 1.0f; +#define BACKTRANS_MOTORS_UPTILT_DUR_S 1.0f; Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc) @@ -281,6 +284,10 @@ void Tiltrotor::update_fw_state() { VtolType::update_fw_state(); + // this is needed to avoid a race condition when entering backtransition when the mc rate controller publishes + // a zero throttle value + _v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0]; + // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; } @@ -362,6 +369,10 @@ void Tiltrotor::update_transition_state() set_alternate_motor_state(motor_state::VALUE, ramp_down_value); + + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; + _v_att_sp->thrust_body[0] = _thrust_transition; + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { // turn on all MC motors set_all_motor_state(motor_state::ENABLED); @@ -371,10 +382,12 @@ void Tiltrotor::update_transition_state() _flag_idle_mc = set_idle_mc(); } - // tilt rotors back - if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * time_since_trans_start / 1.0f; + // tilt rotors back once motors are idle + 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; + progress = math::constrain(progress, 0.0f, 1.0f); + _tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress); } _mc_yaw_weight = 1.0f; @@ -385,7 +398,12 @@ void Tiltrotor::update_transition_state() } // while we quickly rotate back the motors keep throttle at idle - if (time_since_trans_start < 1.0f) { + if (time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { + _mc_throttle_weight = 1.0f; + const float target_throttle = 0.0f; + blendThrottleDuringBacktransition(time_since_trans_start, target_throttle); + + } else if (time_since_trans_start < timeUntilMotorsAreUp()) { _mc_throttle_weight = 0.0f; _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; @@ -394,7 +412,9 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; // slowly ramp up throttle to avoid step inputs - _mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f; + 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); } } @@ -472,3 +492,26 @@ float Tiltrotor::thrust_compensation_for_tilt() // increase vertical thrust by 1/cos(tilt), limit to [-1,0] return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f); } + +void Tiltrotor::blendThrottleAfterFrontTransition(float scale) +{ + const float tecs_throttle = _v_att_sp->thrust_body[0]; + + _v_att_sp->thrust_body[0] = scale * tecs_throttle + (1.0f - scale) * _thrust_transition; +} + +void Tiltrotor::blendThrottleDuringBacktransition(float scale, float target_throttle) +{ + _v_att_sp->thrust_body[2] = -(scale * target_throttle + (1.0f - scale) * _last_thr_in_fw_mode); +} + + +float Tiltrotor::timeUntilMotorsAreUp() +{ + return BACKTRANS_THROTTLE_DOWNRAMP_DUR_S + BACKTRANS_MOTORS_UPTILT_DUR_S; +} + +float Tiltrotor::moveLinear(float start, float stop, float progress) +{ + return start + progress * (stop - start); +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 1525f2b543..1435fb832e 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -59,6 +59,7 @@ public: void update_fw_state() override; void waiting_on_tecs() override; float thrust_compensation_for_tilt(); + void blendThrottleAfterFrontTransition(float scale) override; private: @@ -101,6 +102,12 @@ private: float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ void parameters_update() override; + float timeUntilMotorsAreUp(); + float moveLinear(float start, float stop, float progress); + + void blendThrottleDuringBacktransition(const float scale, const float target_throttle); + + hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ bool _tilt_motors_for_startup{false}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index c06d5f7281..0aa3700a22 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -48,6 +48,8 @@ using namespace matrix; +#define THROTTLE_BLENDING_DUR_S 1.0f + VtolType::VtolType(VtolAttitudeControl *att_controller) : _attc(att_controller), @@ -158,6 +160,7 @@ void VtolType::update_fw_state() } resetAccelToPitchPitchIntegrator(); + _last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; VtolType::set_alternate_motor_state(motor_state::DISABLED); @@ -182,6 +185,17 @@ void VtolType::update_fw_state() && _v_control_mode->flag_control_altitude_enabled) { waiting_on_tecs(); + _throttle_blend_start_ts = hrt_absolute_time(); + + } else if (shouldBlendThrottleAfterFrontTransition()) { + const float progress = (float)(hrt_absolute_time() - _throttle_blend_start_ts) * 1e-6f / THROTTLE_BLENDING_DUR_S; + + if (progress >= 1.0f) { + stopBlendingThrottleAfterFrontTransition(); + + } else { + blendThrottleAfterFrontTransition(progress); + } } check_quadchute_condition(); @@ -193,6 +207,7 @@ void VtolType::update_transition_state() _transition_dt = (float)(t_now - _last_loop_ts) / 1e6f; _transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f); _last_loop_ts = t_now; + _throttle_blend_start_ts = t_now; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index c88cb5bbf3..5123e64dcc 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -188,6 +188,7 @@ public: */ float pusher_assist(); + virtual void blendThrottleAfterFrontTransition(float scale) {}; mode get_mode() {return _vtol_mode;} @@ -195,7 +196,6 @@ public: virtual void parameters_update() = 0; -protected: VtolAttitudeControl *_attc; mode _vtol_mode; @@ -229,6 +229,7 @@ protected: // motors spinning up or cutting too fast when doing transitions. float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) + float _last_thr_in_fw_mode = 0.0f; float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition @@ -278,6 +279,8 @@ protected: private: + hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle + /** * @brief Stores the max pwm values given by the system. */ @@ -316,6 +319,9 @@ private: bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value); void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } + bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; }; + + void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; } };