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: make sure that passing waypoint only allowed if altitude is reached

Browse Source
This commit is contained in:
Dennis Mannhart 2017-06-12 14:10:58 +02:00 committed by Lorenz Meier
parent b769d1cee0
commit 9ec0d72f3c
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1725,7 +1725,11 @@ void MulticopterPositionControl::control_auto(float dt)
} else if (close_to_current) {
/* slow down when close to current setpoint */
if (next_setpoint_valid && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
/* check if altidue is within acceptance radius */
bool reached_altitude = (dist_to_current_z < _nav_rad.get()) ? true : false;
if (reached_altitude && next_setpoint_valid
&& !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
/* since we have a next setpoint use the angle prev-current-next to compute velocity setpoint limit */
/* get velocity close to current that depends on angle between prev-current and current-next line */