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

mc_pos_control: switch to auto only if triplets have been updated

Browse Source
This commit is contained in:
Dennis Mannhart 2017-05-09 11:12:50 +02:00 committed by Lorenz Meier
parent 47faaa5d78
commit 29795fa95f
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -244,6 +244,8 @@ private:
bool _hold_offboard_z = false;
bool _limit_vel_xy = false;
bool _transition_to_non_manual = false;
math::Vector<3> _thrust_int;
math::Vector<3> _pos;
@ -761,6 +763,13 @@ MulticopterPositionControl::poll_subscriptions()
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
/* to avoid time scheduling issue that occurs when the navigator has not updated the triplet
* but the mc_pos_control already received a non-manual control flag
*/
if (_transition_to_non_manual && !_control_mode.flag_control_manual_enabled) {
_transition_to_non_manual = false;
}
}
orb_check(_home_pos_sub, &updated);
@ -1665,9 +1674,15 @@ MulticopterPositionControl::do_control(float dt)
control_manual(dt);
_mode_auto = false;
_transition_to_non_manual = true;
_hold_offboard_xy = false;
_hold_offboard_z = false;
} else if (_transition_to_non_manual) {
/* we reuse the previous setpoints */
calculate_thrust_setpoint(dt);
} else {
control_non_manual(dt);
}
@ -2307,6 +2322,7 @@ MulticopterPositionControl::task_main()
_reset_int_xy = true;
_reset_yaw_sp = true;
_yaw_takeoff = _yaw;
}
was_armed = _control_mode.flag_armed;