fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 20:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tiltrotor: backtransition logic improvements

Browse Source 
-use groundspeed in body x for exit condition
-use airspeed for speed exit condition if no valid groundspeed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-08-12 10:17:42 +02:00
parent cba80a6338
commit 358c67226e
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -135,12 +135,24 @@ void Tiltrotor::update_vtol_state()
break;
case vtol_mode::TRANSITION_BACK:
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const float ground_speed = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy);
const bool ground_speed_below_cruise = _local_pos->v_xy_valid && (ground_speed <= _params->mpc_xy_cruise);
const bool exit_backtransition_tilt_condition = _tilt_control <= (_params_tiltrotor.tilt_mc + 0.01f);
if (_tilt_control <= _params_tiltrotor.tilt_mc && (time_since_trans_start > _params->back_trans_duration
|| ground_speed_below_cruise)) {
// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;
if (_local_pos->v_xy_valid) {
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
}
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
}
@ -509,7 +521,7 @@ void Tiltrotor::blendThrottleAfterFrontTransition(float scale)
void Tiltrotor::blendThrottleDuringBacktransition(float scale, float target_throttle)
{
_thrust_transition = -(scale * target_throttle + (1.0f - scale) * _last_thr_in_fw_mode);
_thrust_transition = scale * target_throttle + (1.0f - scale) * _last_thr_in_fw_mode;
}