Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

VTOL back transition: check only forward velocity

Browse Source
This commit is contained in:
sanderux
2017-09-10 17:48:29 +02:00
committed by Sander Smeets
parent a398dc09c7
commit 1d2a08bd2d
1 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+7 -4
View File
@@ -206,13 +206,16 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed
// XXX: base this on XY hold velocity of MC
float vel = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy);
// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
float x_vel = vel(0);
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f) ||
(_local_pos->v_xy_valid && vel <= _params_standard.mpc_xy_cruise)) {
(_local_pos->v_xy_valid && x_vel <= _params_standard.mpc_xy_cruise)) {
_vtol_schedule.flight_mode = MC_MODE;
}