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

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

Browse Source
This commit is contained in:
tangchao0503
2022-06-13 12:01:30 +08:00
commit 1452bcc2b9
21 changed files with 4316 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
.idea
build

53
CMakeLists.txt Normal file
View File

@ -0,0 +1,53 @@
cmake_minimum_required(VERSION 3.16)
project(ximeaImageRecorder)
set(CMAKE_CXX_STANDARD 14)
SET(CMAKE_INSTALL_PREFIX < /home/pi/bin >)
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
set(CMAKE_AUTORCC ON)
set(CMAKE_CXX_STANDARD 11)
set(QT Core Network SerialPort)
set(TEMPLATE app)
set(TARGET ximeaImageRecorder)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
find_package(Qt5 REQUIRED ${QT})#
include_directories(.)
include_directories(../IrisXimeaImager)
include_directories(../ellipse_n_sdk/SoftwareDevelopment/sbgECom/src)
#include_directories(/home/rock/tc_projects/ellipse_n_sdk/SoftwareDevelopment/sbgECom/src/)
include_directories(../ellipse_n_sdk/SoftwareDevelopment/sbgECom/common)
link_directories(../ellipse_n_sdk/SoftwareDevelopment/sbgECom/bin)
link_libraries(sbgECom)
link_directories(../IrisXimeaImager/build)
add_executable(${CMAKE_PROJECT_NAME}
Source_Files/fileoperation.cpp
Header_Files/fileoperation.h
Source_Files/main.cpp
Header_Files/math_tc.h
Source_Files/sbgbuffer.cpp
Header_Files/sbgbuffer.h
Source_Files/sbgcrc.cpp
Header_Files/sbgcrc.h
Header_Files/sbgerrorcodes.h
Source_Files/sbglogparse.cpp
Header_Files/sbglogparse.h
Source_Files/sbgrecorder.cpp
Header_Files/sbgrecorder.h
Source_Files/udpserver.cpp
Header_Files/udpserver.h
Source_Files/utility_tc.cpp
Header_Files/utility_tc.h
Source_Files/ximeaimager.cpp
Header_Files/ximeaimager.h)
qt5_use_modules(${CMAKE_PROJECT_NAME} ${QT})
target_link_libraries(${CMAKE_PROJECT_NAME}
irisXimeaImager)

35
Header_Files/fileoperation.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef FILEOPERATION_H
#define FILEOPERATION_H
#include <iostream>
#include <QDir>
#include <QFileInfo>
class FileOperation:public QObject
{
Q_OBJECT
public:
FileOperation();
bool copyFileToPath(QString sourceDir ,QString toDir, bool coverFileIfExist);
QStringList getSubfolders(QDir parentFolder);
QString getFlashDrivePath();
QDir getSourcePath();
private:
//0:等待拷贝数据中;1:正在拷贝;2:没有数据可拷贝;3:请插入u盘;
int m_copyFileStatus;
public slots:
void copyFile();
void deleteFile();
signals:
void copyFileStatus(int);
};
#endif // FILEOPERATION_H

136
Header_Files/math_tc.h Normal file
View File

@ -0,0 +1,136 @@
#ifndef MATH_H
#define MATH_H
#include <iostream>
#include <stdexcept>
using namespace std;
template <class T>
void ZZ_MinMax(T num[], int length, int &indexofMax, int &indexofMin)
{
if (num == nullptr || length <= 0)
{
throw invalid_argument("Pay attention to the input array in minmax");
return;
}
int begin = 0;
if (length % 2 == 1)
{
indexofMax = indexofMin = 0;
begin = 1;
}
else
{
if (num[0] < num[1])
{
indexofMax = 1;
indexofMin = 0;
}
else {
indexofMax = 0;
indexofMin = 1;
}
begin = 2;
}
for (int i = begin; i != length; i += 2)
{
if (num[i] < num[i + 1])
{
if (num[indexofMax] < num[i + 1]) indexofMax = i + 1;
if (num[indexofMin] > num[i]) indexofMin = i;
}
else
{
if (num[indexofMax] < num[i]) indexofMax = i;
if (num[indexofMin] > num[i + 1]) indexofMin = i + 1;
}
}
}
template<typename T>
void MinHeapify(T*arry, int size, int element)
{
int lchild = element * 2 + 1, rchild = lchild + 1;//左右子树
while (rchild < size)//子树均在范围内
{
if (arry[element] <= arry[lchild] && arry[element] <= arry[rchild])//如果比左右子树都小，完成整理
{
return;
}
if (arry[lchild] <= arry[rchild])//如果左边最小
{
std::swap(arry[element], arry[lchild]);//把左面的提到上面
element = lchild;//循环时整理子树
}
else//否则右面最小
{
std::swap(arry[element], arry[rchild]);//同理
element = rchild;
}
lchild = element * 2 + 1;
rchild = lchild + 1;//重新计算子树位置
}
if (lchild < size&&arry[lchild] < arry[element])//只有左子树且子树小于自己
{
std::swap(arry[lchild], arry[element]);
}
return;
}
template<typename T>
void MaxHeapify(T*arry, int size, int element)
{
int lchild = element * 2 + 1, rchild = lchild + 1;//左右子树
while (rchild < size)//子树均在范围内
{
if (arry[element] >= arry[lchild] && arry[element] >= arry[rchild])//如果比左右子树都小，完成整理
{
return;
}
if (arry[lchild] >= arry[rchild])//如果左边最小
{
std::swap(arry[element], arry[lchild]);//把左面的提到上面
element = lchild;//循环时整理子树
}
else//否则右面最小
{
std::swap(arry[element], arry[rchild]);//同理
element = rchild;
}
lchild = element * 2 + 1;
rchild = lchild + 1;//重新计算子树位置
}
if (lchild<size&&arry[lchild]>arry[element])//只有左子树且子树小于自己
{
std::swap(arry[lchild], arry[element]);
}
return;
}
template<typename T>
void HeapSort(T*arry, int size)
{
int i;
for (i = size - 1; i >= 0; i--)//从子树开始整理树
{
MinHeapify(arry, size, i);
}
while (size > 0)//拆除树
{
std::swap(arry[size - 1], arry[0]);//将根（最小）与数组最末交换
size--;//树大小减小
MinHeapify(arry, size, 0);//整理树
}
return;
}
#endif // MATH_H

97
Header_Files/sbgbuffer.h Normal file
View File

