tangchao0503/ximeaAirborneSystem
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix bug：解析sbg的sulution mode；

Browse Source
This commit is contained in:
tangchao0503
2023-08-18 16:01:22 +08:00
parent e4a7c4481b
commit ba1b01bccc
1 changed files with 30 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
Source_Files/sbgrecorder.cpp
View File

@ -608,66 +608,43 @@ void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
//判断模式是否为: NAV_POSITION //判断模式是否为: NAV_POSITION
if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER) 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 status=logData.ekfEulerData.status;
uint32_t mode=status>>24;//????????????????????????????????????? uint32_t mode=status & 0xf;
// uint32_t mode=status;//这是错的
//一秒钟发射一次mode //一秒钟发射一次mode
if(m_iSolutionModeCounter%200 == 0) if(m_iSolutionModeCounter%200 == 0)
{ {
emit sbgSolutionModeSignal(mode); emit sbgSolutionModeSignal(mode);
// std::cout << "logData.ekfEulerData.status: " << status << std::endl;
switch (mode) switch (mode) {
{ case SBG_ECOM_SOL_MODE_UNINITIALIZED:
case SBG_ECOM_SOL_MODE_UNINITIALIZED: // std::cout << "此刻模式为: " << "UNINITIALIZED" << std::endl;
// std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl; m_bIsNAV_POSITION_MODE = false;
break; break;
case SBG_ECOM_SOL_MODE_VERTICAL_GYRO: case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
// std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl; // std::cout << "此刻模式为: " << "VERTICAL_GYRO" << std::endl;
break; m_bIsNAV_POSITION_MODE = false;
case SBG_ECOM_SOL_MODE_AHRS: break;
// std::cout<<"此刻模式为: "<<"AHRS"<<std::endl; case SBG_ECOM_SOL_MODE_AHRS:
break; // std::cout << "此刻模式为: " << "AHRS" << std::endl;
case SBG_ECOM_SOL_MODE_NAV_VELOCITY: m_bIsNAV_POSITION_MODE = false;
// std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl; break;
break; case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
case SBG_ECOM_SOL_MODE_NAV_POSITION: // std::cout << "此刻模式为: " << "NAV_VELOCITY" << std::endl;
// std::cout<<"此刻模式为: "<<"NAV_POSITION"<<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; break;
default: default:
break; 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) 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<<"纬度精度为："<<maximal<<std::endl;
// std::cout<<"numSvUsed："<<satelliteCounter<<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); emit sbgAccuracySignal(static_cast<int>(maximal), satelliteCounter);