From 629002738b67c95fb1d39f2e79eea2b7999182ae Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 23 May 2015 12:29:08 +0200 Subject: [PATCH] define new mode for altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 d5360df6f0..10d1441f64 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 @@ -209,6 +209,7 @@ private: enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. @@ -1395,7 +1396,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; } @@ -1407,7 +1408,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - _control_mode_current = FW_POSCTRL_MODE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min +