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

去掉sbg惯导的磁场校正

Browse Source
This commit is contained in:
tangchao0503
2022-06-13 15:12:17 +08:00
parent 1452bcc2b9
commit b5a1da8ceb
2 changed files with 2 additions and 397 deletions
Download Patch File Download Diff File Expand all files Collapse all files

365
Source_Files/udpserver.cpp
View File

@ -8,8 +8,6 @@ UdpServer::UdpServer()
m_udpSocket->bind(45454, QUdpSocket::ShareAddress);
connect(m_udpSocket, SIGNAL(readyRead()),this, SLOT(processPendingDatagrams()));
m_sbgMagCibWorkThread = new sbgMagCibWorkThread();
m_RecordSbgThread=new QThread();
m_sbgRecorder=new sbgtc::SbgRecorder();
m_sbgRecorder->moveToThread(m_RecordSbgThread);
@ -52,7 +50,6 @@ UdpServer::UdpServer()
connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int)),this, SLOT(sendSbgAccuracyState(int)));
connect(m_imager, SIGNAL(ximeaImageStatus(int)),this, SLOT(sendXimeaImageStatus(int)));
connect(m_sbgMagCibWorkThread, SIGNAL(magCalibStateSignal(int)),this, SLOT(sendSbgMagCalibState(int)));
connect(m_copyFile, SIGNAL(copyFileStatus(int)),this, SLOT(sendCopyFileStatus(int)));
@ -61,7 +58,6 @@ UdpServer::UdpServer()
m_clientIpAddress=QHostAddress(QHostAddress::LocalHost);
sendSerialPortStatus(0);
sendXimeaImageStatus(0);
sendSbgMagCalibState(0);
sendCopyFileStatus(0);
@ -183,23 +179,7 @@ void UdpServer::processPendingDatagrams()
}
case 8:
{
std::cout<<"8代表磁场矫正!"<<std::endl;
if(datagramList[1].toInt()==1)
{
//magCalib();
std::cout<<"8-1: 开始磁场矫正!"<<std::endl;
m_sbgMagCibWorkThread->start();
}
else if(datagramList[1].toInt()==0)
{
std::cout<<"8-0: 停止磁场矫正!"<<std::endl;
m_sbgMagCibWorkThread->m_iMagCalibStopControl=0;
}
break;
;
}
case 9:
{
@ -285,18 +265,6 @@ void UdpServer::sendSerialPortStatus(int serialPortStatus)
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendSbgMagCalibState(int SbgMagCalibState)
{
std::cout<<"UdpServer::sendSbgMagCalibState---------------------:"<< SbgMagCalibState <<std::endl;
QByteArray datagram2send;
QString status = "mag," + QString::number(SbgMagCalibState);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendSbgSolutionModeState(int SolutionMode)
{
std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <<std::endl;
@ -349,334 +317,3 @@ void UdpServer::onRecordFinished()
{
std::cout<<"UdpServer::onRecordFinished----------------:影像停止采集"<<std::endl;
}
sbgMagCibWorkThread::sbgMagCibWorkThread()
{
}
void sbgMagCibWorkThread::displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults)
{
//
// Display the magnetic calibration results
//
printf("\n\n======== Magnetic calibration report ========\n");
//
// Convert the quality indicator to human readable output
//
switch (pMagCalibResults->quality)
{
case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL:
{
printf("Quality:\t\toptimal\n");
signalWrap(7);
break;
}
case SBG_ECOM_MAG_CALIB_QUAL_GOOD:
{
printf("Quality:\t\tgood\n");
signalWrap(6);
break;
}
case SBG_ECOM_MAG_CALIB_QUAL_POOR:
{
printf("Quality:\t\tpoor\n");
signalWrap(5);
break;
}
default:
printf("Quality:\t\tundefined\n");
}
//
// Convert the confidence indicator to human readable output
//
switch (pMagCalibResults->confidence)
{
case SBG_ECOM_MAG_CALIB_TRUST_HIGH:
printf("Confidence:\t\thigh\n");
break;
case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM:
printf("Confidence:\t\tmedium\n");
break;
case SBG_ECOM_MAG_CALIB_TRUST_LOW:
printf("Confidence:\t\tlow\n");
break;
default:
printf("Confidence:\t\tundefined\n");
}
//
// Print advanced status
//
printf("Advanced Status:\n");
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS)
{
printf("\t- Not enough valid points. Maybe you are moving too fast.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS)
{
printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE)
{
printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n");
}
//
// Test if we have a 2D or 3D calibration mode
//
if (mode == SBG_ECOM_MAG_CALIB_MODE_2D)
{
//
// In 2D mode, a X or Y motion issue means we have too much motion
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
{
printf("\t- Too much roll motion for a 2D magnetic calibration.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
{
printf("\t- Too much pitch motion for a 2D magnetic calibration.\n");
}
}
else
{
//
// In 3D mode, a X or Y motion issue means we have not enough motion
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
{
printf("\t- Not enough roll motion for a 3D magnetic calibration.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
{
printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n");
}
}
//
// Test if we had enough yaw motion to compute a calibration
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE)
{
//
// Test if we are in
printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n");
}
//
// Display the number of points used to compute the magnetic calibration
//
printf("\n\n");
printf("Used Points:\t%u\n", pMagCalibResults->numPoints);
printf("Max Points:\t%u\n", pMagCalibResults->maxNumPoints);
//
// Display magnetic field deviation errors
//
printf("\n\n-------------------------------------\n");
printf("Magnetic field deviation report\n");
printf("-------------------------------------\n");
printf("\t\tMean\tStd\tMax\n");
printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError);
printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError);
printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegF(pMagCalibResults->meanAccuracy), sbgRadToDegF(pMagCalibResults->stdAccuracy), sbgRadToDegF(pMagCalibResults->maxAccuracy));
printf("\n\n\n");
}
void sbgMagCibWorkThread::run()
{
m_iMagCalibStopControl = 1;
SbgEComHandle comHandle;
SbgErrorCode errorCode;
SbgInterface sbgInterface;
int32 retValue = 0;
SbgEComDeviceInfo deviceInfo;
SbgEComMagCalibResults magCalibResults;
SbgEComMagCalibMode mode;
//
// Create an interface:
// We can choose either a serial for real time operation, or file for previously logged data parsing
// Note interface closing is also differentiated !
//
errorCode = sbgInterfaceSerialCreate(&sbgInterface, "/dev/sbg_serial_port", 460800); // Example for Unix using a FTDI Usb2Uart converter
//errorCode = sbgInterfaceSerialCreate(&sbgInterface, "COM23", 115200); // Example for Windows serial communication
//
// Test that the interface has been created
//
if (errorCode == SBG_NO_ERROR)
{
//
// Create the sbgECom library and associate it with the created interfaces
//
errorCode = sbgEComInit(&comHandle, &sbgInterface);
//
// Test that the sbgECom has been initialized
//
if (errorCode == SBG_NO_ERROR)
{
printf("sbgECom properly Initialized.\n");
printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR);
//
// Get device information
//
errorCode = sbgEComCmdGetInfo(&comHandle, &deviceInfo);
//
// Display device information if no error
//
if (errorCode == SBG_NO_ERROR)
{
printf("Device : %0.9u found\n", deviceInfo.serialNumber);
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get device information.\n");
}
//
// Define the calibration mode to perform
//
mode = SBG_ECOM_MAG_CALIB_MODE_3D;
//
// Start / reset the acquisition of magnetic field data
// Each time this command is called, the device is prepared to acquire a new set of magnetic field data
// You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration
//
errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW);
//
// Make sure that the magnetic calibration has started
//
if (errorCode == SBG_NO_ERROR)
{
//
// The device is now acquiring some magnetic field data.
// Wait for a user input before computing the magnetic calibration
//
printf("The device is acquiring magnetic field data.\n\nPress enter to stop the magnetic field acquisition.\n");
//fgetc(stdin);//-------------------------------------------------------------------------------------------------------------------------------------------
while (m_iMagCalibStopControl)
{
printf("h");
}
printf("kkkkkkkkkkkkkkkkkkkkkkkkkkkk\n");
//
// Try to compute a magnetic calibration and get the results
//
errorCode = sbgEComCmdMagComputeCalib(&comHandle, &magCalibResults);
//
// Make sure that we were able to get magnetic calibration results
//
if (errorCode == SBG_NO_ERROR)
{
//
// Test if the device has computed a valid magnetic calibration
//
if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID)
{
//
// Send the new magnetic calibration data
//
errorCode = sbgEComCmdMagSetCalibData(&comHandle, magCalibResults.offset, magCalibResults.matrix);
//
// Make sure that the new magnetic calibration data has been updated
//
if (errorCode == SBG_NO_ERROR)
{
printf("The new magnetic calibration has been applied.\n");
//
// Display the magnetic calibration status
//
displayMagCalibResults(mode, &magCalibResults);
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to upload new magnetic calibration data.\n");
signalWrap(4);
}
}
else
{
//
// Unable to compute a valid magnetic calibration
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to compute a valid magnetic calibration.\n");
signalWrap(3);
}
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get onboard magnetic calibration results.\n");
signalWrap(2);
}
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to start the onboard magnetic calibration.\n");
signalWrap(1);
}
//
// Close the sbgEcom library
//
sbgEComClose(&comHandle);
}
else
{
//
// Unable to initialize the sbgECom
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to initialize the sbgECom library.\n");
retValue = -1;
signalWrap(0);
}
//
// Close the interface
//
sbgInterfaceSerialDestroy(&sbgInterface);
}
else
{
//
// Unable to create the interface
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to create the interface.\n");
retValue = -1;
signalWrap(0);
}
printf("magCalib process complete!\n");
//fgetc(stdin);
}
void sbgMagCibWorkThread::signalWrap(int state)
{
std::cout<<"sbgMagCibWorkThread::signalWrap---------------------:"<< state <<std::endl;
m_iMagCalibState=state;
emit magCalibStateSignal(m_iMagCalibState);
}