From c287c56f8d5f0a51c71f8041ab19610ad63d6722 Mon Sep 17 00:00:00 2001
From: Marvin Harms <harmsm@ethz.ch>
Date: Wed, 29 Jun 2022 17:42:47 +0200
Subject: [PATCH] re-added turn coordination gains (TESTING CODE)

---
 .../FixedwingPositionINDIControl.cpp          |  4 ++--
 .../FixedwingPositionINDIControl.hpp          |  2 ++
 .../fw_dyn_soar_control_params.c              | 23 +++++++++++++++++++
 3 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 7e6fdcb8c9..1fa925f3e6 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -119,10 +119,10 @@ FixedwingPositionINDIControl::parameters_update()
     _K_a(2,2) = _param_lin_ff_z.get();
     _K_q(0,0) = _param_rot_k_roll.get();
     _K_q(1,1) = _param_rot_k_pitch.get();
-    _K_q(2,2) = 0.f; // rudder is controlled via turn coordination, not INDI
+    _K_q(2,2) = _param_rot_k_yaw.get(); // rudder is controlled via turn coordination, not INDI
     _K_w(0,0) = _param_rot_c_roll.get()*2.f*sqrtf(_param_rot_k_roll.get());
     _K_w(1,1) = _param_rot_c_pitch.get()*2.f*sqrtf(_param_rot_k_pitch.get());
-    _K_w(2,2) = 0.f; // rudder is controlled via turn coordination, not INDI
+    _K_w(2,2) = _param_rot_c_yaw.get()*2.f*sqrtf(_param_rot_k_yaw.get()); // rudder is controlled via turn coordination, not INDI
 
     // aircraft parameters
     _inertia(0,0) = _param_fw_inertia_roll.get();
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
index f8d48fad9a..78cc3d1f56 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
@@ -190,8 +190,10 @@ private:
 		// attitude PD control params
 		(ParamFloat<px4::params::DS_ROT_K_ROLL>) _param_rot_k_roll,
 		(ParamFloat<px4::params::DS_ROT_K_PITCH>) _param_rot_k_pitch,
+		(ParamFloat<px4::params::DS_ROT_K_YAW>) _param_rot_k_yaw,
 		(ParamFloat<px4::params::DS_ROT_C_ROLL>) _param_rot_c_roll,
 		(ParamFloat<px4::params::DS_ROT_C_PITCH>) _param_rot_c_pitch,
+		(ParamFloat<px4::params::DS_ROT_C_YAW>) _param_rot_c_yaw,
 		// low-level controller params (INDI)
 		(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
 		(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
index 4bfa9ed28c..055abc014f 100644
--- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
@@ -329,6 +329,17 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f);
  */
 PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 10.0f);
 
+/**
+ * rudder turn coordination P-gain
+ * 
+ * @unit 
+ * @min 0
+ * @max 20
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_K_YAW, 0.5f);
 
 /**
  * normalized damping coefficient of attitude PD-controller (body roll-direction)
@@ -354,6 +365,18 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 0.75f);
  */
 PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 0.75f);
 
+/**
+ * normalized damping coefficient of rudder (turn coordination)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_C_YAW, 0.75f);
+
 
 // =============================
 // low level INDI control params