增加了校验码等

CMakeLists.txt

@@ -10,7 +10,8 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
 PROJECT( IVP_EXTEND )
 
 set (CMAKE_CXX_STANDARD 11)
-set (MOOSIVP_SOURCE_TREE_BASE "/home/jima/moos-ivp")
+# Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE
+set (MOOSIVP_SOURCE_TREE_BASE "../moos-ivp")
 
 #=======================================================================
 # Set the output directories for the binary and library files

src/pAUV150/AUV150.cpp

@@ -16,7 +16,65 @@
 // #define DEBUG_
 
 using namespace std;
+// until函数
+/**
+ * @brief 解析int16数值，除以100并根据高位判断正负
+ * @param value 待解析的int16数值
+ * @return 解析后的浮点数值
+ */
+float parseInt16WithSign(int16_t value) {
+    // 检查最高位是否为1(负数)
+    if (value & 0x8000) {
+        // 负数：取补码并除以100后取负
+        return -((~value + 1) & 0x7FFF);
+    }
+    // 正数
+    return value;
+}
+/**
+ * @brief 将double编码为uint8，第一位为符号位
+ * @param value 要编码的double值
+ * @param limit 限幅值，输入将被限制在[-limit, limit]范围内
+ * @return 编码后的uint8值，第一位为符号位(0正1负)，后7位为数值
+ */
+uint8_t encodeDoubleToUint8WithSign(double value, double limit) {
+    // 限幅处理
+    if (value > limit) value = limit;
+    if (value < -limit) value = -limit;
     
+    // 直接取整数值
+    uint8_t magnitude = static_cast<uint8_t>(round(fabs(value)));
+    
+    // 确保不会超过127
+    magnitude = magnitude > 127 ? 127 : magnitude;
+    
+    // 设置符号位
+    if (value < 0) {
+        magnitude |= 0x80; // 设置最高位为1表示负数
+    }
+    
+    return magnitude;
+}
+static uint8_t calculateCRC(const FeedbackFrame_150AUV& frame) {
+    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navModeFb);
+    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.dvlStatus) + sizeof(frame.dvlStatus);
+    
+    uint8_t crc = 0;
+    for (const uint8_t* p = start; p < end; ++p) {
+        crc += *p;
+    }
+    return crc;
+}
+static uint8_t calculateCRC(const CommandFrame_150AUV& frame) {
+    const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navMode);
+    const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.payloadCtrl) + sizeof(frame.payloadCtrl);
+    
+    uint8_t crc = 0;
+    for (const uint8_t* p = start; p < end; ++p) {
+        crc += *p;
+    }
+    return crc;
+}
 // 添加静态成员定义
 Controler AUV150::m_control;
 
@@ -99,7 +157,7 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
         }
         else
         {
-            std::cout << "=============//=========" << std::endl;
+            std::cout << "======================" << std::endl;
             m_overrived == true;
         }
     }
@@ -215,9 +273,6 @@ bool AUV150::OnStartUp()
 void AUV150::registerVariables()
 {
   AppCastingMOOSApp::RegisterVariables();
-//   Register("DESIRED_RUDDER",0);
-//   Register("DESIRED_THRUST",0);
-//   Register("DESIRED_ELEVATOR",0);
   Register("DESIRED_HEADING", 0);
   Register("DESIRED_SPEED", 0);
   Register("DESIRED_DEPTH", 0);
@@ -276,6 +331,12 @@ bool AUV150::ListenLoop()
                         m_faultCodes.insert(0x11);
                         continue;
                     }
+                    if(p->crc != calculateCRC(*p))
+                    {
+                        std::cout << "Received data is not a valid frame" << std::endl;
+                        m_faultCodes.insert(0x14); // 0x14 校验错误
+                        continue;
+                    }
                     // 解析数据
                     m_counter = p->counter;
                     updateStatus(*p);
@@ -296,11 +357,16 @@ bool AUV150::ListenLoop()
                         m_control.rtU.y = m_status.y;
                         m_control.rtU.z = m_status.z;
                         m_control.step();
-                        m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f; 
+                        // m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f; 
-                        m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
+                        // m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
-                        m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
+                        // m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
-                        m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
+                        // m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
-                        m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
+                        // m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
+                        m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(m_control.rtY.DirectUpperRudderServoAngleCmd,30); 
+                        m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(m_control.rtY.DirectLowerRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(m_control.rtY.DirectLeftRudderServoAngleCmd,30);
+                        m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(m_control.rtY.DirectRightRudderServoAngleCmd,30);
+                        m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100);   
                         m_status.depth_error = m_control.rtY.depth_error;
                         m_status.heading_error = m_control.rtY.heading_error;
                         m_status.pitch_error = m_control.rtY.pitch_error;
@@ -309,6 +375,7 @@ bool AUV150::ListenLoop()
             // 发送消息 
             try
             {   
+                m_CommandFrame.crc = calculateCRC(m_CommandFrame);
                 m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
                 m_controlGap =  MOOSTime() - m_desiredVarTime;
                 m_controlTime = MOOSTime();
@@ -566,17 +633,18 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
     m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
     m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
 
-    m_status.pitch = feedbackFrame.pitch / 100.0f;
+    // m_status.pitch = feedbackFrame.pitch / 10.0f;
-    m_status.roll = feedbackFrame.roll / 100.0f;
+    m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
-    m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
+    m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f;
-    m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
+    m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
-    m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
+    m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
-    m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f; 
+    m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
-    m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f;
+    m_status.insLongitude = feedbackFrame.insLongitude / 1e5; 
+    m_status.insLatitude = feedbackFrame.insLatitude / 1e5;
     m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
-    m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
+    m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
-    m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
+    m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;
-    m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f;
+    m_status.dvlVelZ = parseInt16WithSign(feedbackFrame.dvlVelZ) / 100.0f;
     m_status.thrusterRPM = feedbackFrame.thrusterRPM;
     m_status.ledSwitch = feedbackFrame.ledSwitch;
     m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;