Implemented altitude and velocity hold mode

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