diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 797c72f52a..429ecceedb 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -43,6 +43,7 @@ #include "vtol_att_control_main.h" 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 @@ -61,6 +62,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); + _params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); } @@ -81,6 +83,10 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.tilt_fw, &v); _params_tiltrotor.tilt_fw = v; + /* vtol tilt mechanism position during motor spinup */ + param_get(_params_handles_tiltrotor.tilt_spinup, &v); + _params_tiltrotor.tilt_spinup = v; + /* vtol front transition phase 2 duration */ param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); _params_tiltrotor.front_trans_dur_p2 = v; @@ -209,9 +215,46 @@ void Tiltrotor::update_mc_state() { VtolType::update_mc_state(); - _tilt_control = VtolType::pusher_assist(); + /*Motor spin up: define the first second after arming as motor spin up time, during which + * the tilt is set to the value of VT_TILT_SPINUP. This allowes the user to set a spin up + * tilt angle in case the propellers don't spin up smootly in full upright (MC mode) position. + */ + + const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt) + const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt) + + // reset this timestamp while disarmed + if (!_v_control_mode->flag_armed) { + _last_timestamp_disarmed = hrt_absolute_time(); + _tilt_motors_for_startup = true; + + } else if (_tilt_motors_for_startup) { + // leave motors tilted forward after arming to allow them to spin up easier + if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) { + _tilt_motors_for_startup = false; + } + } + + if (_tilt_motors_for_startup) { + if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) { + _tilt_control = _params_tiltrotor.tilt_spinup; + + } else { + // duration phase 2: begin to adapt tilt to multicopter tilt + float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup); + _tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() - + (_last_timestamp_disarmed + spin_up_duration_p1)); + } + + _mc_yaw_weight = 0.0f; //disable yaw control during spinup + + } else { + // normal operation + _tilt_control = VtolType::pusher_assist(); + _mc_yaw_weight = 1.0f; + _v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt(); + } - _v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt(); } void Tiltrotor::update_fw_state() diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index e102edb35d..1525f2b543 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -63,9 +63,10 @@ public: private: struct { - float tilt_mc; /**< actuator value corresponding to mc tilt */ + float tilt_mc; /**< actuator value corresponding to mc tilt */ float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ - float tilt_fw; /**< actuator value corresponding to fw tilt */ + float tilt_fw; /**< actuator value corresponding to fw tilt */ + float tilt_spinup; /**< actuator value corresponding to spinup tilt */ float front_trans_dur_p2; } _params_tiltrotor; @@ -73,6 +74,7 @@ private: param_t tilt_mc; param_t tilt_transition; param_t tilt_fw; + param_t tilt_spinup; param_t front_trans_dur_p2; } _params_handles_tiltrotor; @@ -99,6 +101,8 @@ private: float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ void parameters_update() override; + hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ + bool _tilt_motors_for_startup{false}; }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index bf57b9687e..31a6d549c0 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -71,6 +71,20 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); +/** + * Tilt actuator control value commanded when disarmed and during the first second after arming. + * + * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't + * spin-up freely. + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); + /** * Duration of front transition phase 2 *