@ -0,0 +1,97 @@
#ifndef SBGBUFFER_H
#define SBGBUFFER_H
#include "sbgerrorcodes.h"
#include <QByteArray>
namespace sbgtc
{
#define SBG_ECOM_MAX_BUFFER_SIZE (4096) /*!< Maximum reception buffer size in bytes. */
#define SBG_ECOM_MAX_PAYLOAD_SIZE (4086) /*!< Maximum payload size in bytes. */
#define SBG_ECOM_SYNC_1 (0xFF) /*!< First synchronization char of the frame. */
#define SBG_ECOM_SYNC_2 (0x5A) /*!< Second synchronization char of the frame. */
#define SBG_ECOM_ETX (0x33) /*!< End of frame byte. */
/*!
* Windows x86 & x64 support both aligned and unaligned access
*/
#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0)
/*!
* Windows is using little endianess
*/
#define SBG_CONFIG_BIG_ENDIAN (0)
/*!
* Union that allows type punning (access to a floating point number bits)
*/
typedef union _FloatNint
{
float valF;
int32_t valI;
uint32_t valU;
} FloatNint;
/*!
* Union that allows type punning (access to a double number bits)
*/
typedef union _DoubleNint
{
double valF;
uint64_t valU;
int64_t valI;
} DoubleNint;
typedef struct _rawBuffer
{
uint8_t * rxBuffer;
size_t rxBufferSize;
}rawBuffer;
typedef struct _SbgStreamBuffer
{
//SbgSBMode modes; /*!< Defines the stream buffer modes (read/write). */
size_t bufferSize; /*!< Size in bytes of the linked buffer. */
uint8_t *pBufferPtr; /*!< Pointer to the buffer linked with this stream. */
uint8_t *pCurrentPtr; /*!< Current pointer within the buffer. */
SbgErrorCode errorCode; /*!< Current error code on stream buffer. */
} SbgStreamBuffer;
typedef enum _SbgSBSeekOrigin
{
SB_SEEK_SET, /*!< The offset is referenced to the begining of the stream. */
SB_SEEK_CUR_INC, /*!< The offset is referenced to the current cursor position and increment the current cursor. */
SB_SEEK_CUR_DEC, /*!< The offset is referenced to the current cursor position and decrement the current cursor. */
SB_SEEK_END /*!< The offset is referenced to the end of the stream. */
} SbgSBSeekOrigin;
rawBuffer initRawBuffer(QByteArray * sbgMessage);
SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize);
SbgErrorCode sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin);
size_t sbgStreamBufferTell(SbgStreamBuffer *pHandle);
void *sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle);
size_t sbgStreamBufferGetSize(SbgStreamBuffer *pHandle);
size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle);
size_t sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle);
void *sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle);
uint8_t sbgStreamBufferReadUint8LE(SbgStreamBuffer *pHandle);
uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle);
uint32_t sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle);
uint64_t sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle);
int8_t sbgStreamBufferReadInt8LE(SbgStreamBuffer *pHandle);
int32_t sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle);
float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle);
double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle);
SbgErrorCode sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle);
}
#endif // SBGBUFFER_H

182
Header_Files/sbgcrc.h Normal file
View File

@ -0,0 +1,182 @@
#ifndef SBGCRC_H
#define SBGCRC_H
#include <stdint.h>
#include <stddef.h>
namespace sbgtc
{
//----------------------------------------------------------------------//
//- Header (open extern C block) -//
//----------------------------------------------------------------------//
//#include <sbgCommon.h>//???????????????????????????????????????????????????????????????????????????????????
//----------------------------------------------------------------------//
//- Types definitions -//
//----------------------------------------------------------------------//
/*!< Type used to compute a 32 bit Ethernet CRC. */
typedef uint32_t SbgCrc32;
/*!< Type used to compute a 16 bit CRC. */
typedef uint16_t SbgCrc16;
//----------------------------------------------------------------------//
//- 32 bits Ethernet CRC -//
//----------------------------------------------------------------------//
/*!
* Initialize the 32 bit CRC computation system.
* \param[in] pInstance Pointer on an allocated but non initialized Crc32 object.
*/
void sbgCrc32Initialize(SbgCrc32 *pInstance);
/*!
* Compute a 32 bit CRC using an Ethernet polynome.
* Warning: the buffer size should be at least 4 bytes long.
* \param[in] pInstance Read only pointer on a valid Crc32 object.
* \param[in] pData Read only pointer on the data buffer to compute CRC on.
* \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4.
*/
void sbgCrc32Update(SbgCrc32 *pInstance, const void *pData, size_t dataSize);
/*!
* Returns the computed 32 bit CRC value.
* \param[in] pInstance Read only pointer on a valid Crc32 object.
* \return The computed CRC.
*/
uint32_t sbgCrc32Get(const SbgCrc32 *pInstance);
/*!
* Compute a 32 Bit CRC using an Ethernet polynome.
* Warning: the buffer size should be at least 4 bytes long.
* \param[in] pData Read only pointer on the data buffer to compute CRC on.
* \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4.
* \return The computed CRC.
*/
uint32_t sbgCrc32Compute(const void *pData, size_t dataSize);
//----------------------------------------------------------------------//
//- CRC-16 operations -//
//----------------------------------------------------------------------//
/*!
* Initialize the 16 bit CRC computation system.
* \param[in] pInstance Pointer on an allocated but non initialized Crc16 object.
*/
void sbgCrc16Initialize(SbgCrc16 *pInstance);
/*!
* Compute a 16 bit CRC using an the polynome 0x8408.
* \param[in] pInstance Read only pointer on a valid Crc16 object.
* \param[in] pData Read only pointer on the data buffer to compute CRC on.
* \param[in] dataSize Data size in bytes of the buffer.
*/
void sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize);
/*!
* Returns the computed 32 bit CRC value.
* \param[in] pInstance Read only pointer on a valid Crc16 object.
* \return The computed CRC.
*/
uint16_t sbgCrc16Get(const SbgCrc16 *pInstance);
/*!
* Compute a 32 Bit CRC using an the polynome 0x8408.
* \param[in] pData Read only pointer on the data buffer to compute CRC on.
* \param[in] dataSize Data size in bytes of the buffer.
* \return The computed CRC.
*/
uint16_t sbgCrc16Compute(const void *pData, size_t dataSize);
//----------------------------------------------------------------------//
//- Static global CRC tables -//
//----------------------------------------------------------------------//
/*!< CRC table used to compute a 16 bit CRC with the polynom 0x8408. */
static const uint16_t crc16LookupTable[256] = {
0x0000,0x1189,0x2312,0x329B,0x4624,0x57AD,0x6536,0x74BF,0x8C48,0x9DC1,0xAF5A,0xBED3,0xCA6C,0xDBE5,0xE97E,0xF8F7,
0x1081,0x0108,0x3393,0x221A,0x56A5,0x472C,0x75B7,0x643E,0x9CC9,0x8D40,0xBFDB,0xAE52,0xDAED,0xCB64,0xF9FF,0xE876,
0x2102,0x308B,0x0210,0x1399,0x6726,0x76AF,0x4434,0x55BD,0xAD4A,0xBCC3,0x8E58,0x9FD1,0xEB6E,0xFAE7,0xC87C,0xD9F5,
0x3183,0x200A,0x1291,0x0318,0x77A7,0x662E,0x54B5,0x453C,0xBDCB,0xAC42,0x9ED9,0x8F50,0xFBEF,0xEA66,0xD8FD,0xC974,
0x4204,0x538D,0x6116,0x709F,0x0420,0x15A9,0x2732,0x36BB,0xCE4C,0xDFC5,0xED5E,0xFCD7,0x8868,0x99E1,0xAB7A,0xBAF3,
0x5285,0x430C,0x7197,0x601E,0x14A1,0x0528,0x37B3,0x263A,0xDECD,0xCF44,0xFDDF,0xEC56,0x98E9,0x8960,0xBBFB,0xAA72,
0x6306,0x728F,0x4014,0x519D,0x2522,0x34AB,0x0630,0x17B9,0xEF4E,0xFEC7,0xCC5C,0xDDD5,0xA96A,0xB8E3,0x8A78,0x9BF1,
0x7387,0x620E,0x5095,0x411C,0x35A3,0x242A,0x16B1,0x0738,0xFFCF,0xEE46,0xDCDD,0xCD54,0xB9EB,0xA862,0x9AF9,0x8B70,
0x8408,0x9581,0xA71A,0xB693,0xC22C,0xD3A5,0xE13E,0xF0B7,0x0840,0x19C9,0x2B52,0x3ADB,0x4E64,0x5FED,0x6D76,0x7CFF,
0x9489,0x8500,0xB79B,0xA612,0xD2AD,0xC324,0xF1BF,0xE036,0x18C1,0x0948,0x3BD3,0x2A5A,0x5EE5,0x4F6C,0x7DF7,0x6C7E,
0xA50A,0xB483,0x8618,0x9791,0xE32E,0xF2A7,0xC03C,0xD1B5,0x2942,0x38CB,0x0A50,0x1BD9,0x6F66,0x7EEF,0x4C74,0x5DFD,
0xB58B,0xA402,0x9699,0x8710,0xF3AF,0xE226,0xD0BD,0xC134,0x39C3,0x284A,0x1AD1,0x0B58,0x7FE7,0x6E6E,0x5CF5,0x4D7C,
0xC60C,0xD785,0xE51E,0xF497,0x8028,0x91A1,0xA33A,0xB2B3,0x4A44,0x5BCD,0x6956,0x78DF,0x0C60,0x1DE9,0x2F72,0x3EFB,
0xD68D,0xC704,0xF59F,0xE416,0x90A9,0x8120,0xB3BB,0xA232,0x5AC5,0x4B4C,0x79D7,0x685E,0x1CE1,0x0D68,0x3FF3,0x2E7A,
0xE70E,0xF687,0xC41C,0xD595,0xA12A,0xB0A3,0x8238,0x93B1,0x6B46,0x7ACF,0x4854,0x59DD,0x2D62,0x3CEB,0x0E70,0x1FF9,
0xF78F,0xE606,0xD49D,0xC514,0xB1AB,0xA022,0x92B9,0x8330,0x7BC7,0x6A4E,0x58D5,0x495C,0x3DE3,0x2C6A,0x1EF1,0x0F78};
/*!< CRC table used to compute an Ethernet 32 bit CRC using the normal polynom 0x04C11DB7. */
static const uint32_t crc32EthernetTable[256] =
{
0x00000000,
0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b,
0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6,
0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac,
0x5bd4b01b, 0x569796c2, 0x52568b75, 0x6a1936c8, 0x6ed82b7f,
0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a,
0x745e66cd, 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, 0xbe2b5b58,
0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033,
0xa4ad16ea, 0xa06c0b5d, 0xd4326d90, 0xd0f37027, 0xddb056fe,
0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95,
0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4,
0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, 0x34867077, 0x30476dc0,
0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5,
0x2ac12072, 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16,
0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, 0x7897ab07,
0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c,
0x6211e6b5, 0x66d0fb02, 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1,
0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b,
0xbb60adfc, 0xb6238b25, 0xb2e29692, 0x8aad2b2f, 0x8e6c3698,
0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d,
0x94ea7b2a, 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e,
0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, 0xc6bcf05f,
0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34,
0xdc3abded, 0xd8fba05a, 0x690ce0ee, 0x6dcdfd59, 0x608edb80,
0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb,
0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a,
0x58c1663d, 0x558240e4, 0x51435d53, 0x251d3b9e, 0x21dc2629,
0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c,
0x3b5a6b9b, 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, 0xf12f560e,
0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65,
0xeba91bbc, 0xef68060b, 0xd727bbb6, 0xd3e6a601, 0xdea580d8,
0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3,
0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2,
0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, 0x9b3660c6, 0x9ff77d71,
0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74,
0x857130c3, 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640,
0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, 0x7b827d21,
0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a,
0x61043093, 0x65c52d24, 0x119b4be9, 0x155a565e, 0x18197087,
0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d,
0x2056cd3a, 0x2d15ebe3, 0x29d4f654, 0xc5a92679, 0xc1683bce,
0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb,
0xdbee767c, 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18,
0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, 0x89b8fd09,
0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662,
0x933eb0bb, 0x97ffad0c, 0xafb010b1, 0xab710d06, 0xa6322bdf,
0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
};
}
#endif // SBGCRC_H

