From 66cbb6fe5093c18991a5e70e37c6cf1cbc635048 Mon Sep 17 00:00:00 2001
From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com>
Date: Mon, 27 Nov 2023 13:37:19 +0900
Subject: [PATCH] [Spinal] fix: add is_attitude_ctrl_ support for yaw term

---
 .../Jsk_Lib/flight_control/attitude/attitude_control.cpp    | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp
index 1abcca274..b73153cd5 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp
@@ -356,7 +356,11 @@ void AttitudeController::update(void)
           }
           if (axis == Z)
           {
-            yaw_term_[i] = extra_yaw_pi_term_[i] + d_term;
+            if (is_attitude_ctrl_)
+              yaw_term_[i] = extra_yaw_pi_term_[i] + d_term;
+            else
+              yaw_term_[i] = d_term;
+
             control_term_msg_.motors[i].yaw_d = d_term * 1000;  // d_term;
           }
         }