Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Standard VTOL: add airspeed to back transition logic and refactor it a bit

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-08-26 12:08:27 +02:00
parent 358c67226e
commit a66b0829b0
1 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+12 -7
View File
@@ -141,16 +141,21 @@ void Standard::update_vtol_state()
_reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;
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);
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;
float x_vel = vel(0);
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
}
if (time_since_trans_start > _params->back_trans_duration ||
(_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise) ||
can_transition_on_ground()) {
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
}
}