fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 2017-09-05 12:09:49 +02:00 committed by Daniel Agar
parent 2bea09a997
commit d34767b871
1 changed files with 16 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -278,6 +278,8 @@ void Standard::update_vtol_state()
void Standard::update_transition_state()
{
float mc_weight = 1.0f;
VtolType::update_transition_state();
// 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 &&
(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) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
_mc_throttle_weight = weight;
mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} 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 / 2.0f) * 1e6f)
) {
float weight = 1.0f - ((hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
_params_standard.front_trans_time_min / 2.0f) * 1e6f)) /
((_params_standard.front_trans_time_min / 2.0f) * 1e6f));
mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /
((_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
_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));
q_sp.copyTo(_v_att_sp->q_d);
_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
if (_params_standard.back_trans_ramp > FLT_EPSILON) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
((_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;
mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
((_params_standard.back_trans_ramp) * 1000000.0f);
} 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
@ -390,10 +364,12 @@ void Standard::update_transition_state()
}
}
_mc_roll_weight = math::constrain(_mc_roll_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_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
_mc_roll_weight = mc_weight;
_mc_pitch_weight = mc_weight;
_mc_yaw_weight = mc_weight;
_mc_throttle_weight = mc_weight;
}
void Standard::update_mc_state()