re-added turn coordination gains (TESTING CODE)

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();

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,

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