43
Header_Files/sbgerrorcodes.h Normal file
View File

@ -0,0 +1,43 @@
#ifndef SBGERRORCODES_H
#define SBGERRORCODES_H
namespace sbgtc
{
/*!
* Generic errors definitions for SBG Systems projects.
*/
typedef enum _SbgErrorCode
{
SBG_NO_ERROR = 0, /*!< The operation was successfully executed. */
SBG_ERROR, /*!< We have a generic error. */
SBG_NULL_POINTER, /*!< A pointer is null. */
SBG_INVALID_CRC, /*!< The received frame has an invalid CRC. */
SBG_INVALID_FRAME, /*!< The received frame is invalid <br> */
/*!< We have received an unexpected frame (not the cmd we are waiting for or with an invalid data size.<br> */
/*!< This could be caused by a desync between questions and answers.<br> */
/*!< You should flush the serial port to fix this. */
SBG_TIME_OUT, /*!< We have started to receive a frame but not the end. */
SBG_WRITE_ERROR, /*!< All bytes hasn't been written. */
SBG_READ_ERROR, /*!< All bytes hasn't been read. */
SBG_BUFFER_OVERFLOW, /*!< A buffer is too small to contain so much data. */
SBG_INVALID_PARAMETER, /*!< An invalid parameter has been found. */
SBG_NOT_READY, /*!< A device isn't ready (Rx isn't ready for example). */
SBG_MALLOC_FAILED, /*!< Failed to allocate a buffer. */
SGB_CALIB_MAG_NOT_ENOUGH_POINTS, /*!< Not enough points were available to perform magnetometers calibration. */
SBG_CALIB_MAG_INVALID_TAKE, /*!< The calibration procedure could not be properly executed due to insufficient precision. */
SBG_CALIB_MAG_SATURATION, /*!< Saturation were detected when attempt to calibrate magnetos. */
SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE, /*!< 2D calibration procedure could not be performed. */
SBG_DEVICE_NOT_FOUND, /*!< A device couldn't be founded or opened PC only error code */
SBG_OPERATION_CANCELLED, /*!< An operation was canceled. PC only error code*/
SBG_NOT_CONTINUOUS_FRAME, /*!< We have received a frame that isn't a continuous one. PC only error code*/
SBG_INCOMPATIBLE_HARDWARE, /*!< Hence valid; the command cannot be executed because of hardware incompatibility */
SBG_INVALID_VERSION /*!< Incompatible version */
} SbgErrorCode;
}
#endif // SBGERRORCODES_H

234
Header_Files/sbglogparse.h Normal file
View File

