diff --git a/CMakeLists.txt b/CMakeLists.txt index a1e260a..1236d93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ PROJECT( IVP_EXTEND ) set (CMAKE_CXX_STANDARD 11) # Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE -set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp") +# set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp") #======================================================================= # Set the output directories for the binary and library files diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ce4bb65..1fe6556 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -19,7 +19,7 @@ ADD_SUBDIRECTORY(lib_behaviors-test) ADD_SUBDIRECTORY(pExampleApp) ADD_SUBDIRECTORY(pXRelayTest) add_subdirectory(pAUV150) -add_subdirectory(pMotionControler) +# add_subdirectory(pMotionControler) ############################################################################## # END of CMakeLists.txt ############################################################################## diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index 7b17c1a..b5b884e 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -30,6 +30,32 @@ float parseInt16WithSign(uint16_t value) { // 正数 return value; } + +/** + * @brief 解析大端序的度+分.分格式坐标 (无符号版本) + * @param rawValue 原始读取的 uint32_t 数据 (已经过 ntohl 转换,即主机字节序) + * @return double 十进制度数 + */ +double parseInsCoordinate(uint32_t rawValue) { + // rawValue 已经通过 ntohl 从大端序转换为主机字节序 + // 所以不需要再进行字节交换 + + // --- 步骤 2: 分离度和分 --- + const uint32_t SCALE_DEG = 1000000; // 10^6 + const double SCALE_MIN = 10000.0; // 10^4 + + uint32_t degrees = rawValue / SCALE_DEG; // 获取整数度 + uint32_t minRaw = rawValue % SCALE_DEG; // 获取分的整数部分 (MM.MMMM) + + // --- 步骤 3 & 4: 计算最终度数 --- + // 1. 还原分为浮点数 (例如 520630 -> 52.0630) + double actualMinutes = static_cast(minRaw) / SCALE_MIN; + + // 2. 分转度并累加 + double result = static_cast(degrees) + (actualMinutes / 60.0); + + return result; +} /** * @brief 将double编码为uint8,第一位为符号位 * @param value 要编码的double值 @@ -739,8 +765,8 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame) 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 / 1e6; - m_status.insLatitude = feedbackFrame.insLatitude / 1e6; + m_status.insLongitude = parseInsCoordinate(feedbackFrame.insLongitude); + m_status.insLatitude = parseInsCoordinate(feedbackFrame.insLatitude); m_status.insAltitude = feedbackFrame.insAltitude / 100.0f; m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f; m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f; diff --git a/trying123.txt b/trying123.txt deleted file mode 100644 index 53129c4..0000000 --- a/trying123.txt +++ /dev/null @@ -1,2 +0,0 @@ - -hey there