yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

VTOL standard transition use FW_PSP_OFF (#6728)

Browse Source 
* VTOL back transition maintain FW_PSP_OFF

* VTOL Front transition FW_PSP_OFF rampup
This commit is contained in:
Sander Smeets
2017-03-25 05:24:59 +01:00
committed by Daniel Agar
parent 2d92ad2538
commit 16515e1d1e
2 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/vtol_att_control/standard.cpp
View File

@@ -71,6 +71,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _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.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE"); _params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE");
_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
} }
Standard::~Standard() Standard::~Standard()
@@ -122,6 +123,9 @@ Standard::parameters_update()
param_get(_params_handles_standard.airspeed_mode, &i); param_get(_params_handles_standard.airspeed_mode, &i);
_params_standard.airspeed_mode = math::constrain(i, 0, 2); _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; _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 // check front transition timeout
if (_params_standard.front_trans_timeout > FLT_EPSILON) { if (_params_standard.front_trans_timeout > FLT_EPSILON) {
if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { 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) { } 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 // continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) { if (_params_standard.back_trans_dur > 0.0f) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *

2
src/modules/vtol_att_control/standard.h
View File

@@ -77,6 +77,7 @@ private:
float down_pitch_max; float down_pitch_max;
float forward_thrust_scale; float forward_thrust_scale;
int airspeed_mode; int airspeed_mode;
float pitch_setpoint_offset;
} _params_standard; } _params_standard;
struct { struct {
@@ -90,6 +91,7 @@ private:
param_t down_pitch_max; param_t down_pitch_max;
param_t forward_thrust_scale; param_t forward_thrust_scale;
param_t airspeed_mode; param_t airspeed_mode;
param_t pitch_setpoint_offset;
} _params_handles_standard; } _params_handles_standard;
enum vtol_mode { enum vtol_mode {