@ -0,0 +1,234 @@
#ifndef SBGLOGPARSE_H
#define SBGLOGPARSE_H
#include "sbgbuffer.h"
namespace sbgtc
{
//----------------------------------------------------------------------//
//- Definition of all class id for sbgECom -//
//----------------------------------------------------------------------//
/*!
* Enum that defines all the message classes available.
*/
typedef enum _SbgEComClass
{
SBG_ECOM_CLASS_LOG_ECOM_0 = 0x00, /*!< Class that contains sbgECom protocol input/output log messages. */
SBG_ECOM_CLASS_LOG_ECOM_1 = 0x01, /*!< Class that contains special sbgECom output messages that handle high frequency output. */
SBG_ECOM_CLASS_LOG_NMEA_0 = 0x02, /*!< Class that contains NMEA (and NMEA like) output logs. <br>
Note: This class is only used for identification purpose and does not contain any sbgECom message. */
SBG_ECOM_CLASS_LOG_NMEA_1 = 0x03, /*!< Class that contains proprietary NMEA (and NMEA like) output logs. <br>
Note: This class is only used for identification purpose and does not contain any sbgECom message. */
SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 = 0x04, /*!< Class that contains third party output logs.
Note: This class is only used for identification purpose and does not contain any sbgECom message. */
SBG_ECOM_CLASS_LOG_CMD_0 = 0x10, /*!< Class that contains sbgECom protocol commands. */
SBG_ECOM_CLASS_LOG_ECOM_PRIVATE = 0xFE, /*!< Private output logs - internal use only */
SBG_ECOM_CLASS_CMD_PRIVATE = 0xFF /*!< Private commands - internal use only */
} SbgEComClass;
//----------------------------------------------------------------------//
//- Definition of all messages id for sbgECom -//
//----------------------------------------------------------------------//
/*!
* Enum that defines all the available ECom output logs from the sbgECom library.
*/
typedef enum _SbgEComLog
{
SBG_ECOM_LOG_STATUS = 1, /*!< Status general, clock, com aiding, solution, heave */
SBG_ECOM_LOG_UTC_TIME = 2, /*!< Provides UTC time reference */
SBG_ECOM_LOG_IMU_DATA = 3, /*!< Includes IMU status, acc., gyro, temp delta speeds and delta angles values */
SBG_ECOM_LOG_MAG = 4, /*!< Magnetic data with associated accelerometer on each axis */
SBG_ECOM_LOG_MAG_CALIB = 5, /*!< Magnetometer calibration data (raw buffer) */
SBG_ECOM_LOG_EKF_EULER = 6, /*!< Includes roll, pitch, yaw and their accuracies on each axis */
SBG_ECOM_LOG_EKF_QUAT = 7, /*!< Includes the 4 quaternions values */
SBG_ECOM_LOG_EKF_NAV = 8, /*!< Position and velocities in NED coordinates with the accuracies on each axis */
SBG_ECOM_LOG_SHIP_MOTION = 9, /*!< Heave, surge and sway and accelerations on each axis. */
SBG_ECOM_LOG_GPS1_VEL = 13, /*!< GPS velocities from primary or secondary GPS receiver */
SBG_ECOM_LOG_GPS1_POS = 14, /*!< GPS positions from primary or secondary GPS receiver */
SBG_ECOM_LOG_GPS1_HDT = 15, /*!< GPS true heading from dual antenna system */
SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */
SBG_ECOM_LOG_GPS2_VEL = 16, /*!< GPS 2 velocity log data. */
SBG_ECOM_LOG_GPS2_POS = 17, /*!< GPS 2 position log data. */
SBG_ECOM_LOG_GPS2_HDT = 18, /*!< GPS 2 true heading log data. */
SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */
SBG_ECOM_LOG_ODO_VEL = 19, /*!< Provides odometer velocity */
SBG_ECOM_LOG_EVENT_A = 24, /*!< Event markers sent when events are detected on sync in A pin */
SBG_ECOM_LOG_EVENT_B = 25, /*!< Event markers sent when events are detected on sync in B pin */
SBG_ECOM_LOG_EVENT_C = 26, /*!< Event markers sent when events are detected on sync in C pin */
SBG_ECOM_LOG_EVENT_D = 27, /*!< Event markers sent when events are detected on sync in D pin */
SBG_ECOM_LOG_EVENT_E = 28, /*!< Event markers sent when events are detected on sync in E pin */
SBG_ECOM_LOG_DVL_BOTTOM_TRACK = 29, /*!< Doppler Velocity Log for bottom tracking data. */
SBG_ECOM_LOG_DVL_WATER_TRACK = 30, /*!< Doppler Velocity log for water layer data. */
SBG_ECOM_LOG_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */
SBG_ECOM_LOG_AIR_DATA = 36, /*!< Air Data aiding such as barometric altimeter and true air speed. */
SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for subsea navigation. */
SBG_ECOM_LOG_IMU_RAW_DATA = 40, /*!< DEPRECATED: Private only log. */
SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Short IMU message recommended for post processing usages. */
SBG_ECOM_LOG_EVENT_OUT_A = 45, /*!< Event marker used to time stamp each generated Sync Out A signal. */
SBG_ECOM_LOG_EVENT_OUT_B = 46, /*!< Event marker used to time stamp each generated Sync Out B signal. */
SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for subsea navigation. */
SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */
SBG_ECOM_LOG_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */
} SbgEComLog;
/*!
* This type defines any message identifier.
* Because message identifiers enum will be different with each class id, we use a generic uint8_t rather than an enum.
*/
typedef uint8_t SbgEComMsgId;
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
/*!
* Solution filter mode enum.
*/
typedef enum _SbgEComSolutionMode
{
SBG_ECOM_SOL_MODE_UNINITIALIZED = 0, /*!< The Kalman filter is not initialized and the returned data are all invalid. */
SBG_ECOM_SOL_MODE_VERTICAL_GYRO = 1, /*!< The Kalman filter only rely on a vertical reference to compute roll and pitch angles. Heading and navigation data drift freely. */
SBG_ECOM_SOL_MODE_AHRS = 2, /*!< A heading reference is available, the Kalman filter provides full orientation but navigation data drift freely. */
SBG_ECOM_SOL_MODE_NAV_VELOCITY = 3, /*!< The Kalman filter computes orientation and velocity. Position is freely integrated from velocity estimation. */
SBG_ECOM_SOL_MODE_NAV_POSITION = 4 /*!< Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided. */
} SbgEComSolutionMode;
/*!
* EKF computed orientation using euler angles.
*/
typedef struct _SbgLogEkfEulerData
{
uint32_t timeStamp; /*!< Time in us since the sensor power up. */
float euler[3]; /*!< Roll, Pitch and Yaw angles in rad. */
float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */
uint32_t status; /*!< EKF solution status bitmask and enum. */
} SbgLogEkfEulerData;
/*!
* EFK computed orientation using quaternion.
*/
typedef struct _SbgLogEkfQuatData
{
uint32_t timeStamp; /*!< Time in us since the sensor power up. */
float quaternion[4]; /*!< Orientation quaternion stored in W, X, Y, Z form. */
float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */
uint32_t status; /*!< EKF solution status bitmask and enum. */
} SbgLogEkfQuatData;
/*!
* EFK computed navigation data.
*/
typedef struct _SbgLogEkfNavData
{
uint32_t timeStamp; /*!< Time in us since the sensor power up. */
float velocity[3]; /*!< North, East, Down velocity in m.s^-1. */
float velocityStdDev[3]; /*!< North, East, Down velocity 1 sigma standard deviation in m.s^-1. */
double position[3]; /*!< Latitude, Longitude in degrees positive North and East.
Altitude above Mean Sea Level in meters. */
float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */
float positionStdDev[3]; /*!< Latitude, longitude and altitude 1 sigma standard deviation in meters. */
uint32_t status; /*!< EKF solution status bitmask and enum. */
} SbgLogEkfNavData;
/*!
* Structure that stores data for the SBG_ECOM_LOG_UTC_TIME message.
*/
typedef struct _SbgLogUtcData
{
uint32_t timeStamp; /*!< Time in us since the sensor power up. */
uint16_t status; /*!< UTC time and clock status information */
uint16_t year; /*!< Year for example: 2013. */
int8_t month; /*!< Month in year [1 .. 12]. */
int8_t day; /*!< Day in month [1 .. 31]. */
int8_t hour; /*!< Hour in day [0 .. 23]. */
int8_t minute; /*!< Minute in hour [0 .. 59]. */
int8_t second; /*!< Second in minute [0 .. 60]. (60 is used only when a leap second is added) */
int32_t nanoSecond; /*!< Nanosecond of current second in ns. */
uint32_t gpsTimeOfWeek; /*!< GPS time of week in ms. */
} SbgLogUtcData;
/*!
* Structure that stores data for the SBG_ECOM_LOG_GPS#_POS message.
*/
typedef struct _SbgLogGpsPos
{
uint32_t timeStamp; /*!< Time in us since the sensor power up. */
uint32_t status; /*!< GPS position status, type and bitmask. */
uint32_t timeOfWeek; /*!< GPS time of week in ms. */
double latitude; /*!< Latitude in degrees, positive north. */
double longitude; /*!< Longitude in degrees, positive east. */
double altitude; /*!< Altitude above Mean Sea Level in meters. */
float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */
float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */
float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */
float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters. */
uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution (since version 1.4). */
uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4). */
uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4). */
} SbgLogGpsPos;
typedef union _SbgBinaryLogData
{
// SbgLogStatusData statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */
// SbgLogImuData imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */
// SbgLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */
SbgLogEkfEulerData ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */
SbgLogEkfQuatData ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */
SbgLogEkfNavData ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */
// SbgLogShipMotionData shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */
// SbgLogOdometerData odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */
SbgLogUtcData utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */
SbgLogGpsPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */
// SbgLogGpsVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */
// SbgLogGpsHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */
// SbgLogGpsRaw gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */
// SbgLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */
// SbgLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */
// SbgLogDvlData dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */
// SbgLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */
// SbgLogUsblData usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */
// SbgLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */
// SbgLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */
// SbgLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */
// /* Fast logs */
// SbgLogFastImuData fastImuData; /*!< Stores Fast Imu Data for 1KHz output */
} SbgBinaryLogData;
SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData);
SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData);
SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData);
}
#endif // SBGLOGPARSE_H

