diff --git a/CMakeLists.txt b/CMakeLists.txt index 68ce141..915ae68 100644 --- a/CMakeLists.txt +++ b/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 @@ -102,4 +103,4 @@ ENDIF( ${WIN32} ) #======================================================================= # Add Subdirectories #======================================================================= -ADD_SUBDIRECTORY( src ) +ADD_SUBDIRECTORY( src ) \ No newline at end of file diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index adcdb1e..0386f98 100644 --- a/src/pAUV150/AUV150.cpp +++ b/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(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(&frame.navModeFb); + const uint8_t* end = reinterpret_cast(&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(&frame.navMode); + const uint8_t* end = reinterpret_cast(&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.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f; - m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f; - m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f; - m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd; + // m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f; + // m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f; + // m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f; + // m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f; + // 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; @@ -308,7 +374,8 @@ 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.roll = feedbackFrame.roll / 100.0f; - m_status.velocityEast = feedbackFrame.velocityEast / 100.0f; - m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f; - m_status.velocityDown = feedbackFrame.velocityDown / 100.0f; - m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f; - m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f; + // m_status.pitch = feedbackFrame.pitch / 10.0f; + m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f; + m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f; + m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f; + m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f; + m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.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.dvlVelY = feedbackFrame.dvlVelY / 100.0f; - m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f; + m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f; + m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 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;