From 46d969772cc672dee7ca9d0b238ed90b4abb6c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:53:34 -0700 Subject: [PATCH] Fixed wing heading hold: Engage only after wings got close to level already --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d685631125..b7194461e6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -858,7 +858,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 18698f798a..4e4a3356f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1411,12 +1411,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { /* heading / roll is zero, lock onto current heading */ - // XXX calculate a waypoint in some distance - // and lock on to it - /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true;