300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正
This commit is contained in:
tangchao0503
199
Source_Files/fileoperation.cpp
Normal file
199
Source_Files/fileoperation.cpp
Normal file
@ -0,0 +1,199 @@
|
||||
#include "Header_Files/fileoperation.h"
|
||||
|
||||
FileOperation::FileOperation()
|
||||
{
|
||||
m_copyFileStatus = 0;
|
||||
}
|
||||
|
||||
bool FileOperation::copyFileToPath(QString sourceDir ,QString toDir, bool coverFileIfExist)
|
||||
{
|
||||
toDir.replace("\\","/");
|
||||
if (sourceDir == toDir){
|
||||
return true;
|
||||
}
|
||||
if (!QFile::exists(sourceDir)){
|
||||
return false;
|
||||
}
|
||||
QDir *createfile = new QDir;
|
||||
bool exist = createfile->exists(toDir);
|
||||
if (exist){
|
||||
if(coverFileIfExist){
|
||||
createfile->remove(toDir);
|
||||
}
|
||||
}//end if
|
||||
|
||||
if(!QFile::copy(sourceDir, toDir))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
QStringList FileOperation::getSubfolders(QDir parentFolder)
|
||||
{
|
||||
QStringList entryList= parentFolder.entryList();
|
||||
QStringList result;
|
||||
|
||||
//
|
||||
for(int i=0;i<entryList.size();i++)
|
||||
{
|
||||
auto element=entryList.at(i).toLocal8Bit().constData();
|
||||
QString tmp=entryList.at(i);
|
||||
|
||||
if(!strcmp(element,".") || !strcmp(element,".."))//QString::compare()
|
||||
{
|
||||
//std::cout<<"11111111111111111111111111111"<<std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
result.append(entryList.at(i));
|
||||
//std::cout<<""<<entryList.at(i).toLocal8Bit().constData()<<std::endl;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
QString FileOperation::getFlashDrivePath()
|
||||
{
|
||||
//获取u盘路径
|
||||
QDir flashDrivePath("/media");
|
||||
QStringList subfolders=getSubfolders(flashDrivePath);
|
||||
flashDrivePath.cd(subfolders.at(0));
|
||||
|
||||
QStringList result=getSubfolders(flashDrivePath);
|
||||
|
||||
|
||||
if(result.isEmpty())
|
||||
{
|
||||
//std::cout<<"FileOperation::getFlashDrivePath--------------------------: 请插入u盘!"<<std::endl;
|
||||
return "";
|
||||
}
|
||||
|
||||
if(!flashDrivePath.cd(result.at(0)))
|
||||
{
|
||||
std::cout<<"进入u盘失败!"<<std::endl;
|
||||
return "";
|
||||
}
|
||||
|
||||
if(!flashDrivePath.exists("ximeaImageData"))
|
||||
{
|
||||
flashDrivePath.mkdir("ximeaImageData");
|
||||
}
|
||||
flashDrivePath.cd("ximeaImageData");
|
||||
|
||||
QString destinationFolder=flashDrivePath.absolutePath();
|
||||
std::cout<<"进入u盘成功:"<<destinationFolder.toStdString()<<std::endl;
|
||||
|
||||
return destinationFolder;
|
||||
}
|
||||
|
||||
QDir FileOperation::getSourcePath()
|
||||
{
|
||||
//获取源数据文件夹
|
||||
|
||||
QDir sourcePath("/media/nvme");
|
||||
|
||||
// QString homePath=QDir::homePath();
|
||||
// QDir sourcePath(homePath);
|
||||
// bool re=sourcePath.exists("ximeaImageData");
|
||||
// if(!re)
|
||||
// {
|
||||
// std::cout<<"No data!"<<std::endl;
|
||||
// sourcePath.mkdir("ximeaImageData");
|
||||
// }
|
||||
//
|
||||
// sourcePath.cd("ximeaImageData");
|
||||
|
||||
return sourcePath;
|
||||
}
|
||||
|
||||
void FileOperation::copyFile()
|
||||
{
|
||||
QString destinationFolder= getFlashDrivePath();
|
||||
QDir sourcePath= getSourcePath();
|
||||
|
||||
if(destinationFolder.isEmpty())
|
||||
{
|
||||
m_copyFileStatus = 3;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
QStringList timesFilter,sbgFilter,hdrFilter,datFilter;
|
||||
timesFilter<<"*.times";
|
||||
sbgFilter<<"*.sbg";
|
||||
hdrFilter<<"*.hdr";
|
||||
datFilter<<"*.dat";
|
||||
|
||||
QStringList timesEntryList=sourcePath.entryList(timesFilter,QDir::Files);
|
||||
QStringList sbgEntryList=sourcePath.entryList(sbgFilter,QDir::Files);
|
||||
QStringList hdrEntryList=sourcePath.entryList(hdrFilter,QDir::Files);
|
||||
QStringList datEntryList=sourcePath.entryList(datFilter,QDir::Files);
|
||||
|
||||
if(timesEntryList.isEmpty() && sbgEntryList.isEmpty() && hdrEntryList.isEmpty() && datEntryList.isEmpty())
|
||||
{
|
||||
m_copyFileStatus = 2;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
m_copyFileStatus = 1;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
|
||||
|
||||
//copy times file
|
||||
std::cout<<"copy times file"<<std::endl;
|
||||
for(int i=0;i<timesEntryList.size();i++)
|
||||
{
|
||||
// std::cout<<"------"<<timesEntryList.at(i).toLocal8Bit().constData()<<std::endl;
|
||||
// std::cout<<"------"<<timesEntryList.at(i).<<std::endl;
|
||||
|
||||
QString source=sourcePath.absolutePath()+"/"+timesEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+timesEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy sbg file
|
||||
std::cout<<"copy sbg file"<<std::endl;
|
||||
for(int i=0;i<sbgEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+sbgEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+sbgEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy hdr file
|
||||
std::cout<<"copy hdr file"<<std::endl;
|
||||
for(int i=0;i<hdrEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+hdrEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+hdrEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy image file
|
||||
std::cout<<"copy image file"<<std::endl;
|
||||
for(int i=0;i<datEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+datEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+datEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
m_copyFileStatus = 0;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
}
|
||||
|
||||
void FileOperation::deleteFile()
|
||||
{
|
||||
QDir sourcePath= getSourcePath();
|
||||
|
||||
sourcePath.removeRecursively();
|
||||
}
|
||||
13
Source_Files/main.cpp
Normal file
13
Source_Files/main.cpp
Normal file
@ -0,0 +1,13 @@
|
||||
#include <QCoreApplication>
|
||||
#include "Header_Files/udpserver.h"
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
QCoreApplication a(argc, argv);
|
||||
|
||||
//UdpServer* x=new UdpServer();
|
||||
QString str="234234";
|
||||
UdpServer x;
|
||||
|
||||
return a.exec();
|
||||
}
|
||||
636
Source_Files/sbgbuffer.cpp
Normal file
636
Source_Files/sbgbuffer.cpp
Normal file
@ -0,0 +1,636 @@
|
||||
#include "Header_Files/sbgbuffer.h"
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
rawBuffer sbgtc::initRawBuffer(QByteArray * sbgMessage)
|
||||
{
|
||||
rawBuffer tmp;
|
||||
|
||||
tmp.rxBuffer=(uint8_t *)sbgMessage->data();
|
||||
tmp.rxBufferSize=sbgMessage->size();
|
||||
|
||||
return tmp;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
// assert(pLinkedBuffer);
|
||||
|
||||
//
|
||||
// Initialize stream parameters
|
||||
//
|
||||
pHandle->bufferSize = bufferSize;
|
||||
pHandle->errorCode = SBG_NO_ERROR;
|
||||
|
||||
//
|
||||
// Initialize the buffer
|
||||
//
|
||||
pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer;
|
||||
pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer;
|
||||
|
||||
//
|
||||
// For now, we don't handle any error, maybe we could add checks in debug mode only
|
||||
//
|
||||
return SBG_NO_ERROR;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// According to the origin reference point
|
||||
//
|
||||
switch (origin)
|
||||
{
|
||||
case SB_SEEK_SET:
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + offset;
|
||||
break;
|
||||
case SB_SEEK_CUR_INC:
|
||||
pHandle->pCurrentPtr += offset;
|
||||
break;
|
||||
case SB_SEEK_CUR_DEC:
|
||||
pHandle->pCurrentPtr -= offset;
|
||||
break;
|
||||
case SB_SEEK_END:
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset);
|
||||
break;
|
||||
default:
|
||||
pHandle->errorCode = SBG_INVALID_PARAMETER;
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter");
|
||||
}
|
||||
|
||||
//
|
||||
// Make sure that no error has occurred
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if the current ptr is still within the buffer bounds
|
||||
//
|
||||
if (pHandle->pCurrentPtr < pHandle->pBufferPtr)
|
||||
{
|
||||
//
|
||||
// We are before the buffer so clamp to the begining of the buffer and raise an error
|
||||
//
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr;
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
|
||||
//
|
||||
// Stream buffer underflow
|
||||
//
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer");
|
||||
}
|
||||
else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize)
|
||||
{
|
||||
//
|
||||
// We are after the buffer so clamp to the end of the buffer and raise an error
|
||||
//
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize;
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
|
||||
//
|
||||
// Stream buffer overflow
|
||||
//
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return pHandle->errorCode;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferTell(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr;
|
||||
}
|
||||
|
||||
void * sbgtc::sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return pHandle->pCurrentPtr;
|
||||
}
|
||||
|
||||
uint8_t sbgtc::sbgStreamBufferReadUint8LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t))
|
||||
{
|
||||
//
|
||||
// Read the byte
|
||||
//
|
||||
return *((uint8_t*)(pHandle->pCurrentPtr++));
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t sbgtc::sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint16_t bytesValues[2];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint16_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[1] | (bytesValues[0] << 8);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t sbgtc::sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint32_t bytesValues[4];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint32_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Read an uint64_t from a stream buffer (Little endian version).
|
||||
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
||||
* \return The read value or 0 if we have an error.
|
||||
*/
|
||||
uint64_t sbgtc::sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint64_t lowPart;
|
||||
uint64_t highPart;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
lowPart = *((uint64_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint64_t);
|
||||
|
||||
return lowPart;
|
||||
#else
|
||||
//
|
||||
// Read 64 bit value using two 32 bits read to avoid too much 64 bits operations
|
||||
//
|
||||
lowPart = sbgStreamBufferReadUint32LE(pHandle);
|
||||
highPart = sbgStreamBufferReadUint32LE(pHandle);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return (lowPart << 32) | highPart;
|
||||
#else
|
||||
return lowPart | (highPart << 32);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0ull;
|
||||
}
|
||||
|
||||
int8_t sbgtc::sbgStreamBufferReadInt8LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t))
|
||||
{
|
||||
//
|
||||
// Read the byte
|
||||
//
|
||||
return *((int8_t*)(pHandle->pCurrentPtr++));
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t sbgtc::sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
int32_t bytesValues[4];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(int32_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
float sbgtc::sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
FloatNint floatInt;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float))
|
||||
{
|
||||
//
|
||||
// Read the float as an uint32_t
|
||||
//
|
||||
floatInt.valU = sbgStreamBufferReadUint32LE(pHandle);
|
||||
|
||||
//
|
||||
// Return the float using an union to avoid compiler cast
|
||||
//
|
||||
return floatInt.valF;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Read an double from a stream buffer (Little endian version).
|
||||
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
||||
* \return The read value or 0 if we have an error.
|
||||
*/
|
||||
double sbgtc::sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
DoubleNint doubleInt;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double))
|
||||
{
|
||||
//
|
||||
// Read the float as an uint64_t
|
||||
//
|
||||
doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle);
|
||||
|
||||
//
|
||||
// Return the double using an union to avoid compiler cast
|
||||
//
|
||||
return doubleInt.valF;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return error code
|
||||
//
|
||||
return pHandle->errorCode;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the space left in bytes
|
||||
//
|
||||
return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle);
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetSize(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the linked buffer size
|
||||
//
|
||||
return pHandle->bufferSize;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the number of bytes between the begin of the stream and the current pointer
|
||||
//
|
||||
return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr);
|
||||
}
|
||||
|
||||
void * sbgtc::sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return pHandle->pBufferPtr;
|
||||
}
|
||||
67
Source_Files/sbgcrc.cpp
Normal file
67
Source_Files/sbgcrc.cpp
Normal file
@ -0,0 +1,67 @@
|
||||
#include "Header_Files/sbgcrc.h"
|
||||
|
||||
|
||||
uint16_t sbgtc::sbgCrc16Compute(const void *pData, size_t dataSize)
|
||||
{
|
||||
SbgCrc16 crcInst;
|
||||
|
||||
//
|
||||
// Initialize the CRC system
|
||||
//
|
||||
sbgCrc16Initialize(&crcInst);
|
||||
|
||||
//
|
||||
// Compute the CRC
|
||||
//
|
||||
sbgCrc16Update(&crcInst, pData, dataSize);
|
||||
|
||||
//
|
||||
// Return it
|
||||
//
|
||||
return sbgCrc16Get(&crcInst);
|
||||
}
|
||||
|
||||
void sbgtc::sbgCrc16Initialize(SbgCrc16 *pInstance)
|
||||
{
|
||||
//
|
||||
// Test input argument
|
||||
//
|
||||
//assert(pInstance);
|
||||
|
||||
*pInstance = 0;
|
||||
}
|
||||
|
||||
void sbgtc::sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize)
|
||||
{
|
||||
const uint8_t *pBuffer = (const uint8_t*)pData;
|
||||
uint8_t index;
|
||||
size_t i;
|
||||
|
||||
//
|
||||
// Test input arguments
|
||||
//
|
||||
//assert(pInstance);
|
||||
//assert(pData);
|
||||
|
||||
//
|
||||
// For each byte in our buffer
|
||||
//
|
||||
for (i = 0; i < dataSize; i++)
|
||||
{
|
||||
//
|
||||
// Update the current CRC
|
||||
//
|
||||
index = (pBuffer[i] ^ *pInstance) & 0xFF;
|
||||
*pInstance = crc16LookupTable[index] ^ (*pInstance >> 8);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t sbgtc::sbgCrc16Get(const SbgCrc16 *pInstance)
|
||||
{
|
||||
return *pInstance;
|
||||
}
|
||||
|
||||
uint32_t sbgtc::sbgCrc32Get(const SbgCrc32 *pInstance)
|
||||
{
|
||||
return *pInstance;
|
||||
}
|
||||
131
Source_Files/sbglogparse.cpp
Normal file
131
Source_Files/sbglogparse.cpp
Normal file
@ -0,0 +1,131 @@
|
||||
#include "Header_Files/sbglogparse.h"
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData)
|
||||
{
|
||||
//assert(pInputStream);
|
||||
//assert(pOutputData);
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData)
|
||||
{
|
||||
// assert(pInputStream);
|
||||
// assert(pOutputData);
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream);
|
||||
pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \param[in] pInputStream Input stream buffer to read the payload from.
|
||||
* \param[out] pOutputData Pointer on the output structure that stores parsed data.
|
||||
* \return SBG_NO_ERROR if the payload has been parsed.
|
||||
*/
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData)
|
||||
{
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
//
|
||||
// Test if we have a additional information such as base station id (since version 1.4)
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pInputStream) >= 5)
|
||||
{
|
||||
//
|
||||
// Read the additional information
|
||||
//
|
||||
pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream);
|
||||
pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Default the additional information
|
||||
//
|
||||
pOutputData->numSvUsed = 0;
|
||||
pOutputData->baseStationId = 0xFFFF;
|
||||
pOutputData->differentialAge = 0xFFFF;
|
||||
}
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
845
Source_Files/sbgrecorder.cpp
Normal file
845
Source_Files/sbgrecorder.cpp
Normal file
@ -0,0 +1,845 @@
|
||||
#include "Header_Files/sbgrecorder.h"
|
||||
#include <iostream>
|
||||
#include <string.h>
|
||||
#include <QDebug>
|
||||
|
||||
typedef unsigned char BYTE;
|
||||
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
|
||||
sbgtc::SbgRecorder::SbgRecorder()
|
||||
{
|
||||
connect(this, SIGNAL(sbgReady(double,QString)),this, SLOT(onSbgReady(double,QString)));
|
||||
|
||||
m_bIsSbgReady = false;
|
||||
m_bIsRecordHyperspecatralImage = false;
|
||||
m_bIsNAV_POSITION_MODE = false;
|
||||
m_bIsAccuracyLessThan7 = false;
|
||||
m_bIsSyncSystemTimeBaseGpstime = false;
|
||||
|
||||
m_iSolutionMode=SBG_ECOM_SOL_MODE_UNINITIALIZED;
|
||||
m_iSolutionModeCounter=0;
|
||||
|
||||
m_iSbgState=0;
|
||||
|
||||
std::cout<<"SbgRecorder::SbgRecorder(construction) run!"<<std::endl;
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::openSerialPort()
|
||||
{
|
||||
QList<QSerialPortInfo> infos = QSerialPortInfo::availablePorts();
|
||||
|
||||
//std::cout<<"number of availablePorts:"<<infos.size()<<std::endl;
|
||||
|
||||
//只能是大疆psdk程序发送命令给本程序, 而大疆子自己需要一个串口, 所以当只有一个串口时, sbg的就没有串口可用
|
||||
// if(infos.size()==0||infos.size()==1)
|
||||
// {
|
||||
// m_iSbgState=0;
|
||||
// emit serialPortStatus(m_iSbgState);
|
||||
// return;
|
||||
// }
|
||||
|
||||
//如果在构造函数中创建m_serial就会出现错误:
|
||||
//QObject: Cannot create children for a parent that is in a different thread.
|
||||
//(Parent is QSerialPort(0x2e31b20), parent's thread is QThread(0x2e2f130), current thread is QThread(0x2e31110)
|
||||
m_serial=new QSerialPort();
|
||||
|
||||
if(m_serial->isOpen())//如果串口已经打开了 先给他关闭了
|
||||
{
|
||||
m_serial->clear();
|
||||
m_serial->close();
|
||||
}
|
||||
|
||||
// QString portname = "sbg_serial_port";
|
||||
QString portname = "ttyS4";
|
||||
|
||||
m_serial->setPortName(portname);
|
||||
m_serial->open(QIODevice::ReadWrite);
|
||||
|
||||
|
||||
bool x=m_serial->setBaudRate(921600);//
|
||||
if(x)
|
||||
{
|
||||
std::cout<<"波特率被成功设置为:"<<m_serial->baudRate()<<std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"波特率设置失败!"<<std::endl;
|
||||
|
||||
m_iSbgState=1;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
return;
|
||||
}
|
||||
|
||||
m_iSbgState=2;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::closeSerialPort()
|
||||
{
|
||||
if(m_iSbgState>0)
|
||||
{
|
||||
if(m_iSbgState==3)
|
||||
stopRecordSbg();
|
||||
|
||||
m_serial->close();
|
||||
m_iSbgState=0;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::onSbgReady(double second,QString baseFileName)
|
||||
{
|
||||
m_bIsSbgReady=true;
|
||||
m_bIsRecordHyperspecatralImage=false;
|
||||
}
|
||||
|
||||
QByteArray sbgtc::SbgRecorder::merge(QByteArray sbgMessage)
|
||||
{
|
||||
// BYTE mC = 0x0C;
|
||||
// BYTE mD = 0x0D;
|
||||
// BYTE mOut = 0x0C<<4|0x0D;
|
||||
// BYTE mOut1 = mC<<4|mD;
|
||||
|
||||
QByteArray x;
|
||||
QByteArray y;
|
||||
x.resize(sbgMessage.size()/2);
|
||||
y.resize(sbgMessage.size()/2);
|
||||
for(int i=0;i<sbgMessage.size()/2;i++)
|
||||
{
|
||||
char tmp=sbgMessage.at(i*2);
|
||||
char tmp2=tmp<<4;
|
||||
|
||||
x[i]=sbgMessage[i*2]<<4|sbgMessage[i*2+1];
|
||||
y[i]=sbgMessage[i*2]<<4;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
bool sbgtc::SbgRecorder::verify(int index, QByteArray sbgMessage)
|
||||
{
|
||||
if(index<0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
//std::cout<<"index:"<<index<<std::endl;
|
||||
//std::cout<<"count:"<<count<<std::endl;
|
||||
//return;
|
||||
}
|
||||
|
||||
int scale=2;
|
||||
|
||||
QByteArray msgId=sbgMessage.mid(index+2*scale,2);
|
||||
QByteArray classId=sbgMessage.mid(index+3*scale,2);
|
||||
|
||||
QByteArray lengthOfData=sbgMessage.mid(index+4*scale,4);
|
||||
std::reverse(lengthOfData.begin(), lengthOfData.end());
|
||||
int tmp=lengthOfData.toInt();//?????????????
|
||||
|
||||
QByteArray crc=sbgMessage.mid(index+(6+tmp)*scale,4);
|
||||
QByteArray etx=sbgMessage.mid(index+(6+tmp+2)*scale,2);//end of frame
|
||||
|
||||
if(etx=="33")
|
||||
{
|
||||
std::cout<<"结尾符正确!"<<std::endl;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"结尾符不正确!--------"<<std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
sbgtc::SbgErrorCode sbgtc::SbgRecorder::extractOneValidFrame(rawBuffer *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize)
|
||||
{
|
||||
SbgErrorCode errorCode = SBG_NOT_READY;
|
||||
SbgStreamBuffer inputStream;
|
||||
bool syncFound;
|
||||
size_t payloadSize = 0;
|
||||
uint16_t frameCrc;//
|
||||
uint16_t computedCrc;//
|
||||
//size_t i;
|
||||
|
||||
uint8_t receivedMsgClass;
|
||||
uint8_t receivedMsg;
|
||||
size_t payloadOffset;
|
||||
|
||||
while (pHandle->rxBufferSize > 0)
|
||||
{
|
||||
//
|
||||
// For now, we haven't found any start of frame
|
||||
//
|
||||
syncFound = false;
|
||||
|
||||
//
|
||||
// To find a valid start of frame we need at least 2 bytes in the reception buffer
|
||||
//
|
||||
if (pHandle->rxBufferSize >= 2)
|
||||
{
|
||||
//
|
||||
// Try to find a valid start of frame by looking for SYNC_1 and SYNC_2 chars
|
||||
//
|
||||
for (int i = 0; i < pHandle->rxBufferSize-1; i++)
|
||||
{
|
||||
//
|
||||
// A valid start of frame should begin with SYNC and when STX chars
|
||||
//
|
||||
if ( (pHandle->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pHandle->rxBuffer[i+1] == SBG_ECOM_SYNC_2) )
|
||||
{
|
||||
//
|
||||
// We have found the sync char, test if we have dummy bytes at the begining of the frame
|
||||
//
|
||||
if (i > 0)
|
||||
{
|
||||
//
|
||||
// Remove all dumy received bytes before the begining of the frame
|
||||
//
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+i, pHandle->rxBufferSize-i);
|
||||
pHandle->rxBufferSize = pHandle->rxBufferSize-i;
|
||||
}
|
||||
|
||||
//
|
||||
// The sync has been found
|
||||
//
|
||||
syncFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// Check if a valid start of frame has been found
|
||||
//
|
||||
if (syncFound)
|
||||
{
|
||||
//
|
||||
// A valid start of frame has been found, try to extract the frame if we have at least a whole frame.
|
||||
//
|
||||
if (pHandle->rxBufferSize < 8)
|
||||
{
|
||||
//
|
||||
// Don't have enough data for a valid frame
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
//
|
||||
// Initialize an input stream buffer to parse the received frame
|
||||
//
|
||||
sbgStreamBufferInitForRead(&inputStream, pHandle->rxBuffer, pHandle->rxBufferSize);
|
||||
|
||||
//
|
||||
// Skip both the Sync 1 and Sync 2 chars
|
||||
//
|
||||
sbgStreamBufferSeek(&inputStream, sizeof(uint8_t)*2, SB_SEEK_CUR_INC);
|
||||
|
||||
//
|
||||
// Read the command
|
||||
//
|
||||
receivedMsg = sbgStreamBufferReadUint8LE(&inputStream);
|
||||
receivedMsgClass = sbgStreamBufferReadUint8LE(&inputStream);
|
||||
|
||||
//
|
||||
// Read the payload size
|
||||
//
|
||||
payloadSize = (uint16_t)sbgStreamBufferReadUint16LE(&inputStream);
|
||||
|
||||
//
|
||||
// Check that the payload size is valid
|
||||
//
|
||||
if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE)
|
||||
{
|
||||
//
|
||||
// Check if we have received the whole frame
|
||||
//
|
||||
if (pHandle->rxBufferSize < payloadSize+9)
|
||||
{
|
||||
//
|
||||
// Don't have received the whole frame
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
//
|
||||
// We should have the whole frame so for now, skip the payload part but before store the current cursor
|
||||
//
|
||||
payloadOffset = sbgStreamBufferTell(&inputStream);
|
||||
sbgStreamBufferSeek(&inputStream, payloadSize, SB_SEEK_CUR_INC);
|
||||
|
||||
//
|
||||
// Read the frame CRC
|
||||
//
|
||||
frameCrc = sbgStreamBufferReadUint16LE(&inputStream);
|
||||
|
||||
//
|
||||
// Read and test the frame ETX
|
||||
//
|
||||
if (sbgStreamBufferReadUint8LE(&inputStream) == SBG_ECOM_ETX)//
|
||||
{
|
||||
//
|
||||
// Go back at the beginning of the payload part
|
||||
//
|
||||
sbgStreamBufferSeek(&inputStream, payloadOffset, SB_SEEK_SET);
|
||||
|
||||
//
|
||||
// We have a frame so return the received command if needed even if the CRC is still not validated
|
||||
//
|
||||
if (pMsg)
|
||||
{
|
||||
*pMsg = receivedMsg;
|
||||
}
|
||||
if (pMsgClass)
|
||||
{
|
||||
*pMsgClass = receivedMsgClass;
|
||||
}
|
||||
|
||||
//
|
||||
// Compute the CRC of the received frame (Skip SYNC 1 and SYNC 2 chars)
|
||||
//
|
||||
computedCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&inputStream)) + 2, payloadSize + 4);//??????????????????????????????????????????????????????????????????
|
||||
|
||||
//
|
||||
// Check if the received frame has a valid CRC
|
||||
//
|
||||
if (frameCrc == computedCrc)//frameCrc == computedCrc??????????????????????????????????????????????????????????????????????????????????????????????????????
|
||||
{
|
||||
//
|
||||
// Extract the payload if needed
|
||||
//
|
||||
if (payloadSize > 0)
|
||||
{
|
||||
//
|
||||
// Check if input parameters are valid
|
||||
//
|
||||
if ( (pData) && (pSize) )
|
||||
{
|
||||
//
|
||||
// Check if we have enough space to store the payload
|
||||
//
|
||||
if (payloadSize <= maxSize)
|
||||
{
|
||||
//
|
||||
// Copy the payload and return the payload size
|
||||
//
|
||||
*pSize = payloadSize;
|
||||
memcpy(pData, sbgStreamBufferGetCursor(&inputStream), payloadSize);
|
||||
errorCode = SBG_NO_ERROR;
|
||||
|
||||
//std::cout<<"receive valid frame!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Not enough space to store the payload, we will just drop the received data
|
||||
//
|
||||
errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
errorCode = SBG_NULL_POINTER;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// No payload but the frame has been read successfully
|
||||
//
|
||||
errorCode = SBG_NO_ERROR;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have an invalid frame CRC and we will directly return this error
|
||||
//
|
||||
errorCode = SBG_INVALID_CRC;
|
||||
}
|
||||
|
||||
//
|
||||
// We have read a whole valid frame so remove it from the buffer
|
||||
// First, test if the reception buffer contains more than just the current frame
|
||||
//
|
||||
if (pHandle->rxBufferSize > payloadSize+9)
|
||||
{
|
||||
//
|
||||
// We remove the read frame but we keep the remaining data
|
||||
//
|
||||
pHandle->rxBufferSize = pHandle->rxBufferSize-(payloadSize+9);
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+payloadSize+9, pHandle->rxBufferSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have parsed the whole received buffer so just empty it
|
||||
//
|
||||
pHandle->rxBufferSize = 0;
|
||||
}
|
||||
|
||||
//
|
||||
// We have at least found a complete frame
|
||||
//
|
||||
return errorCode;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// The ETX char hasn't been found where it should be
|
||||
//
|
||||
errorCode = SBG_INVALID_FRAME;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// The payload size isn't valid
|
||||
//
|
||||
errorCode = SBG_INVALID_FRAME;
|
||||
}
|
||||
|
||||
//
|
||||
// Frame size invalid or the found frame is invalid so we should have incorrectly detected a start of frame.
|
||||
// Remove the SYNC 1 and SYNC 2 chars to retry to find a new frame
|
||||
//
|
||||
pHandle->rxBufferSize -= 2;
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+2, pHandle->rxBufferSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Unable to find a valid start of frame so check if the last byte is a SYNC char in order to keep it for next time
|
||||
//
|
||||
if (pHandle->rxBuffer[pHandle->rxBufferSize-1] == SBG_ECOM_SYNC_1)
|
||||
{
|
||||
//
|
||||
// Report the SYNC char and discard all other bytes in the buffer
|
||||
//
|
||||
pHandle->rxBuffer[0] = SBG_ECOM_SYNC_1;
|
||||
pHandle->rxBufferSize = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Discard the whole buffer
|
||||
//
|
||||
pHandle->rxBufferSize = 0;
|
||||
}
|
||||
|
||||
//
|
||||
// Unable to find a frame
|
||||
//
|
||||
return SBG_NOT_READY;//？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// The whole buffer has been paresed and no valid frame has been found
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
|
||||
sbgtc::SbgErrorCode sbgtc::SbgRecorder::sbgBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData)
|
||||
{
|
||||
SbgErrorCode errorCode = SBG_NO_ERROR;
|
||||
SbgStreamBuffer inputStream;
|
||||
|
||||
// assert(pPayload);
|
||||
// assert(payloadSize > 0);
|
||||
// assert(pOutputData);
|
||||
|
||||
//
|
||||
// Create an input stream buffer that points to the frame payload so we can easily parse it's content
|
||||
//
|
||||
sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize);
|
||||
|
||||
//
|
||||
// Handle the different classes of messages differently
|
||||
//
|
||||
if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0)
|
||||
{
|
||||
//
|
||||
// Parse the incoming log according to its type
|
||||
//
|
||||
switch (msg)
|
||||
{
|
||||
case SBG_ECOM_LOG_STATUS:
|
||||
// errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_IMU_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_IMU_SHORT:
|
||||
// errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_EULER:
|
||||
errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_QUAT:
|
||||
// errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_NAV:
|
||||
// errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_SHIP_MOTION:
|
||||
case SBG_ECOM_LOG_SHIP_MOTION_HP:
|
||||
// errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_ODO_VEL:
|
||||
// errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_UTC_TIME:
|
||||
errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_VEL:
|
||||
case SBG_ECOM_LOG_GPS2_VEL:
|
||||
// errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_POS:
|
||||
case SBG_ECOM_LOG_GPS2_POS:
|
||||
errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_HDT:
|
||||
case SBG_ECOM_LOG_GPS2_HDT:
|
||||
// errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_RAW:
|
||||
case SBG_ECOM_LOG_GPS2_RAW:
|
||||
// errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_MAG:
|
||||
// errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_MAG_CALIB:
|
||||
// errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DVL_BOTTOM_TRACK:
|
||||
// errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DVL_WATER_TRACK:
|
||||
// errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_AIR_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_USBL:
|
||||
// errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DEPTH:
|
||||
// errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EVENT_A:
|
||||
case SBG_ECOM_LOG_EVENT_B:
|
||||
case SBG_ECOM_LOG_EVENT_C:
|
||||
case SBG_ECOM_LOG_EVENT_D:
|
||||
case SBG_ECOM_LOG_EVENT_E:
|
||||
case SBG_ECOM_LOG_EVENT_OUT_A:
|
||||
case SBG_ECOM_LOG_EVENT_OUT_B:
|
||||
// errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DIAG:
|
||||
// errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = SBG_ERROR;
|
||||
}
|
||||
}
|
||||
// else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1)
|
||||
// {
|
||||
// //
|
||||
// // Parse the message depending on the message ID
|
||||
// //
|
||||
// switch ((SbgEComLog1)msg)
|
||||
// {
|
||||
// case SBG_ECOM_LOG_FAST_IMU_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData);
|
||||
// break;
|
||||
// default:
|
||||
// errorCode = SBG_ERROR;
|
||||
// }
|
||||
// }
|
||||
else
|
||||
{
|
||||
//
|
||||
// Un-handled message class
|
||||
//
|
||||
errorCode = SBG_ERROR;
|
||||
}
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
|
||||
void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
|
||||
{
|
||||
rawBuffer pHandleTmp=initRawBuffer(sbgMessage);
|
||||
rawBuffer * pHandle=&pHandleTmp;
|
||||
|
||||
uint8_t receivedMsgClass;
|
||||
uint8_t receivedMsg;
|
||||
size_t payloadSize;
|
||||
uint8_t pData[SBG_ECOM_MAX_PAYLOAD_SIZE]={0};//payloadData
|
||||
|
||||
|
||||
//重要的第1步：提取有效帧
|
||||
SbgErrorCode errorCode = extractOneValidFrame(pHandle,&receivedMsgClass,&receivedMsg,pData,&payloadSize,sizeof(pData));
|
||||
|
||||
|
||||
|
||||
//SBG_INVALID_CRC,SBG_NO_ERROR,SBG_NOT_READY
|
||||
if(errorCode==SBG_NOT_READY)
|
||||
return;
|
||||
if(errorCode==SBG_INVALID_CRC)
|
||||
std::cout<<"SBG_INVALID_CRC: ------------------------------------------------------"<<std::endl;
|
||||
|
||||
|
||||
//重要的第2步：解析有效帧
|
||||
SbgBinaryLogData logData;
|
||||
memset(&logData,0,sizeof(logData));
|
||||
sbgBinaryLogParse((SbgEComClass)receivedMsgClass,(SbgEComMsgId)receivedMsg,pData,payloadSize,&logData);
|
||||
|
||||
|
||||
//判断模式是否为: NAV_POSITION
|
||||
if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
|
||||
{
|
||||
m_iSolutionModeCounter++;//
|
||||
|
||||
uint32_t status=logData.ekfEulerData.status;
|
||||
uint32_t mode=status>>24;//?????????????????????????????????????
|
||||
// uint32_t mode=status;//这是错的
|
||||
|
||||
//一秒钟发射一次mode
|
||||
if(m_iSolutionModeCounter%200 == 0)
|
||||
{
|
||||
emit sbgSolutionModeSignal(mode);
|
||||
|
||||
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;
|
||||
|
||||
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)
|
||||
{
|
||||
// std::cout<<"纬度精度为："<<logData.gpsPosData.latitudeAccuracy<<std::endl;
|
||||
|
||||
float maximal=0;
|
||||
float latitudeAccuracy=logData.gpsPosData.latitudeAccuracy;
|
||||
float longitudeAccuracy=logData.gpsPosData.longitudeAccuracy;
|
||||
float altitudeAccuracy=logData.gpsPosData.altitudeAccuracy;
|
||||
|
||||
|
||||
if(latitudeAccuracy<longitudeAccuracy)
|
||||
maximal = longitudeAccuracy;
|
||||
else
|
||||
maximal = latitudeAccuracy;
|
||||
|
||||
if(maximal<altitudeAccuracy)
|
||||
maximal = altitudeAccuracy;
|
||||
|
||||
emit sbgAccuracySignal(static_cast<int>(maximal));
|
||||
|
||||
if(maximal<7)
|
||||
{
|
||||
m_bIsAccuracyLessThan7=true;
|
||||
}
|
||||
}
|
||||
else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME && m_bIsAccuracyLessThan7 && !m_bIsSbgReady)
|
||||
{
|
||||
//std::cout<<"0pen time:"<<(long)logData.utcData.timeStamp/1000000<<std::endl;
|
||||
|
||||
uint16_t year = logData.utcData.year;
|
||||
int month = logData.utcData.month;
|
||||
int day = logData.utcData.day;
|
||||
int hour = logData.utcData.hour;
|
||||
int minute = logData.utcData.minute;
|
||||
int second = logData.utcData.second;
|
||||
// int32_t nanoSecond = logData.utcData.nanoSecond;
|
||||
// std::cout<<"gps时间为："<<year<<","<<month<<","<<day<<","<<hour<<","<<minute<<","<<second<<","<<nanoSecond<<std::endl;
|
||||
|
||||
char setGpsTimeCommand[256];
|
||||
sprintf(setGpsTimeCommand,"date --set=\"%d%02d%02d %02d:%02d:%02d\"",year,month,day,hour,minute,second);//02中的2代表2位数字，0代表以0补全
|
||||
|
||||
system(setGpsTimeCommand);//
|
||||
|
||||
m_bIsSyncSystemTimeBaseGpstime=true;
|
||||
}
|
||||
else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
|
||||
{
|
||||
// std::cout<<"1111111111111"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
// std::cout<<"receivedMsg:"<<(int)receivedMsg<<std::endl;
|
||||
}
|
||||
else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_QUAT)
|
||||
{
|
||||
//std::cout<<"1111111111111------"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
}
|
||||
else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_NAV)
|
||||
{
|
||||
//std::cout<<"0000000000000"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//控制开始采集高光谱影像,m_bIsNAV_POSITION_MODE &&
|
||||
if(m_bIsRecordHyperspecatralImage && m_bIsSyncSystemTimeBaseGpstime && receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME)
|
||||
{
|
||||
m_baseFileName=getFileNameBaseOnTime();
|
||||
emit sbgReady(calculateTimeDifferenceBetweenSystemAndSbg(logData),m_baseFileName);
|
||||
}
|
||||
}
|
||||
|
||||
double sbgtc::SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg(SbgBinaryLogData logData)//
|
||||
{
|
||||
std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)logData.utcData.second<<","<<(int)logData.utcData.nanoSecond<<std::endl;
|
||||
|
||||
//获取sbg(gps)时间（纳秒）
|
||||
double year=logData.utcData.year;
|
||||
double month=logData.utcData.month;
|
||||
double day=logData.utcData.day;
|
||||
|
||||
double hour=logData.utcData.hour;//+8
|
||||
double minute=logData.utcData.minute;
|
||||
double second=logData.utcData.second;
|
||||
double nanoSecond=logData.utcData.nanoSecond;
|
||||
|
||||
double timeTemp=day*24*60*60+hour*60*60+minute*60+second;//(day-1)
|
||||
double time=timeTemp+nanoSecond/1000000000;
|
||||
|
||||
printf("SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg--secocnd: %f\n", time);
|
||||
//std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg: "<< time <<std::endl;
|
||||
|
||||
|
||||
//获取系统时间（纳秒）
|
||||
struct timespec systemTime;
|
||||
clock_gettime(CLOCK_REALTIME,&systemTime);
|
||||
tm systemTime_rili;
|
||||
localtime_r(&systemTime.tv_sec, &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;
|
||||
|
||||
|
||||
//计算系统时间和gps时间之间的差距
|
||||
double TimeDifferenceBetweenSystemAndSbg = nanosecondSystem - time;
|
||||
|
||||
return TimeDifferenceBetweenSystemAndSbg;
|
||||
}
|
||||
|
||||
int sbgtc::SbgRecorder::getSbgState() const
|
||||
{
|
||||
return m_iSbgState;
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::startRecordSbg()
|
||||
{
|
||||
openSerialPort();
|
||||
if(m_iSbgState!=2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_iSbgState=3;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
|
||||
m_baseFileName=getFileNameBaseOnTime();
|
||||
|
||||
QString sbgFileName=m_baseFileName+".sbg";
|
||||
|
||||
FILE * fileHandle=fopen(sbgFileName.toStdString().c_str(),"w+b");
|
||||
|
||||
QByteArray requestData;
|
||||
m_bRecordControl=true;
|
||||
while (m_bRecordControl)
|
||||
{
|
||||
//std::cout<<"SbgRecorder::startRecordSbg--------------:"<<std::endl;
|
||||
if(m_serial->waitForReadyRead())
|
||||
{
|
||||
//requestData.resize(m_serial->size());
|
||||
requestData = m_serial->readAll();
|
||||
fwrite(requestData.data(),requestData.size(),1,fileHandle);//????????????
|
||||
|
||||
// if(!m_bIsSbgReady)
|
||||
// {
|
||||
// parseSbgMessage(&requestData);
|
||||
// }
|
||||
parseSbgMessage(&requestData);//边采集惯导数据边解析，并不会导致惯导漏帧
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"SbgRecorder::startRecordSbg----:Wait write response timeout"<<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
m_bIsSbgReady=false;//确保每次执行函数SbgRecorder::startRecordSbg,都会执行parseSbgMessage(&requestData);
|
||||
|
||||
fclose(fileHandle);
|
||||
|
||||
m_iSbgState=2;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::stopRecordSbg()
|
||||
{
|
||||
m_bRecordControl=false;
|
||||
}
|
||||
|
||||
void SbgRecorder::startRecordHyperspectral()
|
||||
{
|
||||
m_bIsRecordHyperspecatralImage=true;
|
||||
std::cout<<"SbgRecorder::startRecordHyperspectral----m_bIsRecordHyperspecatralImage设置为true!"<<std::endl;
|
||||
}
|
||||
|
||||
void SbgRecorder::stopRecordHyperspectral()
|
||||
{
|
||||
m_bIsRecordHyperspecatralImage=false;
|
||||
}
|
||||
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);
|
||||
}
|
||||
71
Source_Files/utility_tc.cpp
Normal file
71
Source_Files/utility_tc.cpp
Normal file
@ -0,0 +1,71 @@
|
||||
#include "Header_Files/utility_tc.h"
|
||||
#include <QDir>
|
||||
|
||||
QString getFileNameBaseOnTime()
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
//获取系统时间
|
||||
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));
|
||||
|
||||
//格式化时间为需要的格式
|
||||
char fileNameTmp[256] = { 0 };
|
||||
char dirNameTmp[256] = { 0 };
|
||||
strftime(fileNameTmp, sizeof(fileNameTmp), "%Y%m%d_%H%M%S", tblock);
|
||||
strftime(dirNameTmp, sizeof(dirNameTmp), "%Y%m%d", tblock);
|
||||
QString fileName(fileNameTmp);
|
||||
QString dirName(dirNameTmp);
|
||||
|
||||
// QString fileName=QString::number(tblock->tm_year+1900)+QString::number(tblock->tm_mon+1)+QString::number(tblock->tm_mday)+"_"+QString::number(tblock->tm_hour)+QString::number(tblock->tm_min);//
|
||||
|
||||
//设置文件绝对路径
|
||||
QDir path("/media/nvme");
|
||||
if(!path.exists(dirName))
|
||||
{
|
||||
path.mkdir(dirName);
|
||||
}
|
||||
path.cd(dirName);
|
||||
QString absoluteFilePath=path.path()+"/"+fileName;
|
||||
// printf("absoluteFilePath is: %s\n", absoluteFilePath.c_str());
|
||||
// qDebug() << "absoluteFilePath is:" << absoluteFilePath;
|
||||
|
||||
return absoluteFilePath;
|
||||
}
|
||||
|
||||
void bubbleSort(unsigned short * a, int n)
|
||||
{
|
||||
// //排序实现１
|
||||
// for(int i=0;i<numberOfPixel;i++)
|
||||
// for(int j=1;j<numberOfPixel-i;j++)
|
||||
// if(a[j-1]>a[j])
|
||||
// swap(a+j-1,a+j);
|
||||
|
||||
//排序实现2
|
||||
int j,k,flag;
|
||||
|
||||
flag = n;
|
||||
while(flag>0)
|
||||
{
|
||||
k=flag;
|
||||
flag=0;
|
||||
for(j=1;j<k;j++)
|
||||
if(a[j-1]>a[j])
|
||||
{
|
||||
swap(a+j-1,a+j);
|
||||
flag=j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void swap(unsigned short * a, unsigned short * b)
|
||||
{
|
||||
int tmp;
|
||||
|
||||
tmp=*a;
|
||||
*a=*b;
|
||||
*b=tmp;
|
||||
}
|
||||
578
Source_Files/ximeaimager.cpp
Normal file
578
Source_Files/ximeaimager.cpp
Normal file
@ -0,0 +1,578 @@
|
||||
#include "Header_Files/ximeaimager.h"
|
||||
|
||||
XimeaImager::XimeaImager()
|
||||
{
|
||||
m_buffer=nullptr;
|
||||
m_bRecordControl=false;
|
||||
m_iFrameCounter=0;
|
||||
m_iImagerState=0;
|
||||
|
||||
//connect(this, SIGNAL(recordFinished()),this, SLOT());
|
||||
}
|
||||
|
||||
void XimeaImager::openImger()
|
||||
{
|
||||
if(m_iImagerState>0)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
//std::cout<<"XimeaImager::openImger111111111111111111111:正在打开相机!"<<std::endl;
|
||||
m_imager.connect();
|
||||
|
||||
// bool haha = m_imager.setSpectralBin(2);
|
||||
// bool haha2 = m_imager.setSpatialBin(2);
|
||||
|
||||
//sn008_bin1
|
||||
float a=1.999564;
|
||||
float b=-279.893;
|
||||
// //sn008_bin2
|
||||
// float a = 3.99912794;
|
||||
// float b = -278.89317732225084;
|
||||
|
||||
m_imager.setGainOffset(a, b);
|
||||
|
||||
|
||||
//SN=008自己修改
|
||||
// int OffsetX=272;
|
||||
// int width=1392;
|
||||
//
|
||||
// int OffsetY=338;
|
||||
// int height=302;
|
||||
//
|
||||
// //SN=008标准
|
||||
// int OffsetX=288;//272
|
||||
// int width=1360;//是16的整数倍
|
||||
//
|
||||
// int OffsetY=340;
|
||||
// int height=300;//302
|
||||
|
||||
|
||||
// m_imager.setRoi(288,1360,348,300);//sn0098
|
||||
m_imager.setRoi(288,1360,340,300);//sn008,corning410，bin1
|
||||
// m_imager.setRoi(144,680,170,151);//sn008,corning410，bin2
|
||||
|
||||
setFramerate(100);
|
||||
|
||||
int frameSizeManual = m_imager.get_band_count()*m_imager.get_sample_count()*2;
|
||||
int frameSizeAuto = m_imager.getBufferSizeOfOneFrame();
|
||||
|
||||
m_iFrameSizeInByte = frameSizeAuto;
|
||||
|
||||
m_buffer = new unsigned short[m_iFrameSizeInByte];
|
||||
|
||||
m_iImagerState = 1;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
std::cout<<"XimeaImager::openImger----------------:ERROR!"<<std::endl;
|
||||
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
catch(char const* e1)
|
||||
{
|
||||
std::cout<<"char *e---------!"<<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::closeImger()
|
||||
{
|
||||
if(m_iImagerState==0)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
m_imager.disconnect();
|
||||
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
std::cout<<"XimeaImager::closeImger-------------------!"<<std::endl;
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::setFramerate(double framerate)
|
||||
{
|
||||
try
|
||||
{
|
||||
m_imager.set_framerate(framerate);
|
||||
|
||||
int maxExposureTimeInUs=1/framerate*1000000*0.8;
|
||||
setExposureTime(maxExposureTimeInUs);
|
||||
// setExposureTime(1000);
|
||||
|
||||
m_iImagerState=2;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
double XimeaImager::getExposureTime()
|
||||
{
|
||||
double exposureTime;
|
||||
try
|
||||
{
|
||||
exposureTime=m_imager.get_integration_time();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return exposureTime;
|
||||
}
|
||||
|
||||
double XimeaImager::setExposureTime(float exposureTime_in_us)
|
||||
{
|
||||
double integrationTime2Return;
|
||||
|
||||
try
|
||||
{
|
||||
//计算最大积分时间
|
||||
float currentFramerate=getFramerate();
|
||||
float maxExposureTime_in_us=1/currentFramerate*1000000;
|
||||
|
||||
//确保设置的积分时间比最大积分时间小
|
||||
if(exposureTime_in_us<maxExposureTime_in_us)
|
||||
{
|
||||
|
||||
m_imager.set_integration_time(exposureTime_in_us);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
m_imager.set_integration_time(maxExposureTime_in_us);
|
||||
}
|
||||
|
||||
//返回设置的积分时间
|
||||
integrationTime2Return=m_imager.get_integration_time();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
|
||||
return integrationTime2Return;
|
||||
}
|
||||
|
||||
double XimeaImager::autoExposure()
|
||||
{
|
||||
double exposureTime;
|
||||
try
|
||||
{
|
||||
//方式１：在曝光时的帧率前提下，从最大曝光时间开始循环采集
|
||||
int maxValueOfOneFrame;
|
||||
float suitableMaxValue=4095 * 0.8;
|
||||
|
||||
double framerate = m_imager.get_framerate();
|
||||
double maxExposureTime = 1/framerate*1000000;
|
||||
exposureTime=setExposureTime(maxExposureTime);
|
||||
|
||||
bool bIsAutoExposureOk=false;
|
||||
while(!bIsAutoExposureOk)
|
||||
{
|
||||
m_imager.start();
|
||||
m_imager.get_frame(m_buffer);
|
||||
m_imager.stop();
|
||||
|
||||
maxValueOfOneFrame=getMaxValueOfOneFrame(m_buffer,m_imager.get_band_count()*m_imager.get_sample_count());
|
||||
|
||||
if(maxValueOfOneFrame <= suitableMaxValue)
|
||||
{
|
||||
bIsAutoExposureOk=true;
|
||||
}
|
||||
else
|
||||
{
|
||||
exposureTime = exposureTime*0.95;
|
||||
exposureTime=setExposureTime(exposureTime);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// //方式2：
|
||||
// int baseExposureTime_in_us=5000;
|
||||
// float suitableMaxValue=4095 * 0.8;
|
||||
// int maxValueOfOneFrame;
|
||||
|
||||
// setExposureTime(baseExposureTime_in_us);
|
||||
|
||||
// m_imager.start();
|
||||
// m_imager.get_frame(m_buffer);
|
||||
// m_imager.stop();
|
||||
|
||||
// std::cout<<"1111111111111111111111111111111111!"<<std::endl;
|
||||
// maxValueOfOneFrame=getMaxValueOfOneFrame(m_buffer,m_imager.get_band_count()*m_imager.get_sample_count());
|
||||
// std::cout<<"2222222222222222222222222222222222!"<<std::endl;
|
||||
|
||||
// float scale=suitableMaxValue/maxValueOfOneFrame;
|
||||
// float suitableExposureTime=baseExposureTime_in_us * scale;
|
||||
// exposureTime=setExposureTime(suitableExposureTime);
|
||||
|
||||
m_iImagerState=3;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
std::cout<<"自动曝光完成!"<<std::endl;
|
||||
return exposureTime;
|
||||
}
|
||||
|
||||
double XimeaImager::getFramerate()
|
||||
{
|
||||
double framerate;
|
||||
|
||||
try
|
||||
{
|
||||
framerate=m_imager.get_framerate();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return framerate;
|
||||
}
|
||||
|
||||
void XimeaImager::setGain(double gain)
|
||||
{
|
||||
try
|
||||
{
|
||||
m_imager.set_gain(gain);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
double XimeaImager::getGain()
|
||||
{
|
||||
try
|
||||
{
|
||||
return m_imager.get_gain();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
int XimeaImager::getSampleCount()
|
||||
{
|
||||
int sampleCount;
|
||||
|
||||
try
|
||||
{
|
||||
sampleCount=m_imager.get_sample_count();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return sampleCount;
|
||||
}
|
||||
|
||||
int XimeaImager::getBandCount()
|
||||
{
|
||||
int bandCount;
|
||||
|
||||
try
|
||||
{
|
||||
bandCount=m_imager.get_band_count();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return bandCount;
|
||||
}
|
||||
|
||||
int XimeaImager::getWindowStartBand()
|
||||
{
|
||||
int windowStartBand;
|
||||
|
||||
try
|
||||
{
|
||||
windowStartBand=m_imager.get_start_band();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return windowStartBand;
|
||||
}
|
||||
|
||||
int XimeaImager::getWindowEndBand()
|
||||
{
|
||||
int windowEndBand;
|
||||
|
||||
try
|
||||
{
|
||||
windowEndBand=m_imager.get_end_band();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return windowEndBand;
|
||||
}
|
||||
|
||||
double XimeaImager::geWavelengthAtBand(int x)
|
||||
{
|
||||
double wavelengthAtBand;
|
||||
|
||||
try
|
||||
{
|
||||
wavelengthAtBand=m_imager.get_wavelength_at_band(x);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return wavelengthAtBand;
|
||||
}
|
||||
|
||||
int XimeaImager::getMaxValueOfOneFrame(unsigned short * data, int numberOfPixel)
|
||||
{
|
||||
//排序
|
||||
//bubbleSort(data,1000);
|
||||
|
||||
|
||||
//计算出最大的10%值的平均值
|
||||
unsigned short maxValue=0;
|
||||
for(int i=0;i<numberOfPixel;i++)
|
||||
{
|
||||
if (data[i]>maxValue)
|
||||
{
|
||||
maxValue=data[i];
|
||||
}
|
||||
}
|
||||
printf("本帧最大值为: %d.\n",maxValue);
|
||||
|
||||
return maxValue;
|
||||
}
|
||||
|
||||
int XimeaImager::getImagerState() const
|
||||
{
|
||||
return m_iImagerState;
|
||||
}
|
||||
|
||||
void XimeaImager::startRecord(double TimeDifferenceBetweensOSAndSbg,QString baseFileName)
|
||||
{
|
||||
try
|
||||
{
|
||||
if(m_iImagerState==4 || m_iImagerState==0 || m_iImagerState>=21)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
m_iImagerStateTemp=m_iImagerState;
|
||||
|
||||
m_iImagerState=4;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
printf("开始采集!\n");
|
||||
m_iFrameCounter=0;
|
||||
m_bRecordControl=true;
|
||||
|
||||
m_imager.start();
|
||||
|
||||
m_baseFileName=baseFileName;
|
||||
|
||||
QString imageFileName=m_baseFileName+".bil";
|
||||
QString timesFileName=m_baseFileName+".times";
|
||||
|
||||
//
|
||||
FILE *hFile=fopen(imageFileName.toStdString().c_str(),"w+b");
|
||||
FILE *hHimesFile=fopen(timesFileName.toStdString().c_str(),"w+");
|
||||
|
||||
using namespace std;
|
||||
ofstream timesFileHandle(timesFileName.toStdString()+"_ofstream");
|
||||
|
||||
|
||||
struct timeval timeStart, timeEnd;
|
||||
double runTime=0;
|
||||
gettimeofday(&timeStart, NULL);
|
||||
|
||||
while (m_bRecordControl)
|
||||
{
|
||||
unsigned short *x=m_imager.get_frame(m_buffer);
|
||||
|
||||
//fwrite(m_buffer,2,getBandCount()*getSampleCount(),hFile);
|
||||
fwrite(m_buffer,1,m_iFrameSizeInByte,hFile);//*********************************
|
||||
//fflush(hFile);//只保证了将IO缓冲写入系统缓冲中，使IO读操作能成功，但系统什么时候写入磁盘，由系统决定，一般是达到一定量时系统他就写入磁盘。
|
||||
//sync();//强制系统将系统文件缓冲的内容写入磁盘
|
||||
|
||||
m_iFrameCounter+=1;
|
||||
|
||||
|
||||
double sbgTime=getSbgTime(TimeDifferenceBetweensOSAndSbg);
|
||||
|
||||
fprintf(hHimesFile,"%f\n",sbgTime);
|
||||
//fwrite(&sbgTime,sizeof(double),1,hHimesFile);
|
||||
|
||||
timesFileHandle << sbgTime << "\n";
|
||||
|
||||
// std::cout<<"XimeaImager::startRecord---std::cout: "<<sbgTime<<std::endl;
|
||||
|
||||
// //用于测试是否漏帧
|
||||
// if(m_iFrameCounter==getFramerate()*20)
|
||||
// {
|
||||
// break;
|
||||
// }
|
||||
|
||||
// unsigned char pixel = *(unsigned char*)image.bp;//Default value: XI_MONO8
|
||||
// unsigned short pixel =*(unsigned short*)image.bp;//XI_RAW16
|
||||
// printf("Image %d (%dx%d) received from camera. First pixel value: %d\n", m_iFrameCounter, (int)image.width, (int)image.height, pixel);
|
||||
}
|
||||
|
||||
gettimeofday(&timeEnd, NULL);
|
||||
runTime = (timeEnd.tv_sec - timeStart.tv_sec ) + (double)(timeEnd.tv_usec -timeStart.tv_usec)/1000000;
|
||||
|
||||
double frameInTheory=runTime * getFramerate();
|
||||
double frameLossed=frameInTheory - m_iFrameCounter;
|
||||
double frameLossRate=frameLossed / frameInTheory;
|
||||
|
||||
std::cout<<"当前采集文件为: "<<baseFileName.toStdString()<<std::endl;
|
||||
std::cout<<"采集时间为: "<<runTime<<std::endl;
|
||||
std::cout<<"理论采集帧数为: "<<frameInTheory<<std::endl;
|
||||
std::cout<<"实际采集帧数为："<<m_iFrameCounter<<std::endl;
|
||||
std::cout<<"丢失帧数为: "<<frameLossed<<std::endl;
|
||||
std::cout<<"丢帧率为: "<<frameLossRate<<std::endl;
|
||||
|
||||
fclose(hFile);
|
||||
fclose(hHimesFile);
|
||||
timesFileHandle.close();
|
||||
|
||||
printf("Stopping acquisition...\n");
|
||||
m_imager.stop();
|
||||
|
||||
writeHdr();
|
||||
|
||||
m_iImagerState=m_iImagerStateTemp;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
emit recordFinished();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::writeHdr()
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
QString hdrPath=m_baseFileName+".hdr";
|
||||
|
||||
ofstream hdrFileHandle(hdrPath.toStdString());
|
||||
hdrFileHandle << "ENVI\n";
|
||||
hdrFileHandle << "interleave = bil\n";
|
||||
hdrFileHandle << "byte order = 0\n";
|
||||
hdrFileHandle << "data type = 2\n";
|
||||
//hdrFileHandle << "bit depth = 12\n";
|
||||
|
||||
hdrFileHandle << "samples = " << getSampleCount() << "\n";
|
||||
hdrFileHandle << "bands = " << getBandCount() << "\n";
|
||||
hdrFileHandle << "lines = " << m_iFrameCounter << "\n";
|
||||
hdrFileHandle << "sample binning = " << m_imager.getSpatialBin() << "\n";
|
||||
hdrFileHandle << "spectral binning = " << m_imager.getSpectralBin() << "\n";
|
||||
|
||||
hdrFileHandle << "framerate = " << getFramerate() << "\n";
|
||||
hdrFileHandle << "shutter = " << getExposureTime()/1000 << "\n";
|
||||
hdrFileHandle << "shutter units = milliseconds\n";
|
||||
hdrFileHandle << "gain = " << getGain() << "\n";
|
||||
|
||||
hdrFileHandle << "wavelength units = nanometers\n";
|
||||
hdrFileHandle << "wavelength = {";
|
||||
//hdrFileHandle << std::setprecision(5);
|
||||
for (int i = getWindowStartBand(); i < getWindowEndBand(); i++)
|
||||
{
|
||||
hdrFileHandle << geWavelengthAtBand(i);
|
||||
if (i < getWindowEndBand() - 1)
|
||||
hdrFileHandle << ", ";
|
||||
else
|
||||
{
|
||||
printf("头文件中写入了多少个波段:%d\n",i-getWindowStartBand()+1);//???????????????
|
||||
}
|
||||
}
|
||||
hdrFileHandle << "}\n";
|
||||
hdrFileHandle.close();
|
||||
}
|
||||
|
||||
void XimeaImager::processXiApiErrorCodes(int xiApiErrorCodes)
|
||||
{
|
||||
using namespace std;
|
||||
switch (xiApiErrorCodes)
|
||||
{
|
||||
case 1:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:Invalid handle!"<<std::endl;
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
case 56:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:NO_DEVICES_FOUND!"<<std::endl;
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
case 57:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:RESOURCE_OR_FUNCTION_LOCKED!"<<std::endl;
|
||||
m_iImagerState=22;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
default:
|
||||
QString ximeaError="ximeaError.txt";
|
||||
ofstream ximeaErrorFile(ximeaError.toStdString().c_str(),ios::app);
|
||||
|
||||
ximeaErrorFile<< xiApiErrorCodes << "\n";
|
||||
|
||||
ximeaErrorFile.close();
|
||||
|
||||
|
||||
|
||||
m_iImagerState=21;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::stopRecord()
|
||||
{
|
||||
//printf("Stop record!\n");
|
||||
m_bRecordControl=false;
|
||||
}
|
||||
|
||||
int XimeaImager::getFrameCounter()
|
||||
{
|
||||
return m_iFrameCounter;
|
||||
}
|
||||
Reference in New Issue
Block a user