diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-09-01 18:13:29 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-09-01 18:13:29 +0200 |
commit | 8ad1aa789b6f10769dd9977151c9a39971b1ebaf (patch) | |
tree | c39083f8bbf16426e1a588626bf35a1c42ebd81d | |
parent | 3c7b9ef94d13d5610fd331849f87bcda7bf0d9c2 (diff) | |
download | px4-firmware-8ad1aa789b6f10769dd9977151c9a39971b1ebaf.tar.gz px4-firmware-8ad1aa789b6f10769dd9977151c9a39971b1ebaf.tar.bz2 px4-firmware-8ad1aa789b6f10769dd9977151c9a39971b1ebaf.zip |
mc_pos_control: reset position setpoint on entering to AUTO mode
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 19 |
1 files changed, 14 insertions, 5 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 847a92151..01fdb4c65 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,7 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; + bool _mode_auto; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -292,7 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_timestamp(0), _reset_pos_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _mode_auto(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -684,6 +686,13 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f void MulticopterPositionControl::control_auto(float dt) { + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -807,9 +816,7 @@ MulticopterPositionControl::control_auto(float dt) } } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); + /* no waypoint, do nothing, setpoint was already reset */ } } @@ -916,10 +923,12 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); + _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); + _mode_auto = false; } else { /* AUTO */ @@ -1271,9 +1280,9 @@ MulticopterPositionControl::task_main() /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; + _mode_auto = false; reset_int_z = true; reset_int_xy = true; - } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ |