From 16515e1d1e8d5424317bf3a43cc87a9f744b5828 Mon Sep 17 00:00:00 2001 From: Sander Smeets Date: Sat, 25 Mar 2017 05:24:59 +0100 Subject: [PATCH] VTOL standard transition use FW_PSP_OFF (#6728) * VTOL back transition maintain FW_PSP_OFF * VTOL Front transition FW_PSP_OFF rampup --- src/modules/vtol_att_control/standard.cpp | 17 +++++++++++++++++ src/modules/vtol_att_control/standard.h | 2 ++ 2 files changed, 19 insertions(+) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index b1b7ce718e..d987b96311 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -71,6 +71,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); _params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF"); } Standard::~Standard() @@ -122,6 +123,9 @@ Standard::parameters_update() param_get(_params_handles_standard.airspeed_mode, &i); _params_standard.airspeed_mode = math::constrain(i, 0, 2); + /* pitch setpoint offset */ + param_get(_params_handles_standard.pitch_setpoint_offset, &v); + _params_standard.pitch_setpoint_offset = math::radians(v); } @@ -288,6 +292,12 @@ void Standard::update_transition_state() _mc_throttle_weight = 1.0f; } + // ramp up FW_PSP_OFF + _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - _mc_pitch_weight); + matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); + _v_att_sp->q_d_valid = true; + // check front transition timeout if (_params_standard.front_trans_timeout > FLT_EPSILON) { if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { @@ -297,6 +307,13 @@ void Standard::update_transition_state() } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + + // maintain FW_PSP_OFF + _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; + matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); + _v_att_sp->q_d_valid = true; + // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index ca605817b1..306618b44b 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -77,6 +77,7 @@ private: float down_pitch_max; float forward_thrust_scale; int airspeed_mode; + float pitch_setpoint_offset; } _params_standard; struct { @@ -90,6 +91,7 @@ private: param_t down_pitch_max; param_t forward_thrust_scale; param_t airspeed_mode; + param_t pitch_setpoint_offset; } _params_handles_standard; enum vtol_mode {