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

make motors spin in POSCTRL and ATTCTRL when landed and throttle applied by user

This commit is contained in:
tumbili 2015-06-19 10:25:40 +02:00
parent 7b588c5bd0
commit 5d92927991
1 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -98,7 +98,8 @@ static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane
#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
@ -1606,8 +1607,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = 0.0f;
} else {
/* Copy thrust and pitch values from tecs */
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
if (_vehicle_status.condition_landed &&
(_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE))
{
// when we are landed in these modes we want the motor to spin
_att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max);
} else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
}
}
/* During a takeoff waypoint while waiting for launch the pitch sp is set