From f2c973e699a09b4cbd10d5e60e75111c96a3daae Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 8 Dec 2022 11:44:07 +0100
Subject: [PATCH] FW Att C: add sys ID ramps for pitch and throttle

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../FixedwingAttitudeControl.cpp              | 27 +++++++++++++++++--
 .../FixedwingAttitudeControl.hpp              |  9 +++++--
 .../fw_att_control/fw_att_control_params.c    | 13 +++++++++
 3 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index b57df7962bb7..554f353cf7b4 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -85,7 +85,7 @@ FixedwingAttitudeControl::parameters_update()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
+FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float dt)
 {
 	if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 
@@ -106,6 +106,29 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 				_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
 				_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
 
+				if (_param_sysid_ramp_en.get() == 1 && _manual_control_setpoint.aux1 > 0.8f) { // use .yaw if aux1 is unavailabe
+						const float pitch_ramp_gradient = radians(0.8f); // 0.8deg/s ramp gradient
+						_att_sp.pitch_body = _pitch_ramp_last - pitch_ramp_gradient * dt;
+						_att_sp.thrust_body[0] = 0.f; // gliding
+						_pitch_ramp_last = _att_sp.pitch_body;
+
+				} else if (_param_sysid_ramp_en.get() == 2 && _manual_control_setpoint.aux1 > 0.8f) { // use .yaw if aux1 is unavailabe
+						const float pitch_ramp_gradient = radians(0.8f); // 0.8deg/s ramp gradient
+						_att_sp.pitch_body = _pitch_ramp_last + pitch_ramp_gradient * dt;
+						_att_sp.thrust_body[0] = 0.f; // gliding
+						_pitch_ramp_last = _att_sp.pitch_body;
+
+				} else if (_param_sysid_ramp_en.get() == 3 && _manual_control_setpoint.aux1 > 0.8f) {
+						const float throttle_ramp_gradient = 2.f; // %/s throttle gradient
+						_att_sp.thrust_body[0] = _throttle_ramp_last + throttle_ramp_gradient * 0.01f * dt;
+						_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set pitch set to trim pitch
+						_throttle_ramp_last = _att_sp.thrust_body[0];
+
+				} else {
+						_pitch_ramp_last = _att_sp.pitch_body;
+						_throttle_ramp_last = _manual_control_setpoint.throttle;
+				}
+
 				Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 				q.copyTo(_att_sp.q_d);
 
@@ -258,7 +281,7 @@ void FixedwingAttitudeControl::Run()
 
 		const matrix::Eulerf euler_angles(_R);
 
-		vehicle_manual_poll(euler_angles.psi());
+		vehicle_manual_poll(euler_angles.psi(), dt);
 
 		vehicle_attitude_setpoint_poll();
 
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 63bb8cbd9339..9cd764b8f78f 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -130,6 +130,9 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	float _groundspeed{0.f};
 	bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
 
+	float _pitch_ramp_last{0.f};
+	float _throttle_ramp_last{0.f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
@@ -155,7 +158,9 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 		(ParamFloat<px4::params::FW_WR_IMAX>) _param_fw_wr_imax,
 		(ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
 
-		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax
+		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
+
+		(ParamInt<px4::params::FW_SYSID_RAMP_EN>) _param_sysid_ramp_en
 	)
 
 	ECL_RollController _roll_ctrl;
@@ -164,7 +169,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	ECL_WheelController _wheel_ctrl;
 
 	void parameters_update();
-	void vehicle_manual_poll(const float yaw_body);
+	void vehicle_manual_poll(const float yaw_body, const float dt);
 	void vehicle_attitude_setpoint_poll();
 	void vehicle_land_detected_poll();
 	float get_airspeed_constrained();
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 0c22c02e76a4..c4532c29f084 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -256,3 +256,16 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_YR_MAX, 30.f);
+
+/**
+ * Enable SYS ID ramps
+ *
+ * Activated as long as aux1 is at 1.
+ *
+ * @value 0 Disabled
+ * @value 1 Pitch ramp from current pitch down, 0 thrust
+ * @value 2 Pitch ramp from current pitch up, 0 thrust
+ * @value 3 Throttle ramp from 0 up, level flight
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_INT32(FW_SYSID_RAMP_EN, 0);