300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正
This commit is contained in:
tangchao0503
2
.gitignore
vendored
Normal file
2
.gitignore
vendored
Normal file
@ -0,0 +1,2 @@
|
||||
.idea
|
||||
build
|
||||
53
CMakeLists.txt
Normal file
53
CMakeLists.txt
Normal 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
35
Header_Files/fileoperation.h
Normal 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
136
Header_Files/math_tc.h
Normal 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
97
Header_Files/sbgbuffer.h
Normal 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
182
Header_Files/sbgcrc.h
Normal 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
43
Header_Files/sbgerrorcodes.h
Normal 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
234
Header_Files/sbglogparse.h
Normal 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
78
Header_Files/sbgrecorder.h
Normal 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
103
Header_Files/udpserver.h
Normal 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
16
Header_Files/utility_tc.h
Normal 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
115
Header_Files/ximeaimager.h
Normal 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
199
Source_Files/fileoperation.cpp
Normal file
@ -0,0 +1,199 @@
|
||||
#include "Header_Files/fileoperation.h"
|
||||
|
||||
FileOperation::FileOperation()
|
||||
{
|
||||
m_copyFileStatus = 0;
|
||||
}
|
||||
|
||||
bool FileOperation::copyFileToPath(QString sourceDir ,QString toDir, bool coverFileIfExist)
|
||||
{
|
||||
toDir.replace("\\","/");
|
||||
if (sourceDir == toDir){
|
||||
return true;
|
||||
}
|
||||
if (!QFile::exists(sourceDir)){
|
||||
return false;
|
||||
}
|
||||
QDir *createfile = new QDir;
|
||||
bool exist = createfile->exists(toDir);
|
||||
if (exist){
|
||||
if(coverFileIfExist){
|
||||
createfile->remove(toDir);
|
||||
}
|
||||
}//end if
|
||||
|
||||
if(!QFile::copy(sourceDir, toDir))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
QStringList FileOperation::getSubfolders(QDir parentFolder)
|
||||
{
|
||||
QStringList entryList= parentFolder.entryList();
|
||||
QStringList result;
|
||||
|
||||
//
|
||||
for(int i=0;i<entryList.size();i++)
|
||||
{
|
||||
auto element=entryList.at(i).toLocal8Bit().constData();
|
||||
QString tmp=entryList.at(i);
|
||||
|
||||
if(!strcmp(element,".") || !strcmp(element,".."))//QString::compare()
|
||||
{
|
||||
//std::cout<<"11111111111111111111111111111"<<std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
result.append(entryList.at(i));
|
||||
//std::cout<<""<<entryList.at(i).toLocal8Bit().constData()<<std::endl;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
QString FileOperation::getFlashDrivePath()
|
||||
{
|
||||
//获取u盘路径
|
||||
QDir flashDrivePath("/media");
|
||||
QStringList subfolders=getSubfolders(flashDrivePath);
|
||||
flashDrivePath.cd(subfolders.at(0));
|
||||
|
||||
QStringList result=getSubfolders(flashDrivePath);
|
||||
|
||||
|
||||
if(result.isEmpty())
|
||||
{
|
||||
//std::cout<<"FileOperation::getFlashDrivePath--------------------------: 请插入u盘!"<<std::endl;
|
||||
return "";
|
||||
}
|
||||
|
||||
if(!flashDrivePath.cd(result.at(0)))
|
||||
{
|
||||
std::cout<<"进入u盘失败!"<<std::endl;
|
||||
return "";
|
||||
}
|
||||
|
||||
if(!flashDrivePath.exists("ximeaImageData"))
|
||||
{
|
||||
flashDrivePath.mkdir("ximeaImageData");
|
||||
}
|
||||
flashDrivePath.cd("ximeaImageData");
|
||||
|
||||
QString destinationFolder=flashDrivePath.absolutePath();
|
||||
std::cout<<"进入u盘成功:"<<destinationFolder.toStdString()<<std::endl;
|
||||
|
||||
return destinationFolder;
|
||||
}
|
||||
|
||||
QDir FileOperation::getSourcePath()
|
||||
{
|
||||
//获取源数据文件夹
|
||||
|
||||
QDir sourcePath("/media/nvme");
|
||||
|
||||
// QString homePath=QDir::homePath();
|
||||
// QDir sourcePath(homePath);
|
||||
// bool re=sourcePath.exists("ximeaImageData");
|
||||
// if(!re)
|
||||
// {
|
||||
// std::cout<<"No data!"<<std::endl;
|
||||
// sourcePath.mkdir("ximeaImageData");
|
||||
// }
|
||||
//
|
||||
// sourcePath.cd("ximeaImageData");
|
||||
|
||||
return sourcePath;
|
||||
}
|
||||
|
||||
void FileOperation::copyFile()
|
||||
{
|
||||
QString destinationFolder= getFlashDrivePath();
|
||||
QDir sourcePath= getSourcePath();
|
||||
|
||||
if(destinationFolder.isEmpty())
|
||||
{
|
||||
m_copyFileStatus = 3;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
QStringList timesFilter,sbgFilter,hdrFilter,datFilter;
|
||||
timesFilter<<"*.times";
|
||||
sbgFilter<<"*.sbg";
|
||||
hdrFilter<<"*.hdr";
|
||||
datFilter<<"*.dat";
|
||||
|
||||
QStringList timesEntryList=sourcePath.entryList(timesFilter,QDir::Files);
|
||||
QStringList sbgEntryList=sourcePath.entryList(sbgFilter,QDir::Files);
|
||||
QStringList hdrEntryList=sourcePath.entryList(hdrFilter,QDir::Files);
|
||||
QStringList datEntryList=sourcePath.entryList(datFilter,QDir::Files);
|
||||
|
||||
if(timesEntryList.isEmpty() && sbgEntryList.isEmpty() && hdrEntryList.isEmpty() && datEntryList.isEmpty())
|
||||
{
|
||||
m_copyFileStatus = 2;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
m_copyFileStatus = 1;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
|
||||
|
||||
//copy times file
|
||||
std::cout<<"copy times file"<<std::endl;
|
||||
for(int i=0;i<timesEntryList.size();i++)
|
||||
{
|
||||
// std::cout<<"------"<<timesEntryList.at(i).toLocal8Bit().constData()<<std::endl;
|
||||
// std::cout<<"------"<<timesEntryList.at(i).<<std::endl;
|
||||
|
||||
QString source=sourcePath.absolutePath()+"/"+timesEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+timesEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy sbg file
|
||||
std::cout<<"copy sbg file"<<std::endl;
|
||||
for(int i=0;i<sbgEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+sbgEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+sbgEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy hdr file
|
||||
std::cout<<"copy hdr file"<<std::endl;
|
||||
for(int i=0;i<hdrEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+hdrEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+hdrEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
//copy image file
|
||||
std::cout<<"copy image file"<<std::endl;
|
||||
for(int i=0;i<datEntryList.size();i++)
|
||||
{
|
||||
QString source=sourcePath.absolutePath()+"/"+datEntryList.at(i);
|
||||
QString destination=destinationFolder+"/"+datEntryList.at(i);
|
||||
|
||||
copyFileToPath(source,destination,false);
|
||||
}
|
||||
|
||||
m_copyFileStatus = 0;
|
||||
emit copyFileStatus(m_copyFileStatus);
|
||||
}
|
||||
|
||||
void FileOperation::deleteFile()
|
||||
{
|
||||
QDir sourcePath= getSourcePath();
|
||||
|
||||
sourcePath.removeRecursively();
|
||||
}
|
||||
13
Source_Files/main.cpp
Normal file
13
Source_Files/main.cpp
Normal file
@ -0,0 +1,13 @@
|
||||
#include <QCoreApplication>
|
||||
#include "Header_Files/udpserver.h"
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
QCoreApplication a(argc, argv);
|
||||
|
||||
//UdpServer* x=new UdpServer();
|
||||
QString str="234234";
|
||||
UdpServer x;
|
||||
|
||||
return a.exec();
|
||||
}
|
||||
636
Source_Files/sbgbuffer.cpp
Normal file
636
Source_Files/sbgbuffer.cpp
Normal file
@ -0,0 +1,636 @@
|
||||
#include "Header_Files/sbgbuffer.h"
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
rawBuffer sbgtc::initRawBuffer(QByteArray * sbgMessage)
|
||||
{
|
||||
rawBuffer tmp;
|
||||
|
||||
tmp.rxBuffer=(uint8_t *)sbgMessage->data();
|
||||
tmp.rxBufferSize=sbgMessage->size();
|
||||
|
||||
return tmp;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
// assert(pLinkedBuffer);
|
||||
|
||||
//
|
||||
// Initialize stream parameters
|
||||
//
|
||||
pHandle->bufferSize = bufferSize;
|
||||
pHandle->errorCode = SBG_NO_ERROR;
|
||||
|
||||
//
|
||||
// Initialize the buffer
|
||||
//
|
||||
pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer;
|
||||
pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer;
|
||||
|
||||
//
|
||||
// For now, we don't handle any error, maybe we could add checks in debug mode only
|
||||
//
|
||||
return SBG_NO_ERROR;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// According to the origin reference point
|
||||
//
|
||||
switch (origin)
|
||||
{
|
||||
case SB_SEEK_SET:
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + offset;
|
||||
break;
|
||||
case SB_SEEK_CUR_INC:
|
||||
pHandle->pCurrentPtr += offset;
|
||||
break;
|
||||
case SB_SEEK_CUR_DEC:
|
||||
pHandle->pCurrentPtr -= offset;
|
||||
break;
|
||||
case SB_SEEK_END:
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset);
|
||||
break;
|
||||
default:
|
||||
pHandle->errorCode = SBG_INVALID_PARAMETER;
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter");
|
||||
}
|
||||
|
||||
//
|
||||
// Make sure that no error has occurred
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if the current ptr is still within the buffer bounds
|
||||
//
|
||||
if (pHandle->pCurrentPtr < pHandle->pBufferPtr)
|
||||
{
|
||||
//
|
||||
// We are before the buffer so clamp to the begining of the buffer and raise an error
|
||||
//
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr;
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
|
||||
//
|
||||
// Stream buffer underflow
|
||||
//
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer");
|
||||
}
|
||||
else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize)
|
||||
{
|
||||
//
|
||||
// We are after the buffer so clamp to the end of the buffer and raise an error
|
||||
//
|
||||
pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize;
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
|
||||
//
|
||||
// Stream buffer overflow
|
||||
//
|
||||
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return pHandle->errorCode;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferTell(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr;
|
||||
}
|
||||
|
||||
void * sbgtc::sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return pHandle->pCurrentPtr;
|
||||
}
|
||||
|
||||
uint8_t sbgtc::sbgStreamBufferReadUint8LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t))
|
||||
{
|
||||
//
|
||||
// Read the byte
|
||||
//
|
||||
return *((uint8_t*)(pHandle->pCurrentPtr++));
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t sbgtc::sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint16_t bytesValues[2];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint16_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[1] | (bytesValues[0] << 8);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t sbgtc::sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint32_t bytesValues[4];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint32_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Read an uint64_t from a stream buffer (Little endian version).
|
||||
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
||||
* \return The read value or 0 if we have an error.
|
||||
*/
|
||||
uint64_t sbgtc::sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
uint64_t lowPart;
|
||||
uint64_t highPart;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
lowPart = *((uint64_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(uint64_t);
|
||||
|
||||
return lowPart;
|
||||
#else
|
||||
//
|
||||
// Read 64 bit value using two 32 bits read to avoid too much 64 bits operations
|
||||
//
|
||||
lowPart = sbgStreamBufferReadUint32LE(pHandle);
|
||||
highPart = sbgStreamBufferReadUint32LE(pHandle);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return (lowPart << 32) | highPart;
|
||||
#else
|
||||
return lowPart | (highPart << 32);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0ull;
|
||||
}
|
||||
|
||||
int8_t sbgtc::sbgStreamBufferReadInt8LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t))
|
||||
{
|
||||
//
|
||||
// Read the byte
|
||||
//
|
||||
return *((int8_t*)(pHandle->pCurrentPtr++));
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t sbgtc::sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
int32_t bytesValues[4];
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t))
|
||||
{
|
||||
//
|
||||
// Test if the platform supports un-aligned access and if the endianness is the same
|
||||
//
|
||||
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
||||
//
|
||||
// Read the current value
|
||||
//
|
||||
bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr);
|
||||
|
||||
//
|
||||
// Increment the current pointer
|
||||
//
|
||||
pHandle->pCurrentPtr += sizeof(int32_t);
|
||||
|
||||
return bytesValues[0];
|
||||
#else
|
||||
//
|
||||
// Read the each bytes
|
||||
//
|
||||
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
||||
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
||||
|
||||
//
|
||||
// Store data according to platform endianness
|
||||
//
|
||||
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
||||
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
||||
#else
|
||||
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0;
|
||||
}
|
||||
|
||||
float sbgtc::sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
FloatNint floatInt;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float))
|
||||
{
|
||||
//
|
||||
// Read the float as an uint32_t
|
||||
//
|
||||
floatInt.valU = sbgStreamBufferReadUint32LE(pHandle);
|
||||
|
||||
//
|
||||
// Return the float using an union to avoid compiler cast
|
||||
//
|
||||
return floatInt.valF;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Read an double from a stream buffer (Little endian version).
|
||||
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
||||
* \return The read value or 0 if we have an error.
|
||||
*/
|
||||
double sbgtc::sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
DoubleNint doubleInt;
|
||||
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
// assert(pHandle);
|
||||
|
||||
//
|
||||
// Test if we haven't already an error
|
||||
//
|
||||
if (pHandle->errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if we can access this item
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double))
|
||||
{
|
||||
//
|
||||
// Read the float as an uint64_t
|
||||
//
|
||||
doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle);
|
||||
|
||||
//
|
||||
// Return the double using an union to avoid compiler cast
|
||||
//
|
||||
return doubleInt.valF;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have a buffer overflow so return 0
|
||||
//
|
||||
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// If we are here, it means we have an error so return 0
|
||||
//
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return error code
|
||||
//
|
||||
return pHandle->errorCode;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the space left in bytes
|
||||
//
|
||||
return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle);
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetSize(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the linked buffer size
|
||||
//
|
||||
return pHandle->bufferSize;
|
||||
}
|
||||
|
||||
size_t sbgtc::sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
//
|
||||
// Return the number of bytes between the begin of the stream and the current pointer
|
||||
//
|
||||
return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr);
|
||||
}
|
||||
|
||||
void * sbgtc::sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
|
||||
{
|
||||
//
|
||||
// Check input parameters
|
||||
//
|
||||
//assert(pHandle);
|
||||
|
||||
return pHandle->pBufferPtr;
|
||||
}
|
||||
67
Source_Files/sbgcrc.cpp
Normal file
67
Source_Files/sbgcrc.cpp
Normal file
@ -0,0 +1,67 @@
|
||||
#include "Header_Files/sbgcrc.h"
|
||||
|
||||
|
||||
uint16_t sbgtc::sbgCrc16Compute(const void *pData, size_t dataSize)
|
||||
{
|
||||
SbgCrc16 crcInst;
|
||||
|
||||
//
|
||||
// Initialize the CRC system
|
||||
//
|
||||
sbgCrc16Initialize(&crcInst);
|
||||
|
||||
//
|
||||
// Compute the CRC
|
||||
//
|
||||
sbgCrc16Update(&crcInst, pData, dataSize);
|
||||
|
||||
//
|
||||
// Return it
|
||||
//
|
||||
return sbgCrc16Get(&crcInst);
|
||||
}
|
||||
|
||||
void sbgtc::sbgCrc16Initialize(SbgCrc16 *pInstance)
|
||||
{
|
||||
//
|
||||
// Test input argument
|
||||
//
|
||||
//assert(pInstance);
|
||||
|
||||
*pInstance = 0;
|
||||
}
|
||||
|
||||
void sbgtc::sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize)
|
||||
{
|
||||
const uint8_t *pBuffer = (const uint8_t*)pData;
|
||||
uint8_t index;
|
||||
size_t i;
|
||||
|
||||
//
|
||||
// Test input arguments
|
||||
//
|
||||
//assert(pInstance);
|
||||
//assert(pData);
|
||||
|
||||
//
|
||||
// For each byte in our buffer
|
||||
//
|
||||
for (i = 0; i < dataSize; i++)
|
||||
{
|
||||
//
|
||||
// Update the current CRC
|
||||
//
|
||||
index = (pBuffer[i] ^ *pInstance) & 0xFF;
|
||||
*pInstance = crc16LookupTable[index] ^ (*pInstance >> 8);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t sbgtc::sbgCrc16Get(const SbgCrc16 *pInstance)
|
||||
{
|
||||
return *pInstance;
|
||||
}
|
||||
|
||||
uint32_t sbgtc::sbgCrc32Get(const SbgCrc32 *pInstance)
|
||||
{
|
||||
return *pInstance;
|
||||
}
|
||||
131
Source_Files/sbglogparse.cpp
Normal file
131
Source_Files/sbglogparse.cpp
Normal file
@ -0,0 +1,131 @@
|
||||
#include "Header_Files/sbglogparse.h"
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData)
|
||||
{
|
||||
//assert(pInputStream);
|
||||
//assert(pOutputData);
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData)
|
||||
{
|
||||
// assert(pInputStream);
|
||||
// assert(pOutputData);
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream);
|
||||
pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream);
|
||||
pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \param[in] pInputStream Input stream buffer to read the payload from.
|
||||
* \param[out] pOutputData Pointer on the output structure that stores parsed data.
|
||||
* \return SBG_NO_ERROR if the payload has been parsed.
|
||||
*/
|
||||
SbgErrorCode sbgtc::sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData)
|
||||
{
|
||||
|
||||
//
|
||||
// Read the frame payload
|
||||
//
|
||||
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
|
||||
pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream);
|
||||
pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
|
||||
|
||||
//
|
||||
// Test if we have a additional information such as base station id (since version 1.4)
|
||||
//
|
||||
if (sbgStreamBufferGetSpace(pInputStream) >= 5)
|
||||
{
|
||||
//
|
||||
// Read the additional information
|
||||
//
|
||||
pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream);
|
||||
pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Default the additional information
|
||||
//
|
||||
pOutputData->numSvUsed = 0;
|
||||
pOutputData->baseStationId = 0xFFFF;
|
||||
pOutputData->differentialAge = 0xFFFF;
|
||||
}
|
||||
|
||||
//
|
||||
// Return if any error has occurred while parsing the frame
|
||||
//
|
||||
return sbgStreamBufferGetLastError(pInputStream);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
845
Source_Files/sbgrecorder.cpp
Normal file
845
Source_Files/sbgrecorder.cpp
Normal file
@ -0,0 +1,845 @@
|
||||
#include "Header_Files/sbgrecorder.h"
|
||||
#include <iostream>
|
||||
#include <string.h>
|
||||
#include <QDebug>
|
||||
|
||||
typedef unsigned char BYTE;
|
||||
|
||||
|
||||
using namespace sbgtc;
|
||||
|
||||
|
||||
sbgtc::SbgRecorder::SbgRecorder()
|
||||
{
|
||||
connect(this, SIGNAL(sbgReady(double,QString)),this, SLOT(onSbgReady(double,QString)));
|
||||
|
||||
m_bIsSbgReady = false;
|
||||
m_bIsRecordHyperspecatralImage = false;
|
||||
m_bIsNAV_POSITION_MODE = false;
|
||||
m_bIsAccuracyLessThan7 = false;
|
||||
m_bIsSyncSystemTimeBaseGpstime = false;
|
||||
|
||||
m_iSolutionMode=SBG_ECOM_SOL_MODE_UNINITIALIZED;
|
||||
m_iSolutionModeCounter=0;
|
||||
|
||||
m_iSbgState=0;
|
||||
|
||||
std::cout<<"SbgRecorder::SbgRecorder(construction) run!"<<std::endl;
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::openSerialPort()
|
||||
{
|
||||
QList<QSerialPortInfo> infos = QSerialPortInfo::availablePorts();
|
||||
|
||||
//std::cout<<"number of availablePorts:"<<infos.size()<<std::endl;
|
||||
|
||||
//只能是大疆psdk程序发送命令给本程序, 而大疆子自己需要一个串口, 所以当只有一个串口时, sbg的就没有串口可用
|
||||
// if(infos.size()==0||infos.size()==1)
|
||||
// {
|
||||
// m_iSbgState=0;
|
||||
// emit serialPortStatus(m_iSbgState);
|
||||
// return;
|
||||
// }
|
||||
|
||||
//如果在构造函数中创建m_serial就会出现错误:
|
||||
//QObject: Cannot create children for a parent that is in a different thread.
|
||||
//(Parent is QSerialPort(0x2e31b20), parent's thread is QThread(0x2e2f130), current thread is QThread(0x2e31110)
|
||||
m_serial=new QSerialPort();
|
||||
|
||||
if(m_serial->isOpen())//如果串口已经打开了 先给他关闭了
|
||||
{
|
||||
m_serial->clear();
|
||||
m_serial->close();
|
||||
}
|
||||
|
||||
// QString portname = "sbg_serial_port";
|
||||
QString portname = "ttyS4";
|
||||
|
||||
m_serial->setPortName(portname);
|
||||
m_serial->open(QIODevice::ReadWrite);
|
||||
|
||||
|
||||
bool x=m_serial->setBaudRate(921600);//
|
||||
if(x)
|
||||
{
|
||||
std::cout<<"波特率被成功设置为:"<<m_serial->baudRate()<<std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"波特率设置失败!"<<std::endl;
|
||||
|
||||
m_iSbgState=1;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
return;
|
||||
}
|
||||
|
||||
m_iSbgState=2;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::closeSerialPort()
|
||||
{
|
||||
if(m_iSbgState>0)
|
||||
{
|
||||
if(m_iSbgState==3)
|
||||
stopRecordSbg();
|
||||
|
||||
m_serial->close();
|
||||
m_iSbgState=0;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::onSbgReady(double second,QString baseFileName)
|
||||
{
|
||||
m_bIsSbgReady=true;
|
||||
m_bIsRecordHyperspecatralImage=false;
|
||||
}
|
||||
|
||||
QByteArray sbgtc::SbgRecorder::merge(QByteArray sbgMessage)
|
||||
{
|
||||
// BYTE mC = 0x0C;
|
||||
// BYTE mD = 0x0D;
|
||||
// BYTE mOut = 0x0C<<4|0x0D;
|
||||
// BYTE mOut1 = mC<<4|mD;
|
||||
|
||||
QByteArray x;
|
||||
QByteArray y;
|
||||
x.resize(sbgMessage.size()/2);
|
||||
y.resize(sbgMessage.size()/2);
|
||||
for(int i=0;i<sbgMessage.size()/2;i++)
|
||||
{
|
||||
char tmp=sbgMessage.at(i*2);
|
||||
char tmp2=tmp<<4;
|
||||
|
||||
x[i]=sbgMessage[i*2]<<4|sbgMessage[i*2+1];
|
||||
y[i]=sbgMessage[i*2]<<4;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
bool sbgtc::SbgRecorder::verify(int index, QByteArray sbgMessage)
|
||||
{
|
||||
if(index<0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
//std::cout<<"index:"<<index<<std::endl;
|
||||
//std::cout<<"count:"<<count<<std::endl;
|
||||
//return;
|
||||
}
|
||||
|
||||
int scale=2;
|
||||
|
||||
QByteArray msgId=sbgMessage.mid(index+2*scale,2);
|
||||
QByteArray classId=sbgMessage.mid(index+3*scale,2);
|
||||
|
||||
QByteArray lengthOfData=sbgMessage.mid(index+4*scale,4);
|
||||
std::reverse(lengthOfData.begin(), lengthOfData.end());
|
||||
int tmp=lengthOfData.toInt();//?????????????
|
||||
|
||||
QByteArray crc=sbgMessage.mid(index+(6+tmp)*scale,4);
|
||||
QByteArray etx=sbgMessage.mid(index+(6+tmp+2)*scale,2);//end of frame
|
||||
|
||||
if(etx=="33")
|
||||
{
|
||||
std::cout<<"结尾符正确!"<<std::endl;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"结尾符不正确!--------"<<std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
sbgtc::SbgErrorCode sbgtc::SbgRecorder::extractOneValidFrame(rawBuffer *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize)
|
||||
{
|
||||
SbgErrorCode errorCode = SBG_NOT_READY;
|
||||
SbgStreamBuffer inputStream;
|
||||
bool syncFound;
|
||||
size_t payloadSize = 0;
|
||||
uint16_t frameCrc;//
|
||||
uint16_t computedCrc;//
|
||||
//size_t i;
|
||||
|
||||
uint8_t receivedMsgClass;
|
||||
uint8_t receivedMsg;
|
||||
size_t payloadOffset;
|
||||
|
||||
while (pHandle->rxBufferSize > 0)
|
||||
{
|
||||
//
|
||||
// For now, we haven't found any start of frame
|
||||
//
|
||||
syncFound = false;
|
||||
|
||||
//
|
||||
// To find a valid start of frame we need at least 2 bytes in the reception buffer
|
||||
//
|
||||
if (pHandle->rxBufferSize >= 2)
|
||||
{
|
||||
//
|
||||
// Try to find a valid start of frame by looking for SYNC_1 and SYNC_2 chars
|
||||
//
|
||||
for (int i = 0; i < pHandle->rxBufferSize-1; i++)
|
||||
{
|
||||
//
|
||||
// A valid start of frame should begin with SYNC and when STX chars
|
||||
//
|
||||
if ( (pHandle->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pHandle->rxBuffer[i+1] == SBG_ECOM_SYNC_2) )
|
||||
{
|
||||
//
|
||||
// We have found the sync char, test if we have dummy bytes at the begining of the frame
|
||||
//
|
||||
if (i > 0)
|
||||
{
|
||||
//
|
||||
// Remove all dumy received bytes before the begining of the frame
|
||||
//
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+i, pHandle->rxBufferSize-i);
|
||||
pHandle->rxBufferSize = pHandle->rxBufferSize-i;
|
||||
}
|
||||
|
||||
//
|
||||
// The sync has been found
|
||||
//
|
||||
syncFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// Check if a valid start of frame has been found
|
||||
//
|
||||
if (syncFound)
|
||||
{
|
||||
//
|
||||
// A valid start of frame has been found, try to extract the frame if we have at least a whole frame.
|
||||
//
|
||||
if (pHandle->rxBufferSize < 8)
|
||||
{
|
||||
//
|
||||
// Don't have enough data for a valid frame
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
//
|
||||
// Initialize an input stream buffer to parse the received frame
|
||||
//
|
||||
sbgStreamBufferInitForRead(&inputStream, pHandle->rxBuffer, pHandle->rxBufferSize);
|
||||
|
||||
//
|
||||
// Skip both the Sync 1 and Sync 2 chars
|
||||
//
|
||||
sbgStreamBufferSeek(&inputStream, sizeof(uint8_t)*2, SB_SEEK_CUR_INC);
|
||||
|
||||
//
|
||||
// Read the command
|
||||
//
|
||||
receivedMsg = sbgStreamBufferReadUint8LE(&inputStream);
|
||||
receivedMsgClass = sbgStreamBufferReadUint8LE(&inputStream);
|
||||
|
||||
//
|
||||
// Read the payload size
|
||||
//
|
||||
payloadSize = (uint16_t)sbgStreamBufferReadUint16LE(&inputStream);
|
||||
|
||||
//
|
||||
// Check that the payload size is valid
|
||||
//
|
||||
if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE)
|
||||
{
|
||||
//
|
||||
// Check if we have received the whole frame
|
||||
//
|
||||
if (pHandle->rxBufferSize < payloadSize+9)
|
||||
{
|
||||
//
|
||||
// Don't have received the whole frame
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
//
|
||||
// We should have the whole frame so for now, skip the payload part but before store the current cursor
|
||||
//
|
||||
payloadOffset = sbgStreamBufferTell(&inputStream);
|
||||
sbgStreamBufferSeek(&inputStream, payloadSize, SB_SEEK_CUR_INC);
|
||||
|
||||
//
|
||||
// Read the frame CRC
|
||||
//
|
||||
frameCrc = sbgStreamBufferReadUint16LE(&inputStream);
|
||||
|
||||
//
|
||||
// Read and test the frame ETX
|
||||
//
|
||||
if (sbgStreamBufferReadUint8LE(&inputStream) == SBG_ECOM_ETX)//
|
||||
{
|
||||
//
|
||||
// Go back at the beginning of the payload part
|
||||
//
|
||||
sbgStreamBufferSeek(&inputStream, payloadOffset, SB_SEEK_SET);
|
||||
|
||||
//
|
||||
// We have a frame so return the received command if needed even if the CRC is still not validated
|
||||
//
|
||||
if (pMsg)
|
||||
{
|
||||
*pMsg = receivedMsg;
|
||||
}
|
||||
if (pMsgClass)
|
||||
{
|
||||
*pMsgClass = receivedMsgClass;
|
||||
}
|
||||
|
||||
//
|
||||
// Compute the CRC of the received frame (Skip SYNC 1 and SYNC 2 chars)
|
||||
//
|
||||
computedCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&inputStream)) + 2, payloadSize + 4);//??????????????????????????????????????????????????????????????????
|
||||
|
||||
//
|
||||
// Check if the received frame has a valid CRC
|
||||
//
|
||||
if (frameCrc == computedCrc)//frameCrc == computedCrc??????????????????????????????????????????????????????????????????????????????????????????????????????
|
||||
{
|
||||
//
|
||||
// Extract the payload if needed
|
||||
//
|
||||
if (payloadSize > 0)
|
||||
{
|
||||
//
|
||||
// Check if input parameters are valid
|
||||
//
|
||||
if ( (pData) && (pSize) )
|
||||
{
|
||||
//
|
||||
// Check if we have enough space to store the payload
|
||||
//
|
||||
if (payloadSize <= maxSize)
|
||||
{
|
||||
//
|
||||
// Copy the payload and return the payload size
|
||||
//
|
||||
*pSize = payloadSize;
|
||||
memcpy(pData, sbgStreamBufferGetCursor(&inputStream), payloadSize);
|
||||
errorCode = SBG_NO_ERROR;
|
||||
|
||||
//std::cout<<"receive valid frame!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Not enough space to store the payload, we will just drop the received data
|
||||
//
|
||||
errorCode = SBG_BUFFER_OVERFLOW;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
errorCode = SBG_NULL_POINTER;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// No payload but the frame has been read successfully
|
||||
//
|
||||
errorCode = SBG_NO_ERROR;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have an invalid frame CRC and we will directly return this error
|
||||
//
|
||||
errorCode = SBG_INVALID_CRC;
|
||||
}
|
||||
|
||||
//
|
||||
// We have read a whole valid frame so remove it from the buffer
|
||||
// First, test if the reception buffer contains more than just the current frame
|
||||
//
|
||||
if (pHandle->rxBufferSize > payloadSize+9)
|
||||
{
|
||||
//
|
||||
// We remove the read frame but we keep the remaining data
|
||||
//
|
||||
pHandle->rxBufferSize = pHandle->rxBufferSize-(payloadSize+9);
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+payloadSize+9, pHandle->rxBufferSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// We have parsed the whole received buffer so just empty it
|
||||
//
|
||||
pHandle->rxBufferSize = 0;
|
||||
}
|
||||
|
||||
//
|
||||
// We have at least found a complete frame
|
||||
//
|
||||
return errorCode;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// The ETX char hasn't been found where it should be
|
||||
//
|
||||
errorCode = SBG_INVALID_FRAME;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// The payload size isn't valid
|
||||
//
|
||||
errorCode = SBG_INVALID_FRAME;
|
||||
}
|
||||
|
||||
//
|
||||
// Frame size invalid or the found frame is invalid so we should have incorrectly detected a start of frame.
|
||||
// Remove the SYNC 1 and SYNC 2 chars to retry to find a new frame
|
||||
//
|
||||
pHandle->rxBufferSize -= 2;
|
||||
memmove(pHandle->rxBuffer, pHandle->rxBuffer+2, pHandle->rxBufferSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Unable to find a valid start of frame so check if the last byte is a SYNC char in order to keep it for next time
|
||||
//
|
||||
if (pHandle->rxBuffer[pHandle->rxBufferSize-1] == SBG_ECOM_SYNC_1)
|
||||
{
|
||||
//
|
||||
// Report the SYNC char and discard all other bytes in the buffer
|
||||
//
|
||||
pHandle->rxBuffer[0] = SBG_ECOM_SYNC_1;
|
||||
pHandle->rxBufferSize = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Discard the whole buffer
|
||||
//
|
||||
pHandle->rxBufferSize = 0;
|
||||
}
|
||||
|
||||
//
|
||||
// Unable to find a frame
|
||||
//
|
||||
return SBG_NOT_READY;//？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// The whole buffer has been paresed and no valid frame has been found
|
||||
//
|
||||
return SBG_NOT_READY;
|
||||
}
|
||||
|
||||
|
||||
sbgtc::SbgErrorCode sbgtc::SbgRecorder::sbgBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData)
|
||||
{
|
||||
SbgErrorCode errorCode = SBG_NO_ERROR;
|
||||
SbgStreamBuffer inputStream;
|
||||
|
||||
// assert(pPayload);
|
||||
// assert(payloadSize > 0);
|
||||
// assert(pOutputData);
|
||||
|
||||
//
|
||||
// Create an input stream buffer that points to the frame payload so we can easily parse it's content
|
||||
//
|
||||
sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize);
|
||||
|
||||
//
|
||||
// Handle the different classes of messages differently
|
||||
//
|
||||
if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0)
|
||||
{
|
||||
//
|
||||
// Parse the incoming log according to its type
|
||||
//
|
||||
switch (msg)
|
||||
{
|
||||
case SBG_ECOM_LOG_STATUS:
|
||||
// errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_IMU_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_IMU_SHORT:
|
||||
// errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_EULER:
|
||||
errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_QUAT:
|
||||
// errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EKF_NAV:
|
||||
// errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_SHIP_MOTION:
|
||||
case SBG_ECOM_LOG_SHIP_MOTION_HP:
|
||||
// errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_ODO_VEL:
|
||||
// errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_UTC_TIME:
|
||||
errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_VEL:
|
||||
case SBG_ECOM_LOG_GPS2_VEL:
|
||||
// errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_POS:
|
||||
case SBG_ECOM_LOG_GPS2_POS:
|
||||
errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_HDT:
|
||||
case SBG_ECOM_LOG_GPS2_HDT:
|
||||
// errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_GPS1_RAW:
|
||||
case SBG_ECOM_LOG_GPS2_RAW:
|
||||
// errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_MAG:
|
||||
// errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_MAG_CALIB:
|
||||
// errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DVL_BOTTOM_TRACK:
|
||||
// errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DVL_WATER_TRACK:
|
||||
// errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_AIR_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_USBL:
|
||||
// errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DEPTH:
|
||||
// errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData);
|
||||
break;
|
||||
case SBG_ECOM_LOG_EVENT_A:
|
||||
case SBG_ECOM_LOG_EVENT_B:
|
||||
case SBG_ECOM_LOG_EVENT_C:
|
||||
case SBG_ECOM_LOG_EVENT_D:
|
||||
case SBG_ECOM_LOG_EVENT_E:
|
||||
case SBG_ECOM_LOG_EVENT_OUT_A:
|
||||
case SBG_ECOM_LOG_EVENT_OUT_B:
|
||||
// errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker);
|
||||
break;
|
||||
case SBG_ECOM_LOG_DIAG:
|
||||
// errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = SBG_ERROR;
|
||||
}
|
||||
}
|
||||
// else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1)
|
||||
// {
|
||||
// //
|
||||
// // Parse the message depending on the message ID
|
||||
// //
|
||||
// switch ((SbgEComLog1)msg)
|
||||
// {
|
||||
// case SBG_ECOM_LOG_FAST_IMU_DATA:
|
||||
// errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData);
|
||||
// break;
|
||||
// default:
|
||||
// errorCode = SBG_ERROR;
|
||||
// }
|
||||
// }
|
||||
else
|
||||
{
|
||||
//
|
||||
// Un-handled message class
|
||||
//
|
||||
errorCode = SBG_ERROR;
|
||||
}
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
|
||||
void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
|
||||
{
|
||||
rawBuffer pHandleTmp=initRawBuffer(sbgMessage);
|
||||
rawBuffer * pHandle=&pHandleTmp;
|
||||
|
||||
uint8_t receivedMsgClass;
|
||||
uint8_t receivedMsg;
|
||||
size_t payloadSize;
|
||||
uint8_t pData[SBG_ECOM_MAX_PAYLOAD_SIZE]={0};//payloadData
|
||||
|
||||
|
||||
//重要的第1步：提取有效帧
|
||||
SbgErrorCode errorCode = extractOneValidFrame(pHandle,&receivedMsgClass,&receivedMsg,pData,&payloadSize,sizeof(pData));
|
||||
|
||||
|
||||
|
||||
//SBG_INVALID_CRC,SBG_NO_ERROR,SBG_NOT_READY
|
||||
if(errorCode==SBG_NOT_READY)
|
||||
return;
|
||||
if(errorCode==SBG_INVALID_CRC)
|
||||
std::cout<<"SBG_INVALID_CRC: ------------------------------------------------------"<<std::endl;
|
||||
|
||||
|
||||
//重要的第2步：解析有效帧
|
||||
SbgBinaryLogData logData;
|
||||
memset(&logData,0,sizeof(logData));
|
||||
sbgBinaryLogParse((SbgEComClass)receivedMsgClass,(SbgEComMsgId)receivedMsg,pData,payloadSize,&logData);
|
||||
|
||||
|
||||
//判断模式是否为: NAV_POSITION
|
||||
if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
|
||||
{
|
||||
m_iSolutionModeCounter++;//
|
||||
|
||||
uint32_t status=logData.ekfEulerData.status;
|
||||
uint32_t mode=status>>24;//?????????????????????????????????????
|
||||
// uint32_t mode=status;//这是错的
|
||||
|
||||
//一秒钟发射一次mode
|
||||
if(m_iSolutionModeCounter%200 == 0)
|
||||
{
|
||||
emit sbgSolutionModeSignal(mode);
|
||||
|
||||
switch (mode)
|
||||
{
|
||||
case SBG_ECOM_SOL_MODE_UNINITIALIZED:
|
||||
std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
|
||||
std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_AHRS:
|
||||
std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
|
||||
std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_NAV_POSITION:
|
||||
std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
switch (mode)
|
||||
{
|
||||
case SBG_ECOM_SOL_MODE_UNINITIALIZED:
|
||||
// std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
|
||||
m_bIsNAV_POSITION_MODE=false;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
|
||||
// std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
|
||||
m_bIsNAV_POSITION_MODE=false;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_AHRS:
|
||||
// std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
|
||||
m_bIsNAV_POSITION_MODE=false;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
|
||||
// std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
|
||||
m_bIsNAV_POSITION_MODE=false;
|
||||
break;
|
||||
case SBG_ECOM_SOL_MODE_NAV_POSITION:
|
||||
// std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;
|
||||
m_bIsNAV_POSITION_MODE=true;
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_GPS1_POS)
|
||||
{
|
||||
// std::cout<<"纬度精度为："<<logData.gpsPosData.latitudeAccuracy<<std::endl;
|
||||
|
||||
float maximal=0;
|
||||
float latitudeAccuracy=logData.gpsPosData.latitudeAccuracy;
|
||||
float longitudeAccuracy=logData.gpsPosData.longitudeAccuracy;
|
||||
float altitudeAccuracy=logData.gpsPosData.altitudeAccuracy;
|
||||
|
||||
|
||||
if(latitudeAccuracy<longitudeAccuracy)
|
||||
maximal = longitudeAccuracy;
|
||||
else
|
||||
maximal = latitudeAccuracy;
|
||||
|
||||
if(maximal<altitudeAccuracy)
|
||||
maximal = altitudeAccuracy;
|
||||
|
||||
emit sbgAccuracySignal(static_cast<int>(maximal));
|
||||
|
||||
if(maximal<7)
|
||||
{
|
||||
m_bIsAccuracyLessThan7=true;
|
||||
}
|
||||
}
|
||||
else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME && m_bIsAccuracyLessThan7 && !m_bIsSbgReady)
|
||||
{
|
||||
//std::cout<<"0pen time:"<<(long)logData.utcData.timeStamp/1000000<<std::endl;
|
||||
|
||||
uint16_t year = logData.utcData.year;
|
||||
int month = logData.utcData.month;
|
||||
int day = logData.utcData.day;
|
||||
int hour = logData.utcData.hour;
|
||||
int minute = logData.utcData.minute;
|
||||
int second = logData.utcData.second;
|
||||
// int32_t nanoSecond = logData.utcData.nanoSecond;
|
||||
// std::cout<<"gps时间为："<<year<<","<<month<<","<<day<<","<<hour<<","<<minute<<","<<second<<","<<nanoSecond<<std::endl;
|
||||
|
||||
char setGpsTimeCommand[256];
|
||||
sprintf(setGpsTimeCommand,"date --set=\"%d%02d%02d %02d:%02d:%02d\"",year,month,day,hour,minute,second);//02中的2代表2位数字，0代表以0补全
|
||||
|
||||
system(setGpsTimeCommand);//
|
||||
|
||||
m_bIsSyncSystemTimeBaseGpstime=true;
|
||||
}
|
||||
else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
|
||||
{
|
||||
// std::cout<<"1111111111111"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
// std::cout<<"receivedMsg:"<<(int)receivedMsg<<std::endl;
|
||||
}
|
||||
else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_QUAT)
|
||||
{
|
||||
//std::cout<<"1111111111111------"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
}
|
||||
else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_NAV)
|
||||
{
|
||||
//std::cout<<"0000000000000"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//控制开始采集高光谱影像,m_bIsNAV_POSITION_MODE &&
|
||||
if(m_bIsRecordHyperspecatralImage && m_bIsSyncSystemTimeBaseGpstime && receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME)
|
||||
{
|
||||
m_baseFileName=getFileNameBaseOnTime();
|
||||
emit sbgReady(calculateTimeDifferenceBetweenSystemAndSbg(logData),m_baseFileName);
|
||||
}
|
||||
}
|
||||
|
||||
double sbgtc::SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg(SbgBinaryLogData logData)//
|
||||
{
|
||||
std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)logData.utcData.second<<","<<(int)logData.utcData.nanoSecond<<std::endl;
|
||||
|
||||
//获取sbg(gps)时间（纳秒）
|
||||
double year=logData.utcData.year;
|
||||
double month=logData.utcData.month;
|
||||
double day=logData.utcData.day;
|
||||
|
||||
double hour=logData.utcData.hour;//+8
|
||||
double minute=logData.utcData.minute;
|
||||
double second=logData.utcData.second;
|
||||
double nanoSecond=logData.utcData.nanoSecond;
|
||||
|
||||
double timeTemp=day*24*60*60+hour*60*60+minute*60+second;//(day-1)
|
||||
double time=timeTemp+nanoSecond/1000000000;
|
||||
|
||||
printf("SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg--secocnd: %f\n", time);
|
||||
//std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg: "<< time <<std::endl;
|
||||
|
||||
|
||||
//获取系统时间（纳秒）
|
||||
struct timespec systemTime;
|
||||
clock_gettime(CLOCK_REALTIME,&systemTime);
|
||||
tm systemTime_rili;
|
||||
localtime_r(&systemTime.tv_sec, &systemTime_rili);
|
||||
|
||||
double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
|
||||
double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;
|
||||
|
||||
|
||||
//计算系统时间和gps时间之间的差距
|
||||
double TimeDifferenceBetweenSystemAndSbg = nanosecondSystem - time;
|
||||
|
||||
return TimeDifferenceBetweenSystemAndSbg;
|
||||
}
|
||||
|
||||
int sbgtc::SbgRecorder::getSbgState() const
|
||||
{
|
||||
return m_iSbgState;
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::startRecordSbg()
|
||||
{
|
||||
openSerialPort();
|
||||
if(m_iSbgState!=2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_iSbgState=3;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
|
||||
m_baseFileName=getFileNameBaseOnTime();
|
||||
|
||||
QString sbgFileName=m_baseFileName+".sbg";
|
||||
|
||||
FILE * fileHandle=fopen(sbgFileName.toStdString().c_str(),"w+b");
|
||||
|
||||
QByteArray requestData;
|
||||
m_bRecordControl=true;
|
||||
while (m_bRecordControl)
|
||||
{
|
||||
//std::cout<<"SbgRecorder::startRecordSbg--------------:"<<std::endl;
|
||||
if(m_serial->waitForReadyRead())
|
||||
{
|
||||
//requestData.resize(m_serial->size());
|
||||
requestData = m_serial->readAll();
|
||||
fwrite(requestData.data(),requestData.size(),1,fileHandle);//????????????
|
||||
|
||||
// if(!m_bIsSbgReady)
|
||||
// {
|
||||
// parseSbgMessage(&requestData);
|
||||
// }
|
||||
parseSbgMessage(&requestData);//边采集惯导数据边解析，并不会导致惯导漏帧
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"SbgRecorder::startRecordSbg----:Wait write response timeout"<<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
m_bIsSbgReady=false;//确保每次执行函数SbgRecorder::startRecordSbg,都会执行parseSbgMessage(&requestData);
|
||||
|
||||
fclose(fileHandle);
|
||||
|
||||
m_iSbgState=2;
|
||||
emit serialPortStatus(m_iSbgState);
|
||||
}
|
||||
|
||||
void sbgtc::SbgRecorder::stopRecordSbg()
|
||||
{
|
||||
m_bRecordControl=false;
|
||||
}
|
||||
|
||||
void SbgRecorder::startRecordHyperspectral()
|
||||
{
|
||||
m_bIsRecordHyperspecatralImage=true;
|
||||
std::cout<<"SbgRecorder::startRecordHyperspectral----m_bIsRecordHyperspecatralImage设置为true!"<<std::endl;
|
||||
}
|
||||
|
||||
void SbgRecorder::stopRecordHyperspectral()
|
||||
{
|
||||
m_bIsRecordHyperspecatralImage=false;
|
||||
}
|
||||
682
Source_Files/udpserver.cpp
Normal file
682
Source_Files/udpserver.cpp
Normal file
@ -0,0 +1,682 @@
|
||||
#include "Header_Files/udpserver.h"
|
||||
|
||||
//using namespace sbgtc;
|
||||
|
||||
UdpServer::UdpServer()
|
||||
{
|
||||
m_udpSocket = new QUdpSocket(this);
|
||||
m_udpSocket->bind(45454, QUdpSocket::ShareAddress);
|
||||
connect(m_udpSocket, SIGNAL(readyRead()),this, SLOT(processPendingDatagrams()));
|
||||
|
||||
m_sbgMagCibWorkThread = new sbgMagCibWorkThread();
|
||||
|
||||
m_RecordSbgThread=new QThread();
|
||||
m_sbgRecorder=new sbgtc::SbgRecorder();
|
||||
m_sbgRecorder->moveToThread(m_RecordSbgThread);
|
||||
m_RecordSbgThread->start();
|
||||
|
||||
m_RecordThread=new QThread();
|
||||
m_imager=new XimeaImager();
|
||||
m_imager->moveToThread(m_RecordThread);
|
||||
m_RecordThread->start();
|
||||
|
||||
m_CopyFileThread=new QThread();
|
||||
m_copyFile=new FileOperation();
|
||||
m_copyFile->moveToThread(m_CopyFileThread);
|
||||
m_CopyFileThread->start();
|
||||
|
||||
//系统采集步骤1:打开sbg串口并采集数据，打开光谱仪
|
||||
// connect(this, SIGNAL(systemStart()),m_sbgRecorder, SLOT(startRecordSbg()));
|
||||
connect(this, SIGNAL(systemStart()),m_imager, SLOT(openImger()));
|
||||
|
||||
connect(this, SIGNAL(systemStop()),m_sbgRecorder, SLOT(closeSerialPort()));//
|
||||
connect(this, SIGNAL(systemStop()),m_imager, SLOT(closeImger()));
|
||||
|
||||
|
||||
//系统采集步骤2:开始采集高光谱影像
|
||||
connect(this, SIGNAL(startRecordHyperspectralSignal()),m_sbgRecorder, SLOT(startRecordHyperspectral()));
|
||||
connect(m_sbgRecorder, SIGNAL(sbgReady(double,QString)),m_imager, SLOT(startRecord(double,QString)));
|
||||
connect(this, SIGNAL(recordXimeaOnlySignal(double,QString)),m_imager, SLOT(startRecord(double,QString)));
|
||||
|
||||
//系统采集步骤3:停止采集
|
||||
connect(m_imager, SIGNAL(recordFinished()),this, SLOT(onRecordFinished()));
|
||||
|
||||
//系统采集步骤4:拷贝文件
|
||||
connect(this, SIGNAL(startCopyFileSignal()),m_copyFile, SLOT(copyFile()));
|
||||
connect(this, SIGNAL(startDeleteFileSignal()),m_copyFile, SLOT(deleteFile()));
|
||||
|
||||
//系统采集步骤5:进程间通讯
|
||||
connect(m_sbgRecorder, SIGNAL(serialPortStatus(int)),this, SLOT(sendSerialPortStatus(int)));
|
||||
|
||||
connect(m_sbgRecorder, SIGNAL(sbgSolutionModeSignal(int)),this, SLOT(sendSbgSolutionModeState(int)));
|
||||
connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int)),this, SLOT(sendSbgAccuracyState(int)));
|
||||
|
||||
connect(m_imager, SIGNAL(ximeaImageStatus(int)),this, SLOT(sendXimeaImageStatus(int)));
|
||||
connect(m_sbgMagCibWorkThread, SIGNAL(magCalibStateSignal(int)),this, SLOT(sendSbgMagCalibState(int)));
|
||||
connect(m_copyFile, SIGNAL(copyFileStatus(int)),this, SLOT(sendCopyFileStatus(int)));
|
||||
|
||||
|
||||
|
||||
//当软件不正常关闭并且重启后，通知其他psdk程序
|
||||
m_clientIpAddress=QHostAddress(QHostAddress::LocalHost);
|
||||
sendSerialPortStatus(0);
|
||||
sendXimeaImageStatus(0);
|
||||
sendSbgMagCalibState(0);
|
||||
sendCopyFileStatus(0);
|
||||
|
||||
|
||||
|
||||
|
||||
std::cout<<"UdpServer::UdpServer--------:System ready!"<<std::endl;
|
||||
}
|
||||
|
||||
void UdpServer::processPendingDatagrams()
|
||||
{
|
||||
using namespace std;
|
||||
while (m_udpSocket->hasPendingDatagrams())
|
||||
{
|
||||
QByteArray datagram;
|
||||
|
||||
datagram.resize(m_udpSocket->pendingDatagramSize());
|
||||
m_udpSocket->readDatagram(datagram.data(), datagram.size(),&m_clientIpAddress, &m_clientPort);
|
||||
|
||||
|
||||
printf("接收数据字节数: %d.\n",datagram.size());
|
||||
|
||||
QList<QByteArray> datagramList=datagram.split(',');
|
||||
printf("有多少个list: %d.\n",datagramList.size());
|
||||
|
||||
QString instruction=datagramList[0].data();// QByteArray转QString方法1
|
||||
switch (instruction.toInt())
|
||||
{
|
||||
case 1://启动系统: 打开sbg串口并采集数据，打开光谱仪
|
||||
{
|
||||
std::cout<<"1代表启动系统!"<<std::endl;
|
||||
emit systemStart();
|
||||
|
||||
break;
|
||||
}
|
||||
case 2://关闭系统:关闭相机和sbg串口,关闭软件
|
||||
{
|
||||
std::cout<<"2代表关闭系统!"<<std::endl;
|
||||
|
||||
|
||||
if(m_sbgRecorder->getSbgState()>=1)
|
||||
{
|
||||
m_sbgRecorder->stopRecordSbg();
|
||||
}
|
||||
|
||||
|
||||
if(m_imager->getImagerState()>=1)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
emit systemStop();
|
||||
|
||||
//QCoreApplication::quit();
|
||||
break;
|
||||
}
|
||||
case 3://系统开始采集高光谱影像
|
||||
{
|
||||
//emit startRecordHyperspectralSignal();//真实的影像开始采集通过惯导中的信号(sbgReady)触发
|
||||
m_sbgRecorder->startRecordHyperspectral();
|
||||
|
||||
// if(m_sbgRecorder->getSbgState()==2)//开始采集前还需要判断相机的状态??????????????????????????????????????????
|
||||
// {
|
||||
|
||||
// }
|
||||
// else if(m_sbgRecorder->getSbgState()==3)
|
||||
// {
|
||||
// std::cout<<"系统已经开始采集!"<<std::endl;
|
||||
// }
|
||||
|
||||
break;
|
||||
}
|
||||
case 4://系统停止采集高光谱影像
|
||||
{
|
||||
std::cout<<"4代表系统停止采集高光谱影像!"<<std::endl;
|
||||
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=4)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 5://
|
||||
{
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
|
||||
{
|
||||
std::cout<<"5代表设置帧率!"<<std::endl;
|
||||
m_imager->setFramerate(datagramList[1].toFloat());
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 6://
|
||||
{
|
||||
if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
|
||||
{
|
||||
std::cout<<"6代表自动曝光!"<<std::endl;
|
||||
m_imager->autoExposure();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
if(datagramList[1].toInt()==1)
|
||||
{
|
||||
std::cout<<"拷贝数据!"<<std::endl;
|
||||
|
||||
emit startCopyFileSignal();
|
||||
}
|
||||
else if(datagramList[1].toInt()==0)
|
||||
{
|
||||
std::cout<<"删除数据!"<<std::endl;
|
||||
|
||||
emit startDeleteFileSignal();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
{
|
||||
std::cout<<"8代表磁场矫正!"<<std::endl;
|
||||
|
||||
if(datagramList[1].toInt()==1)
|
||||
{
|
||||
//magCalib();
|
||||
std::cout<<"8-1: 开始磁场矫正!"<<std::endl;
|
||||
|
||||
m_sbgMagCibWorkThread->start();
|
||||
}
|
||||
else if(datagramList[1].toInt()==0)
|
||||
{
|
||||
std::cout<<"8-0: 停止磁场矫正!"<<std::endl;
|
||||
|
||||
m_sbgMagCibWorkThread->m_iMagCalibStopControl=0;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 9:
|
||||
{
|
||||
std::cout<<"9代表仅采集影像!"<<std::endl;
|
||||
|
||||
if(datagramList[1].toInt()==1)
|
||||
{
|
||||
QString xx = getFileNameBaseOnTime();
|
||||
xx = xx + "testImage";
|
||||
emit recordXimeaOnlySignal(1000.0,xx);
|
||||
|
||||
// emit recordXimeaOnlySignal(1000.0,"testImage");
|
||||
}
|
||||
else if(datagramList[1].toInt()==0)
|
||||
{
|
||||
m_imager->stopRecord();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
std::cout<<">=9没有意义!"<<std::endl;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double UdpServer::getTimeDifferenceBetweenSystemAndSbg(double secondSbg)
|
||||
{
|
||||
// time_t timer;//time_t就是long int 类型
|
||||
// struct tm *tblock;
|
||||
// timer = time(NULL);//返回秒数(精度为秒)，从1970-1-1,00:00:00 可以当成整型输出或用于其它函数
|
||||
// tblock = localtime(&timer);
|
||||
// printf("Local time is: %s\n", asctime(tblock));
|
||||
|
||||
|
||||
|
||||
//https://blog.csdn.net/FUN6367/article/details/89787566
|
||||
struct timespec systemTime;
|
||||
clock_gettime(CLOCK_REALTIME,&systemTime);
|
||||
tm systemTime_rili;
|
||||
localtime_r(&systemTime.tv_sec, &systemTime_rili);
|
||||
|
||||
// std::cout<<"systemTime_rili--年: "<<systemTime_rili.tm_year+1900<<std::endl;
|
||||
// std::cout<<"systemTime_rili--月: "<<systemTime_rili.tm_mon+1<<std::endl;
|
||||
// std::cout<<"systemTime_rili--日: "<<systemTime_rili.tm_mday<<std::endl;
|
||||
// std::cout<<"systemTime_rili--时: "<<systemTime_rili.tm_hour<<std::endl;
|
||||
// std::cout<<"systemTime_rili--分: "<<systemTime_rili.tm_min<<std::endl;
|
||||
// std::cout<<"systemTime_rili--秒: "<<systemTime_rili.tm_sec<<std::endl;
|
||||
|
||||
// printf("Local time is: %s\n", asctime(&systemTime_rili));
|
||||
|
||||
|
||||
|
||||
double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
|
||||
double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;
|
||||
|
||||
printf("UdpServer::getTimeDifferenceBetweenSystemAndSbg------%f\n", nanosecondSystem-secondSbg);
|
||||
|
||||
return nanosecondSystem-secondSbg;
|
||||
}
|
||||
|
||||
void UdpServer::sender(int status)
|
||||
{
|
||||
QByteArray datagram2send;
|
||||
|
||||
datagram2send.operator =(QString::number(status).toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, m_clientPort+1);
|
||||
}
|
||||
|
||||
void UdpServer::sendSerialPortStatus(int serialPortStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< serialPortStatus <<std::endl;
|
||||
|
||||
std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< m_clientIpAddress.AnyIPv4 <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "sbg," + QString::number(serialPortStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendSbgMagCalibState(int SbgMagCalibState)
|
||||
{
|
||||
std::cout<<"UdpServer::sendSbgMagCalibState---------------------:"<< SbgMagCalibState <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "mag," + QString::number(SbgMagCalibState);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendSbgSolutionModeState(int SolutionMode)
|
||||
{
|
||||
std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "SolutionMode," + QString::number(SolutionMode);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendSbgAccuracyState(int Accuracy)
|
||||
{
|
||||
// std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "Accuracy," + QString::number(Accuracy);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendXimeaImageStatus(int ximeaImageStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendXimeaImageStatus---------------------:"<< ximeaImageStatus <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "ximea," + QString::number(ximeaImageStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::sendCopyFileStatus(int fileStatus)
|
||||
{
|
||||
std::cout<<"UdpServer::sendCopyFileStatus---------------------:"<< fileStatus <<std::endl;
|
||||
|
||||
QByteArray datagram2send;
|
||||
|
||||
QString status = "file," + QString::number(fileStatus);
|
||||
|
||||
datagram2send.operator =(status.toStdString().c_str());
|
||||
m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
|
||||
}
|
||||
|
||||
void UdpServer::onRecordFinished()
|
||||
{
|
||||
std::cout<<"UdpServer::onRecordFinished----------------:影像停止采集"<<std::endl;
|
||||
}
|
||||
|
||||
sbgMagCibWorkThread::sbgMagCibWorkThread()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void sbgMagCibWorkThread::displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults)
|
||||
{
|
||||
//
|
||||
// Display the magnetic calibration results
|
||||
//
|
||||
printf("\n\n======== Magnetic calibration report ========\n");
|
||||
|
||||
//
|
||||
// Convert the quality indicator to human readable output
|
||||
//
|
||||
switch (pMagCalibResults->quality)
|
||||
{
|
||||
case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL:
|
||||
{
|
||||
printf("Quality:\t\toptimal\n");
|
||||
signalWrap(7);
|
||||
break;
|
||||
}
|
||||
case SBG_ECOM_MAG_CALIB_QUAL_GOOD:
|
||||
{
|
||||
printf("Quality:\t\tgood\n");
|
||||
signalWrap(6);
|
||||
break;
|
||||
}
|
||||
case SBG_ECOM_MAG_CALIB_QUAL_POOR:
|
||||
{
|
||||
printf("Quality:\t\tpoor\n");
|
||||
signalWrap(5);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
printf("Quality:\t\tundefined\n");
|
||||
}
|
||||
|
||||
//
|
||||
// Convert the confidence indicator to human readable output
|
||||
//
|
||||
switch (pMagCalibResults->confidence)
|
||||
{
|
||||
case SBG_ECOM_MAG_CALIB_TRUST_HIGH:
|
||||
printf("Confidence:\t\thigh\n");
|
||||
break;
|
||||
case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM:
|
||||
printf("Confidence:\t\tmedium\n");
|
||||
break;
|
||||
case SBG_ECOM_MAG_CALIB_TRUST_LOW:
|
||||
printf("Confidence:\t\tlow\n");
|
||||
break;
|
||||
default:
|
||||
printf("Confidence:\t\tundefined\n");
|
||||
}
|
||||
|
||||
//
|
||||
// Print advanced status
|
||||
//
|
||||
printf("Advanced Status:\n");
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS)
|
||||
{
|
||||
printf("\t- Not enough valid points. Maybe you are moving too fast.\n");
|
||||
}
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS)
|
||||
{
|
||||
printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n");
|
||||
}
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE)
|
||||
{
|
||||
printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n");
|
||||
}
|
||||
|
||||
//
|
||||
// Test if we have a 2D or 3D calibration mode
|
||||
//
|
||||
if (mode == SBG_ECOM_MAG_CALIB_MODE_2D)
|
||||
{
|
||||
//
|
||||
// In 2D mode, a X or Y motion issue means we have too much motion
|
||||
//
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
|
||||
{
|
||||
printf("\t- Too much roll motion for a 2D magnetic calibration.\n");
|
||||
}
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
|
||||
{
|
||||
printf("\t- Too much pitch motion for a 2D magnetic calibration.\n");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// In 3D mode, a X or Y motion issue means we have not enough motion
|
||||
//
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
|
||||
{
|
||||
printf("\t- Not enough roll motion for a 3D magnetic calibration.\n");
|
||||
}
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
|
||||
{
|
||||
printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n");
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// Test if we had enough yaw motion to compute a calibration
|
||||
//
|
||||
if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE)
|
||||
{
|
||||
//
|
||||
// Test if we are in
|
||||
printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n");
|
||||
}
|
||||
|
||||
//
|
||||
// Display the number of points used to compute the magnetic calibration
|
||||
//
|
||||
printf("\n\n");
|
||||
printf("Used Points:\t%u\n", pMagCalibResults->numPoints);
|
||||
printf("Max Points:\t%u\n", pMagCalibResults->maxNumPoints);
|
||||
|
||||
//
|
||||
// Display magnetic field deviation errors
|
||||
//
|
||||
printf("\n\n-------------------------------------\n");
|
||||
printf("Magnetic field deviation report\n");
|
||||
printf("-------------------------------------\n");
|
||||
printf("\t\tMean\tStd\tMax\n");
|
||||
printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError);
|
||||
printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError);
|
||||
printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegF(pMagCalibResults->meanAccuracy), sbgRadToDegF(pMagCalibResults->stdAccuracy), sbgRadToDegF(pMagCalibResults->maxAccuracy));
|
||||
printf("\n\n\n");
|
||||
}
|
||||
|
||||
void sbgMagCibWorkThread::run()
|
||||
{
|
||||
m_iMagCalibStopControl = 1;
|
||||
|
||||
|
||||
SbgEComHandle comHandle;
|
||||
SbgErrorCode errorCode;
|
||||
SbgInterface sbgInterface;
|
||||
int32 retValue = 0;
|
||||
SbgEComDeviceInfo deviceInfo;
|
||||
SbgEComMagCalibResults magCalibResults;
|
||||
SbgEComMagCalibMode mode;
|
||||
|
||||
//
|
||||
// Create an interface:
|
||||
// We can choose either a serial for real time operation, or file for previously logged data parsing
|
||||
// Note interface closing is also differentiated !
|
||||
//
|
||||
errorCode = sbgInterfaceSerialCreate(&sbgInterface, "/dev/sbg_serial_port", 460800); // Example for Unix using a FTDI Usb2Uart converter
|
||||
//errorCode = sbgInterfaceSerialCreate(&sbgInterface, "COM23", 115200); // Example for Windows serial communication
|
||||
|
||||
//
|
||||
// Test that the interface has been created
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Create the sbgECom library and associate it with the created interfaces
|
||||
//
|
||||
errorCode = sbgEComInit(&comHandle, &sbgInterface);
|
||||
|
||||
//
|
||||
// Test that the sbgECom has been initialized
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
printf("sbgECom properly Initialized.\n");
|
||||
printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR);
|
||||
|
||||
//
|
||||
// Get device information
|
||||
//
|
||||
errorCode = sbgEComCmdGetInfo(&comHandle, &deviceInfo);
|
||||
|
||||
//
|
||||
// Display device information if no error
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
printf("Device : %0.9u found\n", deviceInfo.serialNumber);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get device information.\n");
|
||||
}
|
||||
|
||||
//
|
||||
// Define the calibration mode to perform
|
||||
//
|
||||
mode = SBG_ECOM_MAG_CALIB_MODE_3D;
|
||||
|
||||
//
|
||||
// Start / reset the acquisition of magnetic field data
|
||||
// Each time this command is called, the device is prepared to acquire a new set of magnetic field data
|
||||
// You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration
|
||||
//
|
||||
errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW);
|
||||
|
||||
//
|
||||
// Make sure that the magnetic calibration has started
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// The device is now acquiring some magnetic field data.
|
||||
// Wait for a user input before computing the magnetic calibration
|
||||
//
|
||||
printf("The device is acquiring magnetic field data.\n\nPress enter to stop the magnetic field acquisition.\n");
|
||||
//fgetc(stdin);//-------------------------------------------------------------------------------------------------------------------------------------------
|
||||
while (m_iMagCalibStopControl)
|
||||
{
|
||||
printf("h");
|
||||
|
||||
}
|
||||
|
||||
printf("kkkkkkkkkkkkkkkkkkkkkkkkkkkk\n");
|
||||
|
||||
//
|
||||
// Try to compute a magnetic calibration and get the results
|
||||
//
|
||||
errorCode = sbgEComCmdMagComputeCalib(&comHandle, &magCalibResults);
|
||||
|
||||
//
|
||||
// Make sure that we were able to get magnetic calibration results
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
//
|
||||
// Test if the device has computed a valid magnetic calibration
|
||||
//
|
||||
if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID)
|
||||
{
|
||||
//
|
||||
// Send the new magnetic calibration data
|
||||
//
|
||||
errorCode = sbgEComCmdMagSetCalibData(&comHandle, magCalibResults.offset, magCalibResults.matrix);
|
||||
|
||||
//
|
||||
// Make sure that the new magnetic calibration data has been updated
|
||||
//
|
||||
if (errorCode == SBG_NO_ERROR)
|
||||
{
|
||||
printf("The new magnetic calibration has been applied.\n");
|
||||
|
||||
//
|
||||
// Display the magnetic calibration status
|
||||
//
|
||||
displayMagCalibResults(mode, &magCalibResults);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to upload new magnetic calibration data.\n");
|
||||
|
||||
signalWrap(4);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Unable to compute a valid magnetic calibration
|
||||
//
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to compute a valid magnetic calibration.\n");
|
||||
|
||||
signalWrap(3);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to get onboard magnetic calibration results.\n");
|
||||
|
||||
signalWrap(2);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to start the onboard magnetic calibration.\n");
|
||||
|
||||
signalWrap(1);
|
||||
}
|
||||
|
||||
//
|
||||
// Close the sbgEcom library
|
||||
//
|
||||
sbgEComClose(&comHandle);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Unable to initialize the sbgECom
|
||||
//
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to initialize the sbgECom library.\n");
|
||||
retValue = -1;
|
||||
|
||||
signalWrap(0);
|
||||
}
|
||||
|
||||
//
|
||||
// Close the interface
|
||||
//
|
||||
sbgInterfaceSerialDestroy(&sbgInterface);
|
||||
}
|
||||
else
|
||||
{
|
||||
//
|
||||
// Unable to create the interface
|
||||
//
|
||||
fprintf(stderr, "ellipseOnboardMagCalib: Unable to create the interface.\n");
|
||||
retValue = -1;
|
||||
|
||||
signalWrap(0);
|
||||
}
|
||||
|
||||
printf("magCalib process complete!\n");
|
||||
//fgetc(stdin);
|
||||
}
|
||||
|
||||
void sbgMagCibWorkThread::signalWrap(int state)
|
||||
{
|
||||
std::cout<<"sbgMagCibWorkThread::signalWrap---------------------:"<< state <<std::endl;
|
||||
|
||||
|
||||
m_iMagCalibState=state;
|
||||
emit magCalibStateSignal(m_iMagCalibState);
|
||||
}
|
||||
71
Source_Files/utility_tc.cpp
Normal file
71
Source_Files/utility_tc.cpp
Normal file
@ -0,0 +1,71 @@
|
||||
#include "Header_Files/utility_tc.h"
|
||||
#include <QDir>
|
||||
|
||||
QString getFileNameBaseOnTime()
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
//获取系统时间
|
||||
time_t timer;//time_t就是long int 类型
|
||||
struct tm *tblock;
|
||||
timer = time(NULL);//返回秒数(精度为秒)，从1970-1-1,00:00:00 可以当成整型输出或用于其它函数
|
||||
tblock = localtime(&timer);
|
||||
//printf("Local time is: %s\n", asctime(tblock));
|
||||
|
||||
//格式化时间为需要的格式
|
||||
char fileNameTmp[256] = { 0 };
|
||||
char dirNameTmp[256] = { 0 };
|
||||
strftime(fileNameTmp, sizeof(fileNameTmp), "%Y%m%d_%H%M%S", tblock);
|
||||
strftime(dirNameTmp, sizeof(dirNameTmp), "%Y%m%d", tblock);
|
||||
QString fileName(fileNameTmp);
|
||||
QString dirName(dirNameTmp);
|
||||
|
||||
// QString fileName=QString::number(tblock->tm_year+1900)+QString::number(tblock->tm_mon+1)+QString::number(tblock->tm_mday)+"_"+QString::number(tblock->tm_hour)+QString::number(tblock->tm_min);//
|
||||
|
||||
//设置文件绝对路径
|
||||
QDir path("/media/nvme");
|
||||
if(!path.exists(dirName))
|
||||
{
|
||||
path.mkdir(dirName);
|
||||
}
|
||||
path.cd(dirName);
|
||||
QString absoluteFilePath=path.path()+"/"+fileName;
|
||||
// printf("absoluteFilePath is: %s\n", absoluteFilePath.c_str());
|
||||
// qDebug() << "absoluteFilePath is:" << absoluteFilePath;
|
||||
|
||||
return absoluteFilePath;
|
||||
}
|
||||
|
||||
void bubbleSort(unsigned short * a, int n)
|
||||
{
|
||||
// //排序实现１
|
||||
// for(int i=0;i<numberOfPixel;i++)
|
||||
// for(int j=1;j<numberOfPixel-i;j++)
|
||||
// if(a[j-1]>a[j])
|
||||
// swap(a+j-1,a+j);
|
||||
|
||||
//排序实现2
|
||||
int j,k,flag;
|
||||
|
||||
flag = n;
|
||||
while(flag>0)
|
||||
{
|
||||
k=flag;
|
||||
flag=0;
|
||||
for(j=1;j<k;j++)
|
||||
if(a[j-1]>a[j])
|
||||
{
|
||||
swap(a+j-1,a+j);
|
||||
flag=j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void swap(unsigned short * a, unsigned short * b)
|
||||
{
|
||||
int tmp;
|
||||
|
||||
tmp=*a;
|
||||
*a=*b;
|
||||
*b=tmp;
|
||||
}
|
||||
578
Source_Files/ximeaimager.cpp
Normal file
578
Source_Files/ximeaimager.cpp
Normal file
@ -0,0 +1,578 @@
|
||||
#include "Header_Files/ximeaimager.h"
|
||||
|
||||
XimeaImager::XimeaImager()
|
||||
{
|
||||
m_buffer=nullptr;
|
||||
m_bRecordControl=false;
|
||||
m_iFrameCounter=0;
|
||||
m_iImagerState=0;
|
||||
|
||||
//connect(this, SIGNAL(recordFinished()),this, SLOT());
|
||||
}
|
||||
|
||||
void XimeaImager::openImger()
|
||||
{
|
||||
if(m_iImagerState>0)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
//std::cout<<"XimeaImager::openImger111111111111111111111:正在打开相机!"<<std::endl;
|
||||
m_imager.connect();
|
||||
|
||||
// bool haha = m_imager.setSpectralBin(2);
|
||||
// bool haha2 = m_imager.setSpatialBin(2);
|
||||
|
||||
//sn008_bin1
|
||||
float a=1.999564;
|
||||
float b=-279.893;
|
||||
// //sn008_bin2
|
||||
// float a = 3.99912794;
|
||||
// float b = -278.89317732225084;
|
||||
|
||||
m_imager.setGainOffset(a, b);
|
||||
|
||||
|
||||
//SN=008自己修改
|
||||
// int OffsetX=272;
|
||||
// int width=1392;
|
||||
//
|
||||
// int OffsetY=338;
|
||||
// int height=302;
|
||||
//
|
||||
// //SN=008标准
|
||||
// int OffsetX=288;//272
|
||||
// int width=1360;//是16的整数倍
|
||||
//
|
||||
// int OffsetY=340;
|
||||
// int height=300;//302
|
||||
|
||||
|
||||
// m_imager.setRoi(288,1360,348,300);//sn0098
|
||||
m_imager.setRoi(288,1360,340,300);//sn008,corning410，bin1
|
||||
// m_imager.setRoi(144,680,170,151);//sn008,corning410，bin2
|
||||
|
||||
setFramerate(100);
|
||||
|
||||
int frameSizeManual = m_imager.get_band_count()*m_imager.get_sample_count()*2;
|
||||
int frameSizeAuto = m_imager.getBufferSizeOfOneFrame();
|
||||
|
||||
m_iFrameSizeInByte = frameSizeAuto;
|
||||
|
||||
m_buffer = new unsigned short[m_iFrameSizeInByte];
|
||||
|
||||
m_iImagerState = 1;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
std::cout<<"XimeaImager::openImger----------------:ERROR!"<<std::endl;
|
||||
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
catch(char const* e1)
|
||||
{
|
||||
std::cout<<"char *e---------!"<<std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::closeImger()
|
||||
{
|
||||
if(m_iImagerState==0)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
m_imager.disconnect();
|
||||
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
std::cout<<"XimeaImager::closeImger-------------------!"<<std::endl;
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::setFramerate(double framerate)
|
||||
{
|
||||
try
|
||||
{
|
||||
m_imager.set_framerate(framerate);
|
||||
|
||||
int maxExposureTimeInUs=1/framerate*1000000*0.8;
|
||||
setExposureTime(maxExposureTimeInUs);
|
||||
// setExposureTime(1000);
|
||||
|
||||
m_iImagerState=2;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
double XimeaImager::getExposureTime()
|
||||
{
|
||||
double exposureTime;
|
||||
try
|
||||
{
|
||||
exposureTime=m_imager.get_integration_time();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return exposureTime;
|
||||
}
|
||||
|
||||
double XimeaImager::setExposureTime(float exposureTime_in_us)
|
||||
{
|
||||
double integrationTime2Return;
|
||||
|
||||
try
|
||||
{
|
||||
//计算最大积分时间
|
||||
float currentFramerate=getFramerate();
|
||||
float maxExposureTime_in_us=1/currentFramerate*1000000;
|
||||
|
||||
//确保设置的积分时间比最大积分时间小
|
||||
if(exposureTime_in_us<maxExposureTime_in_us)
|
||||
{
|
||||
|
||||
m_imager.set_integration_time(exposureTime_in_us);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
m_imager.set_integration_time(maxExposureTime_in_us);
|
||||
}
|
||||
|
||||
//返回设置的积分时间
|
||||
integrationTime2Return=m_imager.get_integration_time();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
|
||||
return integrationTime2Return;
|
||||
}
|
||||
|
||||
double XimeaImager::autoExposure()
|
||||
{
|
||||
double exposureTime;
|
||||
try
|
||||
{
|
||||
//方式１：在曝光时的帧率前提下，从最大曝光时间开始循环采集
|
||||
int maxValueOfOneFrame;
|
||||
float suitableMaxValue=4095 * 0.8;
|
||||
|
||||
double framerate = m_imager.get_framerate();
|
||||
double maxExposureTime = 1/framerate*1000000;
|
||||
exposureTime=setExposureTime(maxExposureTime);
|
||||
|
||||
bool bIsAutoExposureOk=false;
|
||||
while(!bIsAutoExposureOk)
|
||||
{
|
||||
m_imager.start();
|
||||
m_imager.get_frame(m_buffer);
|
||||
m_imager.stop();
|
||||
|
||||
maxValueOfOneFrame=getMaxValueOfOneFrame(m_buffer,m_imager.get_band_count()*m_imager.get_sample_count());
|
||||
|
||||
if(maxValueOfOneFrame <= suitableMaxValue)
|
||||
{
|
||||
bIsAutoExposureOk=true;
|
||||
}
|
||||
else
|
||||
{
|
||||
exposureTime = exposureTime*0.95;
|
||||
exposureTime=setExposureTime(exposureTime);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// //方式2：
|
||||
// int baseExposureTime_in_us=5000;
|
||||
// float suitableMaxValue=4095 * 0.8;
|
||||
// int maxValueOfOneFrame;
|
||||
|
||||
// setExposureTime(baseExposureTime_in_us);
|
||||
|
||||
// m_imager.start();
|
||||
// m_imager.get_frame(m_buffer);
|
||||
// m_imager.stop();
|
||||
|
||||
// std::cout<<"1111111111111111111111111111111111!"<<std::endl;
|
||||
// maxValueOfOneFrame=getMaxValueOfOneFrame(m_buffer,m_imager.get_band_count()*m_imager.get_sample_count());
|
||||
// std::cout<<"2222222222222222222222222222222222!"<<std::endl;
|
||||
|
||||
// float scale=suitableMaxValue/maxValueOfOneFrame;
|
||||
// float suitableExposureTime=baseExposureTime_in_us * scale;
|
||||
// exposureTime=setExposureTime(suitableExposureTime);
|
||||
|
||||
m_iImagerState=3;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
std::cout<<"自动曝光完成!"<<std::endl;
|
||||
return exposureTime;
|
||||
}
|
||||
|
||||
double XimeaImager::getFramerate()
|
||||
{
|
||||
double framerate;
|
||||
|
||||
try
|
||||
{
|
||||
framerate=m_imager.get_framerate();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return framerate;
|
||||
}
|
||||
|
||||
void XimeaImager::setGain(double gain)
|
||||
{
|
||||
try
|
||||
{
|
||||
m_imager.set_gain(gain);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
double XimeaImager::getGain()
|
||||
{
|
||||
try
|
||||
{
|
||||
return m_imager.get_gain();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
int XimeaImager::getSampleCount()
|
||||
{
|
||||
int sampleCount;
|
||||
|
||||
try
|
||||
{
|
||||
sampleCount=m_imager.get_sample_count();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return sampleCount;
|
||||
}
|
||||
|
||||
int XimeaImager::getBandCount()
|
||||
{
|
||||
int bandCount;
|
||||
|
||||
try
|
||||
{
|
||||
bandCount=m_imager.get_band_count();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return bandCount;
|
||||
}
|
||||
|
||||
int XimeaImager::getWindowStartBand()
|
||||
{
|
||||
int windowStartBand;
|
||||
|
||||
try
|
||||
{
|
||||
windowStartBand=m_imager.get_start_band();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return windowStartBand;
|
||||
}
|
||||
|
||||
int XimeaImager::getWindowEndBand()
|
||||
{
|
||||
int windowEndBand;
|
||||
|
||||
try
|
||||
{
|
||||
windowEndBand=m_imager.get_end_band();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return windowEndBand;
|
||||
}
|
||||
|
||||
double XimeaImager::geWavelengthAtBand(int x)
|
||||
{
|
||||
double wavelengthAtBand;
|
||||
|
||||
try
|
||||
{
|
||||
wavelengthAtBand=m_imager.get_wavelength_at_band(x);
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
|
||||
return wavelengthAtBand;
|
||||
}
|
||||
|
||||
int XimeaImager::getMaxValueOfOneFrame(unsigned short * data, int numberOfPixel)
|
||||
{
|
||||
//排序
|
||||
//bubbleSort(data,1000);
|
||||
|
||||
|
||||
//计算出最大的10%值的平均值
|
||||
unsigned short maxValue=0;
|
||||
for(int i=0;i<numberOfPixel;i++)
|
||||
{
|
||||
if (data[i]>maxValue)
|
||||
{
|
||||
maxValue=data[i];
|
||||
}
|
||||
}
|
||||
printf("本帧最大值为: %d.\n",maxValue);
|
||||
|
||||
return maxValue;
|
||||
}
|
||||
|
||||
int XimeaImager::getImagerState() const
|
||||
{
|
||||
return m_iImagerState;
|
||||
}
|
||||
|
||||
void XimeaImager::startRecord(double TimeDifferenceBetweensOSAndSbg,QString baseFileName)
|
||||
{
|
||||
try
|
||||
{
|
||||
if(m_iImagerState==4 || m_iImagerState==0 || m_iImagerState>=21)
|
||||
{
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
m_iImagerStateTemp=m_iImagerState;
|
||||
|
||||
m_iImagerState=4;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
printf("开始采集!\n");
|
||||
m_iFrameCounter=0;
|
||||
m_bRecordControl=true;
|
||||
|
||||
m_imager.start();
|
||||
|
||||
m_baseFileName=baseFileName;
|
||||
|
||||
QString imageFileName=m_baseFileName+".bil";
|
||||
QString timesFileName=m_baseFileName+".times";
|
||||
|
||||
//
|
||||
FILE *hFile=fopen(imageFileName.toStdString().c_str(),"w+b");
|
||||
FILE *hHimesFile=fopen(timesFileName.toStdString().c_str(),"w+");
|
||||
|
||||
using namespace std;
|
||||
ofstream timesFileHandle(timesFileName.toStdString()+"_ofstream");
|
||||
|
||||
|
||||
struct timeval timeStart, timeEnd;
|
||||
double runTime=0;
|
||||
gettimeofday(&timeStart, NULL);
|
||||
|
||||
while (m_bRecordControl)
|
||||
{
|
||||
unsigned short *x=m_imager.get_frame(m_buffer);
|
||||
|
||||
//fwrite(m_buffer,2,getBandCount()*getSampleCount(),hFile);
|
||||
fwrite(m_buffer,1,m_iFrameSizeInByte,hFile);//*********************************
|
||||
//fflush(hFile);//只保证了将IO缓冲写入系统缓冲中，使IO读操作能成功，但系统什么时候写入磁盘，由系统决定，一般是达到一定量时系统他就写入磁盘。
|
||||
//sync();//强制系统将系统文件缓冲的内容写入磁盘
|
||||
|
||||
m_iFrameCounter+=1;
|
||||
|
||||
|
||||
double sbgTime=getSbgTime(TimeDifferenceBetweensOSAndSbg);
|
||||
|
||||
fprintf(hHimesFile,"%f\n",sbgTime);
|
||||
//fwrite(&sbgTime,sizeof(double),1,hHimesFile);
|
||||
|
||||
timesFileHandle << sbgTime << "\n";
|
||||
|
||||
// std::cout<<"XimeaImager::startRecord---std::cout: "<<sbgTime<<std::endl;
|
||||
|
||||
// //用于测试是否漏帧
|
||||
// if(m_iFrameCounter==getFramerate()*20)
|
||||
// {
|
||||
// break;
|
||||
// }
|
||||
|
||||
// unsigned char pixel = *(unsigned char*)image.bp;//Default value: XI_MONO8
|
||||
// unsigned short pixel =*(unsigned short*)image.bp;//XI_RAW16
|
||||
// printf("Image %d (%dx%d) received from camera. First pixel value: %d\n", m_iFrameCounter, (int)image.width, (int)image.height, pixel);
|
||||
}
|
||||
|
||||
gettimeofday(&timeEnd, NULL);
|
||||
runTime = (timeEnd.tv_sec - timeStart.tv_sec ) + (double)(timeEnd.tv_usec -timeStart.tv_usec)/1000000;
|
||||
|
||||
double frameInTheory=runTime * getFramerate();
|
||||
double frameLossed=frameInTheory - m_iFrameCounter;
|
||||
double frameLossRate=frameLossed / frameInTheory;
|
||||
|
||||
std::cout<<"当前采集文件为: "<<baseFileName.toStdString()<<std::endl;
|
||||
std::cout<<"采集时间为: "<<runTime<<std::endl;
|
||||
std::cout<<"理论采集帧数为: "<<frameInTheory<<std::endl;
|
||||
std::cout<<"实际采集帧数为："<<m_iFrameCounter<<std::endl;
|
||||
std::cout<<"丢失帧数为: "<<frameLossed<<std::endl;
|
||||
std::cout<<"丢帧率为: "<<frameLossRate<<std::endl;
|
||||
|
||||
fclose(hFile);
|
||||
fclose(hHimesFile);
|
||||
timesFileHandle.close();
|
||||
|
||||
printf("Stopping acquisition...\n");
|
||||
m_imager.stop();
|
||||
|
||||
writeHdr();
|
||||
|
||||
m_iImagerState=m_iImagerStateTemp;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
emit recordFinished();
|
||||
}
|
||||
catch(int xiApiErrorCodes)
|
||||
{
|
||||
processXiApiErrorCodes(xiApiErrorCodes);
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::writeHdr()
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
QString hdrPath=m_baseFileName+".hdr";
|
||||
|
||||
ofstream hdrFileHandle(hdrPath.toStdString());
|
||||
hdrFileHandle << "ENVI\n";
|
||||
hdrFileHandle << "interleave = bil\n";
|
||||
hdrFileHandle << "byte order = 0\n";
|
||||
hdrFileHandle << "data type = 2\n";
|
||||
//hdrFileHandle << "bit depth = 12\n";
|
||||
|
||||
hdrFileHandle << "samples = " << getSampleCount() << "\n";
|
||||
hdrFileHandle << "bands = " << getBandCount() << "\n";
|
||||
hdrFileHandle << "lines = " << m_iFrameCounter << "\n";
|
||||
hdrFileHandle << "sample binning = " << m_imager.getSpatialBin() << "\n";
|
||||
hdrFileHandle << "spectral binning = " << m_imager.getSpectralBin() << "\n";
|
||||
|
||||
hdrFileHandle << "framerate = " << getFramerate() << "\n";
|
||||
hdrFileHandle << "shutter = " << getExposureTime()/1000 << "\n";
|
||||
hdrFileHandle << "shutter units = milliseconds\n";
|
||||
hdrFileHandle << "gain = " << getGain() << "\n";
|
||||
|
||||
hdrFileHandle << "wavelength units = nanometers\n";
|
||||
hdrFileHandle << "wavelength = {";
|
||||
//hdrFileHandle << std::setprecision(5);
|
||||
for (int i = getWindowStartBand(); i < getWindowEndBand(); i++)
|
||||
{
|
||||
hdrFileHandle << geWavelengthAtBand(i);
|
||||
if (i < getWindowEndBand() - 1)
|
||||
hdrFileHandle << ", ";
|
||||
else
|
||||
{
|
||||
printf("头文件中写入了多少个波段:%d\n",i-getWindowStartBand()+1);//???????????????
|
||||
}
|
||||
}
|
||||
hdrFileHandle << "}\n";
|
||||
hdrFileHandle.close();
|
||||
}
|
||||
|
||||
void XimeaImager::processXiApiErrorCodes(int xiApiErrorCodes)
|
||||
{
|
||||
using namespace std;
|
||||
switch (xiApiErrorCodes)
|
||||
{
|
||||
case 1:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:Invalid handle!"<<std::endl;
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
case 56:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:NO_DEVICES_FOUND!"<<std::endl;
|
||||
m_iImagerState=0;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
case 57:
|
||||
std::cout<<"XimeaImager::processXiApiErrorCodes-----------:RESOURCE_OR_FUNCTION_LOCKED!"<<std::endl;
|
||||
m_iImagerState=22;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
|
||||
break;
|
||||
default:
|
||||
QString ximeaError="ximeaError.txt";
|
||||
ofstream ximeaErrorFile(ximeaError.toStdString().c_str(),ios::app);
|
||||
|
||||
ximeaErrorFile<< xiApiErrorCodes << "\n";
|
||||
|
||||
ximeaErrorFile.close();
|
||||
|
||||
|
||||
|
||||
m_iImagerState=21;
|
||||
emit ximeaImageStatus(m_iImagerState);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void XimeaImager::stopRecord()
|
||||
{
|
||||
//printf("Stop record!\n");
|
||||
m_bRecordControl=false;
|
||||
}
|
||||
|
||||
int XimeaImager::getFrameCounter()
|
||||
{
|
||||
return m_iFrameCounter;
|
||||
}
|
||||
Reference in New Issue
Block a user