From 6cd17d4a84fec118ce1a09d8c861b63807d0a4a3 Mon Sep 17 00:00:00 2001
From: cxh <zxwl@56.com>
Date: Fri, 30 May 2025 13:58:15 +0800
Subject: [PATCH] Auto commit at 2025-05-30 13:58:15

---
 src/mc/src/mc.cpp                   |  8 +++++---
 src/mqtt_report/src/mqtt_report.cpp | 24 ++++++++++++++++++++++--
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp
index a259d5a..d3c3668 100644
--- a/src/mc/src/mc.cpp
+++ b/src/mc/src/mc.cpp
@@ -341,11 +341,13 @@ int main(int argc, char **argv)
   // 添加定时器设置
   setupTimers(node);
 
-  // ROS 2 spin
+  // 在 ROS shutdown 时自动清理
+  rclcpp::on_shutdown([&]()
+                      { canctl.close(); });
+
+  // 事件循环
   rclcpp::spin(node);
 
-  // 关闭 CAN 接口
-  canctl.close();
   rclcpp::shutdown(); // 关闭 ROS 2
   return 0;
 }
diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp
index f5ad316..9610bda 100644
--- a/src/mqtt_report/src/mqtt_report.cpp
+++ b/src/mqtt_report/src/mqtt_report.cpp
@@ -10,6 +10,26 @@ Config config; // 清扫车配置文件
 
 GeneralMsg info_report; // 常规消息上报
 
+std::deque<float> waterLevelHistory;
+const size_t maxSamples = 10;
+
+void updateWaterLevel(float newLevel)
+{
+    waterLevelHistory.push_back(newLevel);
+    if (waterLevelHistory.size() > maxSamples)
+    {
+        waterLevelHistory.pop_front();
+    }
+
+    float sum = 0;
+    for (float v : waterLevelHistory)
+        sum += v;
+    float filteredLevel = sum / waterLevelHistory.size();
+
+    filteredLevel = std::clamp(filteredLevel, 0.0f, 180.0f);
+    info_report.waterLevel = static_cast<int>(std::round(filteredLevel / 1.8f));
+}
+
 // 解析can报文，做消息上报
 void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
 {
@@ -25,7 +45,7 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
     case 0x1A2:
     {
         const uint8_t gearByte = msg->data[0];
-        info_report.gear = (gearByte & 0x01) ? 3 : ((gearByte >> 1) & 0x03);
+        info_report.gear = (gearByte & 0x01) ? 3 : ((gearByte >> 2) & 0x03);
 
         const int tempRaw = (msg->data[5] << 8) | msg->data[6];
         info_report.motorTemp = static_cast<int>(tempRaw * 0.1f - 100.0f);
@@ -51,7 +71,7 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
         uint16_t WaterValue = (WaterValue_h & 0x3f) * 16 + (WaterValue_l & 0x0f);
         float WaterLevel = WaterValue * (-0.235) + 199.75;
         WaterLevel = std::clamp(WaterLevel, 0.0f, 180.0f);
-        info_report.waterLevel = static_cast<int>(std::round(WaterLevel / 1.8f)); // 液位百分比
+        updateWaterLevel(WaterLevel);
         break;
     }