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

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

@ -99,6 +99,7 @@ static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #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_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 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 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,10 +1607,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = 0.0f; _att_sp.thrust = 0.0f;
} else { } else {
/* Copy thrust and pitch values from tecs */ /* Copy thrust and pitch values from tecs */
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() : _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max); _tecs.get_throttle_demand(), throttle_max);
} }
}
/* During a takeoff waypoint while waiting for launch the pitch sp is set /* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */ * already (not by tecs) */
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&