[Spinal] fix: add is_attitude_ctrl_ support for yaw term

jsk-ros-pkg · Nov 27, 2023 · 66cbb6f · 66cbb6f
1 parent 5ba7c61
commit 66cbb6f
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/...l_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/...l_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;
           }
         }