fw_att_control: update manual_control_setpoint usage

1 files changed, 14 insertions, 3 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 f139c25f4..4464b5e01 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -173,6 +173,8 @@ private:
 		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
 		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
 		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float roll_max;						/**< Max Roll in rad */
+		float pitch_max;					/**< Max Pitch in rad */
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -211,6 +213,8 @@ private:
 		param_t trim_yaw;
 		param_t rollsp_offset_deg;
 		param_t pitchsp_offset_deg;
+		param_t roll_max;
+		param_t pitch_max;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
 	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
 
+	_parameter_handles.roll_max = param_find("FW_R_MAX");
+	_parameter_handles.pitch_max = param_find("FW_P_MAX");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
 	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
 	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.roll_max, &(_parameters.roll_max));
+	param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max));
+	_parameters.roll_max = math::radians(_parameters.roll_max);
+	_parameters.pitch_max = math::radians(_parameters.pitch_max);
 
 
 	/* pitch control parameters */
@@ -700,8 +711,8 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
-					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
+					pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
@@ -809,7 +820,7 @@ FixedwingAttitudeControl::task_main()
 			} else {
 				/* manual/direct control */
 				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = _manual.pitch;
+				_actuators.control[1] = -_manual.pitch;
 				_actuators.control[2] = _manual.yaw;
 				_actuators.control[3] = _manual.throttle;
 				_actuators.control[4] = _manual.flaps;