fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 20:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL standatd - Simplify multicopter weight in update_transition_state()

Browse Source
This commit is contained in:
bresch
2018-01-29 09:32:51 -05:00
committed by Daniel Agar
parent 2bea09a997
commit d34767b871
1 changed files with 16 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+16 -40
View File
@@ -278,6 +278,8 @@ void Standard::update_vtol_state()
void Standard::update_transition_state() void Standard::update_transition_state()
{ {
float mc_weight = 1.0f;
VtolType::update_transition_state(); VtolType::update_transition_state();
// copy virtual attitude setpoint to real attitude setpoint // copy virtual attitude setpoint to real attitude setpoint
@@ -299,40 +301,22 @@ void Standard::update_transition_state()
_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend && _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f) (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
) { ) {
float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin; _airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
// time based blending when no airspeed sensor is set // time based blending when no airspeed sensor is set
} else if (_params_standard.airspeed_disabled && } else if (_params_standard.airspeed_disabled &&
hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) && hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) &&
hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f) hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f)
) { ) {
float weight = 1.0f - ((hrt_elapsed_time(&_vtol_schedule.transition_start) - (( mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
_params_standard.front_trans_time_min / 2.0f) * 1e6f)) / _params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /
((_params_standard.front_trans_time_min / 2.0f) * 1e6f)); ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f));
weight = math::constrain(weight, 0.0f, 1.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
} }
// ramp up FW_PSP_OFF // ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - _mc_pitch_weight); _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); 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); q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true; _v_att_sp->q_d_valid = true;
@@ -367,19 +351,9 @@ void Standard::update_transition_state()
// 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_ramp > FLT_EPSILON) { if (_params_standard.back_trans_ramp > FLT_EPSILON) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
((_params_standard.back_trans_ramp) * 1000000.0f); ((_params_standard.back_trans_ramp) * 1000000.0f);
weight = math::constrain(weight, 0.0f, 1.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
} }
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
@@ -390,10 +364,12 @@ void Standard::update_transition_state()
} }
} }
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); _mc_roll_weight = mc_weight;
_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f); _mc_pitch_weight = mc_weight;
_mc_yaw_weight = mc_weight;
_mc_throttle_weight = mc_weight;
} }
void Standard::update_mc_state() void Standard::update_mc_state()