fw pitch sp and roll sp offset parameter

2 files changed, 25 insertions, 6 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 b040fb5df..ab6c1415a 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -170,6 +170,8 @@ private:
 		float trim_roll;
 		float trim_pitch;
 		float trim_yaw;
+		float rollsp_offset;
+		float pitchsp_offset;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -206,6 +208,8 @@ private:
 		param_t trim_roll;
 		param_t trim_pitch;
 		param_t trim_yaw;
+		param_t rollsp_offset;
+		param_t pitchsp_offset;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -347,6 +351,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
 	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
 	_parameter_handles.trim_yaw = param_find("TRIM_YAW");
+	_parameter_handles.rollsp_offset = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -411,6 +417,9 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
 	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
 	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
+	param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset));
+	param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset));
+
 
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
@@ -665,13 +674,13 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp = 0.0f;
-				float pitch_sp = 0.0f;
+				float roll_sp = _parameters.rollsp_offset;
+				float pitch_sp = _parameters.pitchsp_offset;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
-					roll_sp = _att_sp.roll_body;
-					pitch_sp = _att_sp.pitch_body;
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -692,8 +701,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;
-					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f;
+					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset;
+					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 1c615094c..c80a44f2a 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 // @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
 // @Range		0.0 to 30
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+
+// @DisplayName		Roll Setpoint Offset
+// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
+
+// @DisplayName		Pitch Setpoint Offset
+// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);