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

allow manual yaw in all manual modes and also use it as threshold to snap into heading lock

This commit is contained in:
Andreas Antener 2016-07-18 23:46:46 +02:00
parent 9f7f6e4d3d
commit 0a997577f5
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -980,7 +980,8 @@ FixedwingAttitudeControl::task_main()
yaw_sp = _att_sp.yaw_body;
throttle_sp = _att_sp.thrust;
if (!_vcontrol_mode.flag_control_velocity_enabled) {
/* allow manual yaw in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
yaw_manual = _manual.r;
}

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

@ -108,7 +108,7 @@ 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.15f // 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/yaw input from user which does not change the locked heading
#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid
#define THROTTLE_THRESH 0.05f ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode
@ -1882,8 +1882,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_status_s::TECS_MODE_NORMAL);
/* heading control */
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabs(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading