300TC 机载系统 完整功能,(1)采集影像(2)采集和解析惯导数据(3)惯导磁场校正

This commit is contained in:
tangchao0503
2022-06-13 12:01:30 +08:00
commit 1452bcc2b9
21 changed files with 4316 additions and 0 deletions

682
Source_Files/udpserver.cpp Normal file
View 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);
}