From 1b514be1e5febd6ca66dc644eefea66081f673b4 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Wed, 29 Sep 2021 23:43:20 +0200
Subject: [PATCH] Add yaw time constant

---
 .../FixedwingAttitudeControl.cpp               |  1 +
 .../FixedwingAttitudeControl.hpp               |  1 +
 .../fw_att_control/fw_att_control_params.c     | 18 ++++++++++++++++++
 3 files changed, 20 insertions(+)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 4d26c09d01..bea8f04c2a 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -100,6 +100,7 @@ FixedwingAttitudeControl::parameters_update()
 	_roll_ctrl.set_integrator_max(_param_fw_rr_imax.get());
 
 	/* yaw control parameters */
+	_yaw_ctrl.set_time_constant(_param_fw_y_tc.get());
 	_yaw_ctrl.set_k_p(_param_fw_yr_p.get());
 	_yaw_ctrl.set_k_i(_param_fw_yr_i.get());
 	_yaw_ctrl.set_k_ff(_param_fw_yr_ff.get());
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 5adb4088e8..f6d593cdee 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -200,6 +200,7 @@ private:
 		(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_TC>) _param_fw_y_tc,
 		(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
 		(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
 		(ParamFloat<px4::params::FW_YR_IMAX>) _param_fw_yr_imax,
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 4bc2ad6bf8..9df5d0bb00 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -272,6 +272,24 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
  */
 PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f);
 
+/**
+ * Attitude yaw time constant
+ *
+ * This defines the latency between a yaw step input and the achieved setpoint
+ * (inverse to a P gain). Half a second is a good start value and fits for
+ * most average systems. Smaller systems may require smaller values, but as
+ * this will wear out servos faster, the value should only be decreased as
+ * needed.
+ *
+ * @unit s
+ * @min 0.2
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_Y_TC, 0.4f);
+
 /**
  * Roll control to yaw control feedforward gain.
  *