Merge pull request #106 from liyixin135/follow-feedforward

Follow feedforward
rm-controls · Jul 18, 2024 · de20f92 · de20f92
2 parents b62ee1e + f642aed
commit de20f92
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp
@@ -47,11 +47,19 @@ 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);
+  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 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_);
+  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 ChassisGimbalManual::checkReferee()