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

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