From ba1b01bccc438af0937144c1098f7552c6fafcfd Mon Sep 17 00:00:00 2001
From: tangchao0503 <735056338@qq.com>
Date: Fri, 18 Aug 2023 16:01:22 +0800
Subject: [PATCH] =?UTF-8?q?fix=20bug=EF=BC=9A=E8=A7=A3=E6=9E=90sbg?=
 =?UTF-8?q?=E7=9A=84sulution=20mode=EF=BC=9B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Source_Files/sbgrecorder.cpp | 79 ++++++++++++++----------------------
 1 file changed, 30 insertions(+), 49 deletions(-)

diff --git a/Source_Files/sbgrecorder.cpp b/Source_Files/sbgrecorder.cpp
index 4d195f9..0a563f8 100644
--- a/Source_Files/sbgrecorder.cpp
+++ b/Source_Files/sbgrecorder.cpp
@@ -608,66 +608,43 @@ void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
     //判断模式是否为: NAV_POSITION
     if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
     {
-        m_iSolutionModeCounter++;//
+        m_iSolutionModeCounter++;
 
         uint32_t status=logData.ekfEulerData.status;
-        uint32_t mode=status>>24;//?????????????????????????????????????
-//        uint32_t mode=status;//这是错的
+        uint32_t mode=status & 0xf;
 
         //一秒钟发射一次mode
         if(m_iSolutionModeCounter%200 == 0)
         {
             emit sbgSolutionModeSignal(mode);
+//            std::cout << "logData.ekfEulerData.status: " << status << std::endl;
 
-            switch (mode)
-            {
-            case SBG_ECOM_SOL_MODE_UNINITIALIZED:
-//                std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
-                break;
-            case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
-//                std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
-                break;
-            case SBG_ECOM_SOL_MODE_AHRS:
-//                std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
-                break;
-            case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
-//                std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
-                break;
-            case SBG_ECOM_SOL_MODE_NAV_POSITION:
-//                std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;
+            switch (mode) {
+                case SBG_ECOM_SOL_MODE_UNINITIALIZED:
+//                    std::cout << "此刻模式为: " << "UNINITIALIZED" << std::endl;
+                    m_bIsNAV_POSITION_MODE = false;
+                    break;
+                case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
+//                    std::cout << "此刻模式为: " << "VERTICAL_GYRO" << std::endl;
+                    m_bIsNAV_POSITION_MODE = false;
+                    break;
+                case SBG_ECOM_SOL_MODE_AHRS:
+//                    std::cout << "此刻模式为: " << "AHRS" << std::endl;
+                    m_bIsNAV_POSITION_MODE = false;
+                    break;
+                case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
+//                    std::cout << "此刻模式为: " << "NAV_VELOCITY" << std::endl;
+                    m_bIsNAV_POSITION_MODE = false;
+                    break;
+                case SBG_ECOM_SOL_MODE_NAV_POSITION:
+//                    std::cout << "此刻模式为: " << "NAV_POSITION" << std::endl;
+                    m_bIsNAV_POSITION_MODE = true;
 
-                break;
-            default:
-                break;
+                    break;
+                default:
+                    break;
             }
         }
-
-        switch (mode)
-        {
-        case SBG_ECOM_SOL_MODE_UNINITIALIZED:
-//            std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
-            m_bIsNAV_POSITION_MODE=false;
-            break;
-        case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
-//            std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
-            m_bIsNAV_POSITION_MODE=false;
-            break;
-        case SBG_ECOM_SOL_MODE_AHRS:
-//            std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
-            m_bIsNAV_POSITION_MODE=false;
-            break;
-        case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
-//            std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
-            m_bIsNAV_POSITION_MODE=false;
-            break;
-        case SBG_ECOM_SOL_MODE_NAV_POSITION:
-//            std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;
-            m_bIsNAV_POSITION_MODE=true;
-
-            break;
-        default:
-            break;
-        }
     }
     else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_GPS1_POS)
     {
@@ -687,6 +664,10 @@ void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
 
 //        std::cout<<"纬度精度为："<<maximal<<std::endl;
 //        std::cout<<"numSvUsed："<<satelliteCounter<<std::endl;
+//
+//        std::cout<<"latitude："<<logData.gpsPosData.latitude<<std::endl;
+//        std::cout<<"longitude："<<logData.gpsPosData.longitude<<std::endl;
+//        std::cout<<"altitude："<<logData.gpsPosData.altitude<<std::endl;
 
         emit sbgAccuracySignal(static_cast<int>(maximal), satelliteCounter);