diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 73fa1037ab..492a9b8453 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -41,15 +41,15 @@ #include "tiltrotor.h" #include "vtol_att_control_main.h" -#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition -#define DELTA 0.5f // the time it should take to tilt the rotors forward completly after the airspeed - // for transitioning into fixed wing mode has been reached Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc), _rear_motors(ENABLED), -_tilt_control(0.0f) +_tilt_control(0.0f), +_roll_weight_mc(1.0f), +_yaw_weight_mc(1.0f), +_min_front_trans_dur(0.5f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -64,7 +64,9 @@ _tilt_control(0.0f) _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); } Tiltrotor::~Tiltrotor() @@ -102,15 +104,23 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.airspeed_trans, &v); _params_tiltrotor.airspeed_trans = v; + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); + _params_tiltrotor.airspeed_blend_start = v; + /* vtol lock elevons in multicopter */ param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); _params_tiltrotor.elevons_mc_lock = l; - /* avoid parameters which will lead to zero division in the transition code */ - _params.front_trans_dur = math::constrain(_params.front_trans_dur, 0.5f, 10.0f); + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; - if (fabsf(_params.airspeed_trans - ARSP_BLEND_START) < 1.0f ) { - _params.airspeed_trans = ARSP_BLEND_START + 1.0f; + /* avoid parameters which will lead to zero division in the transition code */ + _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + + if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; } return OK; @@ -253,8 +263,8 @@ void Tiltrotor::update_transition_state() } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { - _mc_roll_weight = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { + _mc_roll_weight = 1.0f - (_airspeed->true_airspeed_m_s - _params_tiltrotor.airspeed_blend_start) / (_params_tiltrotor.airspeed_trans - _params_tiltrotor.airspeed_blend_start); } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -269,7 +279,7 @@ void Tiltrotor::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(DELTA*1000000.0f); + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2*1000000.0f); _mc_roll_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_rear_motors != IDLE) { diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 5473f8888f..e54e8cfc72 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -86,7 +86,9 @@ private: float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ float tilt_fw; /**< actuator value corresponding to fw tilt */ float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ int elevons_mc_lock; /**< lock elevons in multicopter mode */ + float front_trans_dur_p2; } _params_tiltrotor; struct { @@ -96,7 +98,9 @@ private: param_t tilt_transition; param_t tilt_fw; param_t airspeed_trans; + param_t airspeed_blend_start; param_t elevons_mc_lock; + param_t front_trans_dur_p2; } _params_handles_tiltrotor; enum vtol_mode { @@ -127,6 +131,8 @@ private: float _roll_weight_mc; /**< multicopter desired roll moment weight */ float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ + const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + /** * Write control values to actuator output topics. */ diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d4e925f072..7a87fbd8f1 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -72,3 +72,15 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);