300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正
This commit is contained in:
tangchao0503
682
Source_Files/udpserver.cpp
Normal file
682
Source_Files/udpserver.cpp
Normal file
@ -0,0 +1,682 @@
|
||||
#include "Header_Files/udpserver.h"
|
||||
|
||||
//using namespace sbgtc;
|
||||
|
||||
UdpServer::UdpServer()
|
||||
{
|
||||
m_udpSocket = new QUdpSocket(this);
|
||||
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);
|
||||
m_RecordSbgThread->start();
|
||||
|
||||
m_RecordThread=new QThread();
|
||||
m_imager=new XimeaImager();
|
||||
m_imager->moveToThread(m_RecordThread);
|
||||
m_RecordThread->start();
|
||||
|
||||
m_CopyFileThread=new QThread();
|
||||
m_copyFile=new FileOperation();
|
||||
m_copyFile->moveToThread(m_CopyFileThread);
|
||||
m_CopyFileThread->start();
|
||||
|
||||
//系统采集步骤1:打开sbg串口并采集数据，打开光谱仪
|
||||
// connect(this, SIGNAL(systemStart()),m_sbgRecorder, SLOT(startRecordSbg()));
|
||||
connect(this, SIGNAL(systemStart()),m_imager, SLOT(openImger()));
|
||||
|
||||
connect(this, SIGNAL(systemStop()),m_sbgRecorder, SLOT(closeSerialPort()));//
|
||||
connect(this, SIGNAL(systemStop()),m_imager, SLOT(closeImger()));
|
||||
|
||||
|
||||
//系统采集步骤2:开始采集高光谱影像
|
||||
connect(this, SIGNAL(startRecordHyperspectralSignal()),m_sbgRecorder, SLOT(startRecordHyperspectral()));
|
||||
connect(m_sbgRecorder, SIGNAL(sbgReady(double,QString)),m_imager, SLOT(startRecord(double,QString)));
|
||||
connect(this, SIGNAL(recordXimeaOnlySignal(double,QString)),m_imager, SLOT(startRecord(double,QString)));
|
||||
|
||||
//系统采集步骤3:停止采集
|
||||
connect(m_imager, SIGNAL(recordFinished()),this, SLOT(onRecordFinished()));
|
||||
|
||||
//系统采集步骤4:拷贝文件
|
||||
connect(this, SIGNAL(startCopyFileSignal()),m_copyFile, SLOT(copyFile()));
|
||||
connect(this, SIGNAL(startDeleteFileSignal()),m_copyFile, SLOT(deleteFile()));
|
||||
|
||||
//系统采集步骤5:进程间通讯
|
||||
connect(m_sbgRecorder, SIGNAL(serialPortStatus(int)),this, SLOT(sendSerialPortStatus(int)));
|
||||
|
||||
connect(m_sbgRecorder, SIGNAL(sbgSolutionModeSignal(int)),this, SLOT(sendSbgSolutionModeState(int)));
|
||||
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)));
|
||||
|
||||
|
||||
|
||||
//当软件不正常关闭并且重启后，通知其他psdk程序
|
||||
m_clientIpAddress=QHostAddress(QHostAddress::LocalHost);
|
||||
sendSerialPortStatus(0);
|
||||
sendXimeaImageStatus(0);
|
||||
sendSbgMagCalibState(0);
|
||||
sendCopyFileStatus(0);
|
||||
|
||||
|
||||
|
||||
|
||||
std::cout<<"UdpServer::UdpServer--------:System ready!"<<std::endl;
|
||||
}
|
||||
|
||||
void UdpServer::processPendingDatagrams()
|
||||
{
|
||||
using namespace std;
|
||||
while (m_udpSocket->hasPendingDatagrams())
|
||||
{
|
||||
QByteArray datagram;
|
||||
|
||||
datagram.resize(m_udpSocket->pendingDatagramSize());
|
||||
m_udpSocket->readDatagram(datagram.data(), datagram.size(),&m_clientIpAddress, &m_clientPort);
|
||||
|
||||
|
||||
printf("接收数据字节数: %d.\n",datagram.size());
|
||||
|
||||
QList<QByteArray> datagramList=datagram.split(',');
|
||||
printf("有多少个list: %d.\n",datagramList.size());
|
||||
|
||||
QString instruction=datagramList[0].data();// QByteArray转QString方法1
|
||||
switch (instruction.toInt())
|
||||
{
|
||||
case 1://启动系统: 打开sbg串口并采集数据，打开光谱仪
|
||||
{
|
||||
std::cout<<"1代表启动系统!"<<std::endl;
|
||||
emit systemStart();
|
||||
|
||||
break;
|
||||
}
|
||||
case 2://关闭系统:关闭相机和sbg串口,关闭软件
|
||||
{
|
||||
std::cout<<"2代表关闭系统!"<<std::endl;
|
||||
|
||||
|
||||
if(m_sbgRecorder->getSbgState()>=1)
|
||||
{
|
||||
m_sbgRecorder->stopRecordSbg();
|
||||
}
|
||||
|
||||
|
||||
if(m_imager->getImagerState()>=1)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
emit systemStop();
|
||||
|
||||
//QCoreApplication::quit();
|
||||
break;
|
||||
}
|
||||
case 3://系统开始采集高光谱影像
|
||||
{
|
||||
//emit startRecordHyperspectralSignal();//真实的影像开始采集通过惯导中的信号(sbgReady)触发
|
||||
m_sbgRecorder->startRecordHyperspectral();
|
||||
|
||||
// if(m_sbgRecorder->getSbgState()==2)//开始采集前还需要判断相机的状态??????????????????????????????????????????
|
||||
// {
|
||||
|
||||
// }
|
||||
// else if(m_sbgRecorder->getSbgState()==3)
|
||||
// {
|
||||
// std::cout<<"系统已经开始采集!"<<std::endl;
|
||||
// }
|
||||
|
||||
break;
|
||||
}
|
||||
case 4://系统停止采集高光谱影像
|
||||
{
|
||||
std::cout<<"4代表系统停止采集高光谱影像!"<<std::endl;
|
||||
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=4)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 5://
|
||||
{
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
|
||||
{
|
||||
std::cout<<"5代表设置帧率!"<<std::endl;
|
||||
m_imager->setFramerate(datagramList[1].toFloat());
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 6://
|
||||
{
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
|
||||
{
|
||||
std::cout<<"6代表自动曝光!"<<std::endl;
|
||||
m_imager->autoExposure();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
if(datagramList[1].toInt()==1)
|
||||
{
|
||||
std::cout<<"拷贝数据!"<<std::endl;
|
||||
|
||||
emit startCopyFileSignal();
|
||||
}
|
||||
else if(datagramList[1].toInt()==0)
|
||||
{
|
||||
std::cout<<"删除数据!"<<std::endl;
|
||||
|
||||
emit startDeleteFileSignal();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
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:
|
||||
{
|
||||
std::cout<<"9代表仅采集影像!"<<std::endl;
|
||||
|
||||
if(datagramList[1].toInt()==1)
|
||||
{
|
||||
QString xx = getFileNameBaseOnTime();
|
||||
xx = xx + "testImage";
|
||||
emit recordXimeaOnlySignal(1000.0,xx);
|
||||
|
||||
// emit recordXimeaOnlySignal(1000.0,"testImage");
|
||||
}
|
||||
else if(datagramList[1].toInt()==0)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
std::cout<<">=9没有意义!"<<std::endl;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double UdpServer::getTimeDifferenceBetweenSystemAndSbg(double secondSbg)
|
||||
{
|
||||
// time_t timer;//time_t就是long int 类型
|
||||
// struct tm *tblock;
|
||||
// timer = time(NULL);//返回秒数(精度为秒)，从1970-1-1,00:00:00 可以当成整型输出或用于其它函数
|
||||
// tblock = localtime(&timer);
|
||||
// printf("Local time is: %s\n", asctime(tblock));
|
||||
|
||||
|
||||
|
||||
//https://blog.csdn.net/FUN6367/article/details/89787566
|
||||
struct timespec systemTime;
|
||||
clock_gettime(CLOCK_REALTIME,&systemTime);
|
||||
tm systemTime_rili;
|
||||
localtime_r(&systemTime.tv_sec, &systemTime_rili);
|
||||
|
||||
// std::cout<<"systemTime_rili--年: "<<systemTime_rili.tm_year+1900<<std::endl;
|
||||
// std::cout<<"systemTime_rili--月: "<<systemTime_rili.tm_mon+1<<std::endl;
|
||||
// std::cout<<"systemTime_rili--日: "<<systemTime_rili.tm_mday<<std::endl;
|
||||
// std::cout<<"systemTime_rili--时: "<<systemTime_rili.tm_hour<<std::endl;
|
||||
// std::cout<<"systemTime_rili--分: "<<systemTime_rili.tm_min<<std::endl;
|
||||
// std::cout<<"systemTime_rili--秒: "<<systemTime_rili.tm_sec<<std::endl;
|
||||
|
||||
// printf("Local time is: %s\n", asctime(&systemTime_rili));
|
||||
|
||||
|
||||
|
||||
double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
|
||||
double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;
|
||||
|
||||
printf("UdpServer::getTimeDifferenceBetweenSystemAndSbg------%f\n", nanosecondSystem-secondSbg);
|
||||
|
||||
return nanosecondSystem-secondSbg;
|
||||
}
|
||||
|
||||
void UdpServer::sender(int status)
|
||||
{
|
||||
QByteArray datagram2send;
|
||||
|
||||
datagram2send.operator =(QString::number(status).toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, m_clientPort+1);
|
||||
}
|
||||
|
||||
void UdpServer::sendSerialPortStatus(int serialPortStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< serialPortStatus <<std::endl;
|
||||
|
||||
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< m_clientIpAddress.AnyIPv4 <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "sbg," + QString::number(serialPortStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
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;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "SolutionMode," + QString::number(SolutionMode);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendSbgAccuracyState(int Accuracy)
|
||||
{
|
||||
// std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "Accuracy," + QString::number(Accuracy);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendXimeaImageStatus(int ximeaImageStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendXimeaImageStatus---------------------:"<< ximeaImageStatus <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "ximea," + QString::number(ximeaImageStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendCopyFileStatus(int fileStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendCopyFileStatus---------------------:"<< fileStatus <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "file," + QString::number(fileStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
Reference in New Issue
Block a user