diff options
author | Friedemann Ludwig <frieludwig@hotmail.com> | 2014-11-28 04:11:39 +0100 |
---|---|---|
committer | Friedemann Ludwig <frieludwig@hotmail.com> | 2014-11-28 04:11:39 +0100 |
commit | 8e8b622f4f5c4737e0f7dad33870f23742449fe9 (patch) | |
tree | 9515af2f41256b0d3d72ddf660abead7b27a78f9 /src/modules/fw_att_control | |
parent | 8cc59ca01a2beb74d1069bb603baed47f59ddbc6 (diff) | |
download | px4-firmware-8e8b622f4f5c4737e0f7dad33870f23742449fe9.tar.gz px4-firmware-8e8b622f4f5c4737e0f7dad33870f23742449fe9.tar.bz2 px4-firmware-8e8b622f4f5c4737e0f7dad33870f23742449fe9.zip |
Implemented altitude and velocity hold mode
Diffstat (limited to 'src/modules/fw_att_control')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 23 |
1 files changed, 21 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 e770c11a2..a295d61ac 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -751,8 +751,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; @@ -769,6 +768,26 @@ 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 |