From 9681f819a8c23222e946403db0df9e55392ce872 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 29 Sep 2021 11:11:01 +0200
Subject: [PATCH] mc_att: inject identification signal in att control output

---
 src/modules/mc_att_control/mc_att_control.hpp     |  2 ++
 .../mc_att_control/mc_att_control_main.cpp        | 15 ++++++++++++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index c5da9ac6e7..54c425fb0a 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -47,6 +47,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -104,6 +105,7 @@ private:
 
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 3acf95dfc8..7cc0103a91 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -316,13 +316,26 @@ MulticopterAttitudeControl::Run()
 
 			Vector3f rates_sp = _attitude_control.update(q);
 
+			const hrt_abstime now = hrt_absolute_time();
+			autotune_attitude_control_status_s pid_autotune;
+
+			if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
+				if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
+				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
+				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
+				     || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
+				    && ((now - pid_autotune.timestamp) < 1_s)) {
+					rates_sp += Vector3f(pid_autotune.rate_sp);
+				}
+			}
+
 			// publish rate setpoint
 			vehicle_rates_setpoint_s v_rates_sp{};
 			v_rates_sp.roll = rates_sp(0);
 			v_rates_sp.pitch = rates_sp(1);
 			v_rates_sp.yaw = rates_sp(2);
 			_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
-			v_rates_sp.timestamp = hrt_absolute_time();
+			v_rates_sp.timestamp = now;
 
 			_v_rates_sp_pub.publish(v_rates_sp);
 		}