78
Header_Files/sbgrecorder.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef SBGRECORDER_H
#define SBGRECORDER_H
#include <QtSerialPort/QSerialPort>
#include <QtSerialPort/QSerialPortInfo>
#include "sbgerrorcodes.h"
#include "sbgbuffer.h"
#include "sbglogparse.h"
#include "sbgcrc.h"
#include "utility_tc.h"
namespace sbgtc
{
class SbgRecorder:public QObject
{
Q_OBJECT
public:
SbgRecorder();
QByteArray merge(QByteArray sbgMessage);
bool verify(int index,QByteArray sbgMessage);
SbgErrorCode extractOneValidFrame(rawBuffer *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize);
SbgErrorCode sbgBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData);
void parseSbgMessage(QByteArray * sbgMessage);
double calculateTimeDifferenceBetweenSystemAndSbg(SbgBinaryLogData logData);
int getSbgState() const;
private:
//0:没有惯导可用串口;1:惯导对应的串口打开成功,但是波特率等参数设置失败;2:惯导对应的串口打开成功;3:正在采集;
int m_iSbgState;
QSerialPort * m_serial;
bool m_bRecordControl;
bool m_bIsSbgReady;
bool m_bIsRecordHyperspecatralImage;
bool m_bIsNAV_POSITION_MODE;
bool m_bIsAccuracyLessThan7;
bool m_bIsSyncSystemTimeBaseGpstime;
uint32_t m_iSolutionMode;
int m_iSolutionModeCounter;
QString m_baseFileName;
public slots:
void openSerialPort();
void closeSerialPort();
void onSbgReady(double second,QString baseFileName);
void startRecordSbg();
void stopRecordSbg();
void startRecordHyperspectral();
void stopRecordHyperspectral();
signals:
void sbgReady(double second,QString baseFileName);
void serialPortStatus(int);
void sbgSolutionModeSignal(int);
void sbgAccuracySignal(int);
};
}
#endif // SBGRECORDER_H

103
Header_Files/udpserver.h Normal file
View File

@ -0,0 +1,103 @@
#ifndef UDPSERVER_H
#define UDPSERVER_H
#include <iostream>
#include <string.h>
#include <time.h>
#include <ctime>
#include <QCoreApplication>
#include <QUdpSocket>
#include <QObject>
#include <QThread>
#include <QDir>
#include "ximeaimager.h"
#include "sbgrecorder.h"
#include "fileoperation.h"
extern "C"
{
#include <sbgEComLib.h>
#include <stdlib.h>
}
class sbgMagCibWorkThread:public QThread
{
Q_OBJECT
public:
sbgMagCibWorkThread();
int m_iMagCalibStopControl;
void displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults);
protected:
void run();
private:
// 0:串口打开错误;
// 1:磁场矫正失败;
// 2:Unable to get onboard magnetic calibration results;
// 3:磁场数据无效:invalid;
// 4:写入磁场数据失败;
// 5:POOR;
// 6:GOOD;
// 7:OPTIMAL;
int m_iMagCalibState;
void signalWrap(int state);
signals:
void magCalibStateSignal(int);
};
class UdpServer:public QObject
{
Q_OBJECT
public:
UdpServer();
QThread * m_RecordThread;
QThread * m_RecordSbgThread;
QThread * m_CopyFileThread;
private slots:
void processPendingDatagrams();
private:
QUdpSocket * m_udpSocket;
sbgMagCibWorkThread * m_sbgMagCibWorkThread;
QHostAddress m_clientIpAddress;
quint16 m_clientPort;
XimeaImager * m_imager;
sbgtc::SbgRecorder * m_sbgRecorder;
FileOperation * m_copyFile;
double getTimeDifferenceBetweenSystemAndSbg(double secondSbg);
void sender(int status);
signals:
void systemStart();
void systemStop();
void startRecordHyperspectralSignal();
void startCopyFileSignal();
void startDeleteFileSignal();
void recordXimeaOnlySignal(double,QString);
public slots:
void onRecordFinished();
void sendSerialPortStatus(int serialPortStatus);
void sendSbgMagCalibState(int SbgMagCalibState);
void sendSbgSolutionModeState(int SolutionMode);
void sendSbgAccuracyState(int Accuracy);
void sendXimeaImageStatus(int ximeaImageStatus);
void sendCopyFileStatus(int fileStatus);
};
#endif // UDPSERVER_H

