diff options
author | tumbili <bapstr@ethz.ch> | 2014-12-15 22:34:01 +0100 |
---|---|---|
committer | tumbili <bapstr@ethz.ch> | 2014-12-15 22:34:01 +0100 |
commit | 736f57f436d05f3c2f10a8d7c12e54db29ed364c (patch) | |
tree | 574df4584959a240cece053b96c1849a591b073c /src/modules/fw_att_control | |
parent | 51a7fbeee091c20eb52bafade3e2041e26a785cc (diff) | |
parent | aa40c69853be0dc7e79bc3084472b77f9667c1f1 (diff) | |
download | px4-firmware-736f57f436d05f3c2f10a8d7c12e54db29ed364c.tar.gz px4-firmware-736f57f436d05f3c2f10a8d7c12e54db29ed364c.tar.bz2 px4-firmware-736f57f436d05f3c2f10a8d7c12e54db29ed364c.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into vtol_merge
Diffstat (limited to 'src/modules/fw_att_control')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 22 |
1 files changed, 20 insertions, 2 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 956fc94de..8d18ae68c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -831,8 +831,7 @@ FixedwingAttitudeControl::task_main() * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ - if (_vcontrol_mode.flag_control_velocity_enabled || - _vcontrol_mode.flag_control_position_enabled || + if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; @@ -849,6 +848,25 @@ FixedwingAttitudeControl::task_main() if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } + } else if (_vcontrol_mode.flag_control_velocity_enabled) { + /* + * Velocity should be controlled and manual is enabled. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians |