dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW position controller: Do handle idle mission items correctly

This commit is contained in:
Lorenz Meier 2015-06-12 15:55:55 +02:00
parent 540ffa7861
commit 6c0539c243
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1085,7 +1085,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
@ -1544,6 +1549,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
} else {
/* Copy thrust and pitch values from tecs */
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :