From d729e976fd779502a00ec2dd2947a6affd140f52 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Wed, 17 Jul 2024 11:39:35 +0800
Subject: [PATCH] Use dbusFeedforward for chassis only when gimbal in rate.

---
 src/chassis_gimbal_manual.cpp         | 2 --
 src/chassis_gimbal_shooter_manual.cpp | 8 ++++++++
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp
index b5427647..d7b4ffab 100644
--- a/src/chassis_gimbal_manual.cpp
+++ b/src/chassis_gimbal_manual.cpp
@@ -47,14 +47,12 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ManualBase::updateRc(dbus_data);
   gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y);
-  chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
 }
 
 void ChassisGimbalManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ManualBase::updatePc(dbus_data);
   gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, dbus_data->m_y * gimbal_scale_);
-  chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
 }
 
 void ChassisGimbalManual::checkReferee()
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
index af95de4d..5ec57be4 100644
--- a/src/chassis_gimbal_shooter_manual.cpp
+++ b/src/chassis_gimbal_shooter_manual.cpp
@@ -251,6 +251,10 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu
 
   if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP)
     gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
+  if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE)
+    chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
+  else
+    chassis_cmd_sender_->setFollowVelDes(0);
 }
 
 void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
@@ -265,6 +269,10 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu
     else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW)
       chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
   }
+  if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE)
+    chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
+  else
+    chassis_cmd_sender_->setFollowVelDes(0);
 }
 
 void ChassisGimbalShooterManual::rightSwitchDownRise()