16
Header_Files/utility_tc.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef UTILITY_TC_H
#define UTILITY_TC_H
#include <iostream>
#include <QString>
#include <QDebug>
QString getFileNameBaseOnTime();
//https://blog.csdn.net/MoreWindows/article/details/6657829
void bubbleSort(unsigned short * a, int n);
void swap(unsigned short * a, unsigned short * b);
#endif // UTILITY_TC_H

115
Header_Files/ximeaimager.h Normal file
View File

@ -0,0 +1,115 @@
#ifndef XIMEAIMAGER_H
#define XIMEAIMAGER_H
/*
This is the reference example application code for XIMEA cameras.
You can use it to simplify development of your camera application.
Sample name:
xiAPI / Capture-10-images
Description:
Open camera, capture 10 images while printing first pixel from each image.
Workflow:
1: Open camera
2: Set parameters
3: Start acquisition
4: Each image captured - print dimensions and value of the first pixel
*/
#include <stdio.h>
#include <iostream>
#include <sys/time.h>
#include <memory.h>
#include <fstream>
#include <unistd.h>
#include <exception>
#include <QObject>
#include <qthread.h>
#include "irisximeaimager.h"
#include "math.h"
#include "utility_tc.h"
//#ifdef WIN32
//#include <xiApi.h> // Windows
//#else
//#include <m3api/xiApi.h> // Linux, OSX
//#endif
class XimeaImager : public QObject
{
Q_OBJECT
public:
XimeaImager();
void setFramerate(double framerate);
double getFramerate();
double setExposureTime(float exposureTime);
double getExposureTime();
double autoExposure();
void setGain(double gain);
double getGain();
int getSampleCount();
int getBandCount();
int getWindowStartBand();
int getWindowEndBand();
double geWavelengthAtBand(int x);
void stopRecord();
int getFrameCounter();
int getMaxValueOfOneFrame(unsigned short * data, int numberOfPixel);
int getImagerState() const;
private:
//0:未打开;1:打开;2:设置帧率;3:自动曝光;4:正在采集; 21,未处理错误;22:RESOURCE_OR_FUNCTION_LOCKED;23,;
int m_iImagerState;
int m_iImagerStateTemp;
QString m_baseFileName;
Iris::IrisXimeaImager m_imager;
unsigned short * m_buffer;
bool m_bRecordControl;
int m_iFrameCounter;
int m_iFrameSizeInByte;
void writeHdr();
void processXiApiErrorCodes(int xiApiErrorCodes);
inline double getSbgTime(double TimeDifferenceBetweensOSAndSbg)
{
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;
// printf("\n");
// printf("XimeaImager::getSbgTime------系统时间纳秒%d\n", systemTime.tv_nsec);
// printf("XimeaImager::getSbgTime------系统时间(未偏移)%f\n", nanosecondSystem);
// printf("XimeaImager::getSbgTime------系统时间(偏移)%f\n", nanosecondSystem-TimeDifferenceBetweensOSAndSbg);
return nanosecondSystem-TimeDifferenceBetweensOSAndSbg;
}
public slots:
void openImger();
void closeImger();
void startRecord(double TimeDifferenceBetweensOSAndSbg,QString baseFileName);
signals:
void recordFinished();
void ximeaImageStatus(int);
};
#endif // XIMEAIMAGER_H

199
Source_Files/fileoperation.cpp Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View File

