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
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);