From 5ba7c616768da899c9f29882e2cfb62e845729ed Mon Sep 17 00:00:00 2001
From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com>
Date: Mon, 27 Nov 2023 12:55:37 +0900
Subject: [PATCH] [Spinal] fix a bug happened in cherry-pick

---
 .../flight_control/attitude/attitude_control.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

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 99337e1ce..1abcca274 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
@@ -327,9 +327,19 @@ void AttitudeController::update(void)
       {
         for (int axis = 0; axis < 3; axis++)
         {
-          p_term = error_angle[axis] * thrust_p_gain_[i][axis];
-          i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis];
-          d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis];
+          if (is_attitude_ctrl_)
+          {
+            p_term = error_angle[axis] * thrust_p_gain_[i][axis];
+            i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis];
+            d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis];
+          }
+          else
+          {
+            p_term = 0;
+            i_term = 0;
+            d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis];
+          }
+
           if (axis == X)
           {
             roll_pitch_term_[i] = p_term + i_term + d_term;  // [N]