diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-16 10:23:41 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-16 10:23:41 +0100 |
| commit | 851415e48ea428f16a48ac558f6cb6b9aef0c171 (patch) | |
| tree | b922228a56b78744cb1743cc12a9e60b73042826 /src/modules/fw_att_control | |
| parent | 71f6a34367794a887704e2898f8a10101bacfb12 (diff) | |
| parent | aa40c69853be0dc7e79bc3084472b77f9667c1f1 (diff) | |
| download | px4-firmware-851415e48ea428f16a48ac558f6cb6b9aef0c171.tar.gz px4-firmware-851415e48ea428f16a48ac558f6cb6b9aef0c171.tar.bz2 px4-firmware-851415e48ea428f16a48ac558f6cb6b9aef0c171.zip | |
Merge commit 'aa40c69853be0dc7e79bc3084472b77f9667c1f1' into dev_ros_mcatt
Conflicts:
makefiles/config_px4fmu-v2_test.mk
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 57c1e72f3..6bb9bdee3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -752,8 +752,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; @@ -770,6 +769,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 |