From b5a1da8ceb5f475eee372f808ae8d37b8ce4d22e Mon Sep 17 00:00:00 2001 From: tangchao0503 <735056338@qq.com> Date: Mon, 13 Jun 2022 15:12:17 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8E=BB=E6=8E=89sbg=E6=83=AF=E5=AF=BC?= =?UTF-8?q?=E7=9A=84=E7=A3=81=E5=9C=BA=E6=A0=A1=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Header_Files/udpserver.h | 34 +--- Source_Files/udpserver.cpp | 365 +------------------------------------ 2 files changed, 2 insertions(+), 397 deletions(-) diff --git a/Header_Files/udpserver.h b/Header_Files/udpserver.h index c3a8133..c0c74c3 100644 --- a/Header_Files/udpserver.h +++ b/Header_Files/udpserver.h @@ -18,39 +18,10 @@ extern "C" { - #include +// #include #include } - -class sbgMagCibWorkThread:public QThread -{ - Q_OBJECT -public: - sbgMagCibWorkThread(); - - int m_iMagCalibStopControl; - void displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults); -protected: - void run(); -private: -// 0:串口打开错误; -// 1:磁场矫正失败; -// 2:Unable to get onboard magnetic calibration results; -// 3:磁场数据无效:invalid; -// 4:写入磁场数据失败; -// 5:POOR; -// 6:GOOD; -// 7:OPTIMAL; - int m_iMagCalibState; - - void signalWrap(int state); - -signals: - void magCalibStateSignal(int); - -}; - class UdpServer:public QObject { Q_OBJECT @@ -67,8 +38,6 @@ class UdpServer:public QObject private: QUdpSocket * m_udpSocket; - sbgMagCibWorkThread * m_sbgMagCibWorkThread; - QHostAddress m_clientIpAddress; quint16 m_clientPort; @@ -92,7 +61,6 @@ public slots: void onRecordFinished(); void sendSerialPortStatus(int serialPortStatus); - void sendSbgMagCalibState(int SbgMagCalibState); void sendSbgSolutionModeState(int SolutionMode); void sendSbgAccuracyState(int Accuracy); diff --git a/Source_Files/udpserver.cpp b/Source_Files/udpserver.cpp index 1085f65..3325541 100644 --- a/Source_Files/udpserver.cpp +++ b/Source_Files/udpserver.cpp @@ -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代表磁场矫正!"<start(); - } - else if(datagramList[1].toInt()==0) - { - std::cout<<"8-0: 停止磁场矫正!"<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 <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); -} - void UdpServer::sendSbgSolutionModeState(int SolutionMode) { std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <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 <