From 47b5d51369c6413acfe07e79a2eb81d2bc7cb259 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 6 Mar 2020 11:43:06 +0300 Subject: [PATCH] vtol_type: added explicit control over deceleration during backtransition Signed-off-by: RomanBapst --- src/modules/vtol_att_control/standard.cpp | 6 +-- .../vtol_att_control_main.cpp | 8 ++++ .../vtol_att_control/vtol_att_control_main.h | 3 ++ .../vtol_att_control_params.c | 41 +++++++++++++++++++ src/modules/vtol_att_control/vtol_type.cpp | 36 ++++++++++++++++ src/modules/vtol_att_control/vtol_type.h | 10 +++++ 6 files changed, 101 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 1b00205a8b..57244eb9f8 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -280,9 +280,9 @@ void Standard::update_transition_state() } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { - // maintain FW_PSP_OFF - _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + 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); @@ -412,4 +412,4 @@ Standard::waiting_on_tecs() { // keep thrust from transition _v_att_sp->thrust_body[0] = _pusher_throttle; -}; +}; \ No newline at end of file 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 34f54103d0..fa0e9d384c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -85,6 +85,10 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN"); _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); + _params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF"); + _params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I"); + _params_handles.back_trans_dec_sp = param_find("VT_B_DEC_SP"); + _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); @@ -285,6 +289,10 @@ VtolAttitudeControl::parameters_update() // make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed _params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f); + param_get(_params_handles.back_trans_dec_sp, &_params.back_trans_dec_sp); + param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff); + param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i); + // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { _vtol_type->parameters_update(); 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 cb77aa8431..e036761db7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -203,6 +203,9 @@ private: param_t diff_thrust_scale; param_t down_pitch_max; param_t forward_thrust_scale; + param_t dec_to_pitch_ff; + param_t dec_to_pitch_i; + param_t back_trans_dec_sp; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 4e94b7632f..f3776d863b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -315,3 +315,44 @@ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); + +/** + * Target deceleration during backtransition. + * + * The vehicle will use its pitch angle to try and maintain a target deceleration during the backtransition. + * This parameter only applies for standard vtol and tiltrotors. + * + * @unit m/s2 + * @min 0.5 + * @max 5 + * @decimal 1 + * @increment 0.5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_SP, 2.0f); + +/** + * Backtransition deceleration setpoint to pitch feedforward gain. + * + * + * @unit rad*s*s/m + * @min 0 + * @max 0.2 + * @decimal 1 + * @increment 0.05 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.12f); + +/** + * Backtransition deceleration setpoint to pitch I gain. + * + * + * @unit rad*s/m + * @min 0 + * @max 0.3 + * @decimal 1 + * @increment 0.05 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 5b30e470c8..9df115c7e0 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -177,9 +177,45 @@ void VtolType::update_fw_state() void VtolType::update_transition_state() { + hrt_abstime t_now = hrt_absolute_time(); + _transition_dt = (float)(t_now - _last_loop_ts) / 1e6f; + _transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f); + _last_loop_ts = t_now; + + + check_quadchute_condition(); } +float VtolType::update_and_get_backtransition_pitch_sp() +{ + // maximum up or down pitch the controller is allowed to demand + const float pitch_lim = 0.3f; + + // calculate acceleration in body x direction + const Dcmf R_to_body(Quatf(_v_att->q).inversed()); + const Vector3f acc = R_to_body * Vector3f(_local_pos->ax, _local_pos->ay, _local_pos->az); + float accel_body_x = acc(0); + float accel_error_x = 0.0f; + + // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) + accel_error_x = _params->back_trans_dec_sp + accel_body_x; + float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ; + + float integrator_input = _params->dec_to_pitch_i * accel_error_x; + + if ((pitch_sp_new >= pitch_lim && accel_error_x > 0.0f) || + (pitch_sp_new <= -pitch_lim && accel_error_x < 0.0f) + ) { + integrator_input = 0.0f; + } + + _accel_to_pitch_integ += integrator_input * _transition_dt; + + return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim); + +} + bool VtolType::can_transition_on_ground() { return !_v_control_mode->flag_armed || _land_detected->landed; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 047bd2b84d..93eb2b3666 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -72,6 +72,9 @@ struct Params { float diff_thrust_scale; float down_pitch_max; float forward_thrust_scale; + float dec_to_pitch_ff; + float dec_to_pitch_i; + float back_trans_dec_sp; }; // Has to match 1:1 msg/vtol_vehicle_status.msg @@ -222,6 +225,11 @@ protected: motor_state _motor_state = motor_state::DISABLED; + hrt_abstime _last_loop_ts = 0; + float _transition_dt = 0; + + float _accel_to_pitch_integ = 0; + /** @@ -253,6 +261,8 @@ protected: */ motor_state set_motor_state(const motor_state current_state, const motor_state next_state, const int value = 0); + float update_and_get_backtransition_pitch_sp(); + private: