Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 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
+10 -5
View File
@@ -141,16 +141,21 @@ void Standard::update_vtol_state()
_reverse_output = 0.0f; _reverse_output = 0.0f;
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { } 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;
if (_local_pos->v_xy_valid) {
const Dcmf R_to_body(Quatf(_v_att->q).inversed()); 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); 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 || const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
(_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise) ||
can_transition_on_ground()) { if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE; _vtol_schedule.flight_mode = vtol_mode::MC_MODE;
} }
} }