@ -0,0 +1,682 @@
#include "Header_Files/udpserver.h"
//using namespace sbgtc;
UdpServer::UdpServer()
{
m_udpSocket = new QUdpSocket(this);
m_udpSocket->bind(45454, QUdpSocket::ShareAddress);
connect(m_udpSocket, SIGNAL(readyRead()),this, SLOT(processPendingDatagrams()));
m_sbgMagCibWorkThread = new sbgMagCibWorkThread();
m_RecordSbgThread=new QThread();
m_sbgRecorder=new sbgtc::SbgRecorder();
m_sbgRecorder->moveToThread(m_RecordSbgThread);
m_RecordSbgThread->start();
m_RecordThread=new QThread();
m_imager=new XimeaImager();
m_imager->moveToThread(m_RecordThread);
m_RecordThread->start();
m_CopyFileThread=new QThread();
m_copyFile=new FileOperation();
m_copyFile->moveToThread(m_CopyFileThread);
m_CopyFileThread->start();
//系统采集步骤1:打开sbg串口并采集数据，打开光谱仪
// connect(this, SIGNAL(systemStart()),m_sbgRecorder, SLOT(startRecordSbg()));
connect(this, SIGNAL(systemStart()),m_imager, SLOT(openImger()));
connect(this, SIGNAL(systemStop()),m_sbgRecorder, SLOT(closeSerialPort()));//
connect(this, SIGNAL(systemStop()),m_imager, SLOT(closeImger()));
//系统采集步骤2:开始采集高光谱影像
connect(this, SIGNAL(startRecordHyperspectralSignal()),m_sbgRecorder, SLOT(startRecordHyperspectral()));
connect(m_sbgRecorder, SIGNAL(sbgReady(double,QString)),m_imager, SLOT(startRecord(double,QString)));
connect(this, SIGNAL(recordXimeaOnlySignal(double,QString)),m_imager, SLOT(startRecord(double,QString)));
//系统采集步骤3:停止采集
connect(m_imager, SIGNAL(recordFinished()),this, SLOT(onRecordFinished()));
//系统采集步骤4:拷贝文件
connect(this, SIGNAL(startCopyFileSignal()),m_copyFile, SLOT(copyFile()));
connect(this, SIGNAL(startDeleteFileSignal()),m_copyFile, SLOT(deleteFile()));
//系统采集步骤5:进程间通讯
connect(m_sbgRecorder, SIGNAL(serialPortStatus(int)),this, SLOT(sendSerialPortStatus(int)));
connect(m_sbgRecorder, SIGNAL(sbgSolutionModeSignal(int)),this, SLOT(sendSbgSolutionModeState(int)));
connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int)),this, SLOT(sendSbgAccuracyState(int)));
connect(m_imager, SIGNAL(ximeaImageStatus(int)),this, SLOT(sendXimeaImageStatus(int)));
connect(m_sbgMagCibWorkThread, SIGNAL(magCalibStateSignal(int)),this, SLOT(sendSbgMagCalibState(int)));
connect(m_copyFile, SIGNAL(copyFileStatus(int)),this, SLOT(sendCopyFileStatus(int)));
//当软件不正常关闭并且重启后，通知其他psdk程序
m_clientIpAddress=QHostAddress(QHostAddress::LocalHost);
sendSerialPortStatus(0);
sendXimeaImageStatus(0);
sendSbgMagCalibState(0);
sendCopyFileStatus(0);
std::cout<<"UdpServer::UdpServer--------:System ready!"<<std::endl;
}
void UdpServer::processPendingDatagrams()
{
using namespace std;
while (m_udpSocket->hasPendingDatagrams())
{
QByteArray datagram;
datagram.resize(m_udpSocket->pendingDatagramSize());
m_udpSocket->readDatagram(datagram.data(), datagram.size(),&m_clientIpAddress, &m_clientPort);
printf("接收数据字节数: %d.\n",datagram.size());
QList<QByteArray> datagramList=datagram.split(',');
printf("有多少个list: %d.\n",datagramList.size());
QString instruction=datagramList[0].data();// QByteArray转QString方法1
switch (instruction.toInt())
{
case 1://启动系统: 打开sbg串口并采集数据，打开光谱仪
{
std::cout<<"1代表启动系统!"<<std::endl;
emit systemStart();
break;
}
case 2://关闭系统:关闭相机和sbg串口,关闭软件
{
std::cout<<"2代表关闭系统!"<<std::endl;
if(m_sbgRecorder->getSbgState()>=1)
{
m_sbgRecorder->stopRecordSbg();
}
if(m_imager->getImagerState()>=1)
{
m_imager->stopRecord();
}
emit systemStop();
//QCoreApplication::quit();
break;
}
case 3://系统开始采集高光谱影像
{
//emit startRecordHyperspectralSignal();//真实的影像开始采集通过惯导中的信号(sbgReady)触发
m_sbgRecorder->startRecordHyperspectral();
// if(m_sbgRecorder->getSbgState()==2)//开始采集前还需要判断相机的状态??????????????????????????????????????????
// {
// }
// else if(m_sbgRecorder->getSbgState()==3)
// {
// std::cout<<"系统已经开始采集!"<<std::endl;
// }
break;
}
case 4://系统停止采集高光谱影像
{
std::cout<<"4代表系统停止采集高光谱影像!"<<std::endl;
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=4)
{
m_imager->stopRecord();
}
break;
}
case 5://
{
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
{
std::cout<<"5代表设置帧率!"<<std::endl;
m_imager->setFramerate(datagramList[1].toFloat());
}
break;
}
case 6://
{
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
{
std::cout<<"6代表自动曝光!"<<std::endl;
m_imager->autoExposure();
}
break;
}
case 7:
{
if(datagramList[1].toInt()==1)
{
std::cout<<"拷贝数据!"<<std::endl;
emit startCopyFileSignal();
}
else if(datagramList[1].toInt()==0)
{
std::cout<<"删除数据!"<<std::endl;
emit startDeleteFileSignal();
}
break;
}
case 8:
{
std::cout<<"8代表磁场矫正!"<<std::endl;
if(datagramList[1].toInt()==1)
{
//magCalib();
std::cout<<"8-1: 开始磁场矫正!"<<std::endl;
m_sbgMagCibWorkThread->start();
}
else if(datagramList[1].toInt()==0)
{
std::cout<<"8-0: 停止磁场矫正!"<<std::endl;
m_sbgMagCibWorkThread->m_iMagCalibStopControl=0;
}
break;
}
case 9:
{
std::cout<<"9代表仅采集影像!"<<std::endl;
if(datagramList[1].toInt()==1)
{
QString xx = getFileNameBaseOnTime();
xx = xx + "testImage";
emit recordXimeaOnlySignal(1000.0,xx);
// emit recordXimeaOnlySignal(1000.0,"testImage");
}
else if(datagramList[1].toInt()==0)
{
m_imager->stopRecord();
}
break;
}
default:
std::cout<<">=9没有意义!"<<std::endl;
break;
}
}
}
double UdpServer::getTimeDifferenceBetweenSystemAndSbg(double secondSbg)
{
// time_t timer;//time_t就是long int 类型
// struct tm *tblock;
// timer = time(NULL);//返回秒数(精度为秒)，从1970-1-1,00:00:00 可以当成整型输出或用于其它函数
// tblock = localtime(&timer);
// printf("Local time is: %s\n", asctime(tblock));
//https://blog.csdn.net/FUN6367/article/details/89787566
struct timespec systemTime;
clock_gettime(CLOCK_REALTIME,&systemTime);
tm systemTime_rili;
localtime_r(&systemTime.tv_sec, &systemTime_rili);
// std::cout<<"systemTime_rili--年: "<<systemTime_rili.tm_year+1900<<std::endl;
// std::cout<<"systemTime_rili--月: "<<systemTime_rili.tm_mon+1<<std::endl;
// std::cout<<"systemTime_rili--日: "<<systemTime_rili.tm_mday<<std::endl;
// std::cout<<"systemTime_rili--时: "<<systemTime_rili.tm_hour<<std::endl;
// std::cout<<"systemTime_rili--分: "<<systemTime_rili.tm_min<<std::endl;
// std::cout<<"systemTime_rili--秒: "<<systemTime_rili.tm_sec<<std::endl;
// printf("Local time is: %s\n", asctime(&systemTime_rili));
double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;
printf("UdpServer::getTimeDifferenceBetweenSystemAndSbg------%f\n", nanosecondSystem-secondSbg);
return nanosecondSystem-secondSbg;
}
void UdpServer::sender(int status)
{
QByteArray datagram2send;
datagram2send.operator =(QString::number(status).toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, m_clientPort+1);
}
void UdpServer::sendSerialPortStatus(int serialPortStatus)
{
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< serialPortStatus <<std::endl;
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< m_clientIpAddress.AnyIPv4 <<std::endl;
QByteArray datagram2send;
QString status = "sbg," + QString::number(serialPortStatus);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendSbgMagCalibState(int SbgMagCalibState)
{
std::cout<<"UdpServer::sendSbgMagCalibState---------------------:"<< SbgMagCalibState <<std::endl;
QByteArray datagram2send;
QString status = "mag," + QString::number(SbgMagCalibState);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendSbgSolutionModeState(int SolutionMode)
{
std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <<std::endl;
QByteArray datagram2send;
QString status = "SolutionMode," + QString::number(SolutionMode);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendSbgAccuracyState(int Accuracy)
{
// std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <<std::endl;
QByteArray datagram2send;
QString status = "Accuracy," + QString::number(Accuracy);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendXimeaImageStatus(int ximeaImageStatus)
{
std::cout<<"UdpServer::sendXimeaImageStatus---------------------:"<< ximeaImageStatus <<std::endl;
QByteArray datagram2send;
QString status = "ximea," + QString::number(ximeaImageStatus);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::sendCopyFileStatus(int fileStatus)
{
std::cout<<"UdpServer::sendCopyFileStatus---------------------:"<< fileStatus <<std::endl;
QByteArray datagram2send;
QString status = "file," + QString::number(fileStatus);
datagram2send.operator =(status.toStdString().c_str());
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}
void UdpServer::onRecordFinished()
{
std::cout<<"UdpServer::onRecordFinished----------------:影像停止采集"<<std::endl;
}
sbgMagCibWorkThread::sbgMagCibWorkThread()
{
}
void sbgMagCibWorkThread::displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults)
{
//
// Display the magnetic calibration results
//
printf("\n\n======== Magnetic calibration report ========\n");
//
// Convert the quality indicator to human readable output
//
switch (pMagCalibResults->quality)
{
case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL:
{
printf("Quality:\t\toptimal\n");
signalWrap(7);
break;
}
case SBG_ECOM_MAG_CALIB_QUAL_GOOD:
{
printf("Quality:\t\tgood\n");
signalWrap(6);
break;
}
case SBG_ECOM_MAG_CALIB_QUAL_POOR:
{
printf("Quality:\t\tpoor\n");
signalWrap(5);
break;
}
default:
printf("Quality:\t\tundefined\n");
}
//
// Convert the confidence indicator to human readable output
//
switch (pMagCalibResults->confidence)
{
case SBG_ECOM_MAG_CALIB_TRUST_HIGH:
printf("Confidence:\t\thigh\n");
break;
case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM:
printf("Confidence:\t\tmedium\n");
break;
case SBG_ECOM_MAG_CALIB_TRUST_LOW:
printf("Confidence:\t\tlow\n");
break;
default:
printf("Confidence:\t\tundefined\n");
}
//
// Print advanced status
//
printf("Advanced Status:\n");
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS)
{
printf("\t- Not enough valid points. Maybe you are moving too fast.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS)
{
printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE)
{
printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n");
}
//
// Test if we have a 2D or 3D calibration mode
//
if (mode == SBG_ECOM_MAG_CALIB_MODE_2D)
{
//
// In 2D mode, a X or Y motion issue means we have too much motion
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
{
printf("\t- Too much roll motion for a 2D magnetic calibration.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
{
printf("\t- Too much pitch motion for a 2D magnetic calibration.\n");
}
}
else
{
//
// In 3D mode, a X or Y motion issue means we have not enough motion
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
{
printf("\t- Not enough roll motion for a 3D magnetic calibration.\n");
}
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
{
printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n");
}
}
//
// Test if we had enough yaw motion to compute a calibration
//
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE)
{
//
// Test if we are in
printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n");
}
//
// Display the number of points used to compute the magnetic calibration
//
printf("\n\n");
printf("Used Points:\t%u\n", pMagCalibResults->numPoints);
printf("Max Points:\t%u\n", pMagCalibResults->maxNumPoints);
//
// Display magnetic field deviation errors
//
printf("\n\n-------------------------------------\n");
printf("Magnetic field deviation report\n");
printf("-------------------------------------\n");
printf("\t\tMean\tStd\tMax\n");
printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError);
printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError);
printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegF(pMagCalibResults->meanAccuracy), sbgRadToDegF(pMagCalibResults->stdAccuracy), sbgRadToDegF(pMagCalibResults->maxAccuracy));
printf("\n\n\n");
}
void sbgMagCibWorkThread::run()
{
m_iMagCalibStopControl = 1;
SbgEComHandle comHandle;
SbgErrorCode errorCode;
SbgInterface sbgInterface;
int32 retValue = 0;
SbgEComDeviceInfo deviceInfo;
SbgEComMagCalibResults magCalibResults;
SbgEComMagCalibMode mode;
//
// Create an interface:
// We can choose either a serial for real time operation, or file for previously logged data parsing
// Note interface closing is also differentiated !
//
errorCode = sbgInterfaceSerialCreate(&sbgInterface, "/dev/sbg_serial_port", 460800); // Example for Unix using a FTDI Usb2Uart converter
//errorCode = sbgInterfaceSerialCreate(&sbgInterface, "COM23", 115200); // Example for Windows serial communication
//
// Test that the interface has been created
//
if (errorCode == SBG_NO_ERROR)
{
//
// Create the sbgECom library and associate it with the created interfaces
//
errorCode = sbgEComInit(&comHandle, &sbgInterface);
//
// Test that the sbgECom has been initialized
//
if (errorCode == SBG_NO_ERROR)
{
printf("sbgECom properly Initialized.\n");
printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR);
//
// Get device information
//
errorCode = sbgEComCmdGetInfo(&comHandle, &deviceInfo);
//
// Display device information if no error
//
if (errorCode == SBG_NO_ERROR)
{
printf("Device : %0.9u found\n", deviceInfo.serialNumber);
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get device information.\n");
}
//
// Define the calibration mode to perform
//
mode = SBG_ECOM_MAG_CALIB_MODE_3D;
//
// Start / reset the acquisition of magnetic field data
// Each time this command is called, the device is prepared to acquire a new set of magnetic field data
// You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration
//
errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW);
//
// Make sure that the magnetic calibration has started
//
if (errorCode == SBG_NO_ERROR)
{
//
// The device is now acquiring some magnetic field data.
// Wait for a user input before computing the magnetic calibration
//
printf("The device is acquiring magnetic field data.\n\nPress enter to stop the magnetic field acquisition.\n");
//fgetc(stdin);//-------------------------------------------------------------------------------------------------------------------------------------------
while (m_iMagCalibStopControl)
{
printf("h");
}
printf("kkkkkkkkkkkkkkkkkkkkkkkkkkkk\n");
//
// Try to compute a magnetic calibration and get the results
//
errorCode = sbgEComCmdMagComputeCalib(&comHandle, &magCalibResults);
//
// Make sure that we were able to get magnetic calibration results
//
if (errorCode == SBG_NO_ERROR)
{
//
// Test if the device has computed a valid magnetic calibration
//
if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID)
{
//
// Send the new magnetic calibration data
//
errorCode = sbgEComCmdMagSetCalibData(&comHandle, magCalibResults.offset, magCalibResults.matrix);
//
// Make sure that the new magnetic calibration data has been updated
//
if (errorCode == SBG_NO_ERROR)
{
printf("The new magnetic calibration has been applied.\n");
//
// Display the magnetic calibration status
//
displayMagCalibResults(mode, &magCalibResults);
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to upload new magnetic calibration data.\n");
signalWrap(4);
}
}
else
{
//
// Unable to compute a valid magnetic calibration
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to compute a valid magnetic calibration.\n");
signalWrap(3);
}
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get onboard magnetic calibration results.\n");
signalWrap(2);
}
}
else
{
fprintf(stderr, "ellipseOnboardMagCalib: Unable to start the onboard magnetic calibration.\n");
signalWrap(1);
}
//
// Close the sbgEcom library
//
sbgEComClose(&comHandle);
}
else
{
//
// Unable to initialize the sbgECom
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to initialize the sbgECom library.\n");
retValue = -1;
signalWrap(0);
}
//
// Close the interface
//
sbgInterfaceSerialDestroy(&sbgInterface);
}
else
{
//
// Unable to create the interface
//
fprintf(stderr, "ellipseOnboardMagCalib: Unable to create the interface.\n");
retValue = -1;
signalWrap(0);
}
printf("magCalib process complete!\n");
//fgetc(stdin);
}
void sbgMagCibWorkThread::signalWrap(int state)
{
std::cout<<"sbgMagCibWorkThread::signalWrap---------------------:"<< state <<std::endl;
m_iMagCalibState=state;
emit magCalibStateSignal(m_iMagCalibState);
}

71
Source_Files/utility_tc.cpp Normal file
View 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
View 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;
}