Merge branch 'humble' of http://36.153.162.171:3000/lyq/sweeper_200 into humble

合并

src/control/can_radio_ctrl/src/can_radio_ctrl.cpp

@@ -74,37 +74,36 @@ class CanRadioCtrlNode : public rclcpp::Node
 
         if (has_received && enabled)
         {
-            // 解析挡位
-            switch (radio_data.gear)  // 通道E
-            {
-                case 0:
-                    msg.gear = 1;
-                    break;  // R挡
-                case 1:
-                    msg.gear = 0;
-                    break;  // N挡
-                case 2:
-                    msg.gear = 2;
-                    break;  // D挡
-                default:
-                    msg.gear = 0;
-                    LOG_WARN("Unknown gear: %d", radio_data.gear);
-                    break;
-            }
-
-            // 解析油门/刹车
-            if (radio_data.throttle <= 0)
-            {
-                msg.brake = 1;
-                msg.rpm = 0;
-            }
-            else
+            if (radio_data.throttle > 0)
             {
+                // 油门 > 0 → 前进挡 D
+                msg.gear = 2;
                 msg.brake = 0;
+
+                // 计算前进转速
                 float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f;
                 int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
                 msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
             }
+            else if (radio_data.throttle < 0)
+            {
+                // 油门 < 0 → 后退挡 R
+                msg.gear = 1;
+                msg.brake = 0;
+
+                // 取绝对值计算后退转速（后退速度和前进一致）
+                float throttle_abs = fabs(radio_data.throttle);
+                float throttle_ratio = static_cast<float>(throttle_abs) / 450.0f;
+                int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
+                msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
+            }
+            else
+            {
+                // 油门 = 0 → 空挡 + 刹车
+                msg.gear = 0;
+                msg.brake = 1;
+                msg.rpm = 0;
+            }
 
             // 解析清扫开关
             msg.sweep = (radio_data.sweep != 0);  // 通道B
@@ -136,15 +135,9 @@ class CanRadioCtrlNode : public rclcpp::Node
                 const char* gear_str = "未知挡位";
                 switch (msg.gear)
                 {
-                    case 0:
-                        gear_str = "空挡";
-                        break;
-                    case 1:
-                        gear_str = "后退挡";
-                        break;
-                    case 2:
-                        gear_str = "前进挡";
-                        break;
+                    case 0: gear_str = "空挡"; break;
+                    case 1: gear_str = "后退挡"; break;
+                    case 2: gear_str = "前进挡"; break;
                 }
                 const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
 
@@ -157,23 +150,19 @@ class CanRadioCtrlNode : public rclcpp::Node
                     "  清扫状态: %s\n"
                     "=====================================",
                     brake_str, gear_str, static_cast<int>(msg.rpm), msg.angle, sweep_str);
-                print_counter = 0;  // 重置计数器
+                print_counter = 0;
             }
         }
         else
         {
-            // 未接收到数据或数据不安全
+            // 未接收到数据或数据不安全（不变）
             int interval = PRINT_INTERVAL / 2;
             if (++print_counter >= interval)
             {
                 if (!has_received)
-                {
                     LOG_INFO("Waiting for CAN radio control data...");
-                }
                 else
-                {
                     LOG_WARN("CAN radio control unabled!");
-                }
                 print_counter = 0;
             }
             return;