diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index cd250d1c..71ac115c 100644 --- 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()