commit 1452bcc2b9e1766ab2f0a8e7c88ccd6d2b2f3228 Author: tangchao0503 <735056338@qq.com> Date: Mon Jun 13 12:01:30 2022 +0800 300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4afcf19 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.idea +build \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..4da79a4 --- /dev/null +++ b/CMakeLists.txt @@ -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) diff --git a/Header_Files/fileoperation.h b/Header_Files/fileoperation.h new file mode 100644 index 0000000..801e627 --- /dev/null +++ b/Header_Files/fileoperation.h @@ -0,0 +1,35 @@ +#ifndef FILEOPERATION_H +#define FILEOPERATION_H + +#include + +#include +#include + +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 diff --git a/Header_Files/math_tc.h b/Header_Files/math_tc.h new file mode 100644 index 0000000..26dfa9c --- /dev/null +++ b/Header_Files/math_tc.h @@ -0,0 +1,136 @@ +#ifndef MATH_H +#define MATH_H + +#include +#include + +using namespace std; + +template +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 +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 +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 (lchildarry[element])//只有左子树且子树小于自己 + { + std::swap(arry[lchild], arry[element]); + } + return; +} + + +template +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 diff --git a/Header_Files/sbgbuffer.h b/Header_Files/sbgbuffer.h new file mode 100644 index 0000000..3eaabd4 --- /dev/null +++ b/Header_Files/sbgbuffer.h @@ -0,0 +1,97 @@ +#ifndef SBGBUFFER_H +#define SBGBUFFER_H + +#include "sbgerrorcodes.h" +#include + +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 diff --git a/Header_Files/sbgcrc.h b/Header_Files/sbgcrc.h new file mode 100644 index 0000000..5e2a167 --- /dev/null +++ b/Header_Files/sbgcrc.h @@ -0,0 +1,182 @@ +#ifndef SBGCRC_H +#define SBGCRC_H + +#include +#include + +namespace sbgtc +{ + //----------------------------------------------------------------------// + //- Header (open extern C block) -// + //----------------------------------------------------------------------// + + + //#include //??????????????????????????????????????????????????????????????????????????????????? + + //----------------------------------------------------------------------// + //- 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 diff --git a/Header_Files/sbgerrorcodes.h b/Header_Files/sbgerrorcodes.h new file mode 100644 index 0000000..28d6bb4 --- /dev/null +++ b/Header_Files/sbgerrorcodes.h @@ -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
*/ + /*!< We have received an unexpected frame (not the cmd we are waiting for or with an invalid data size.
*/ + /*!< This could be caused by a desync between questions and answers.
*/ + /*!< 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 diff --git a/Header_Files/sbglogparse.h b/Header_Files/sbglogparse.h new file mode 100644 index 0000000..1f75f81 --- /dev/null +++ b/Header_Files/sbglogparse.h @@ -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.
+ 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.
+ 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 diff --git a/Header_Files/sbgrecorder.h b/Header_Files/sbgrecorder.h new file mode 100644 index 0000000..b20274a --- /dev/null +++ b/Header_Files/sbgrecorder.h @@ -0,0 +1,78 @@ +#ifndef SBGRECORDER_H +#define SBGRECORDER_H + +#include +#include + +#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 diff --git a/Header_Files/udpserver.h b/Header_Files/udpserver.h new file mode 100644 index 0000000..c3a8133 --- /dev/null +++ b/Header_Files/udpserver.h @@ -0,0 +1,103 @@ +#ifndef UDPSERVER_H +#define UDPSERVER_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ximeaimager.h" +#include "sbgrecorder.h" +#include "fileoperation.h" + +extern "C" +{ + #include + #include +} + + +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 diff --git a/Header_Files/utility_tc.h b/Header_Files/utility_tc.h new file mode 100644 index 0000000..eb6cefc --- /dev/null +++ b/Header_Files/utility_tc.h @@ -0,0 +1,16 @@ +#ifndef UTILITY_TC_H +#define UTILITY_TC_H + +#include + +#include +#include + +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 diff --git a/Header_Files/ximeaimager.h b/Header_Files/ximeaimager.h new file mode 100644 index 0000000..5fc3259 --- /dev/null +++ b/Header_Files/ximeaimager.h @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "irisximeaimager.h" +#include "math.h" +#include "utility_tc.h" + + +//#ifdef WIN32 +//#include // Windows +//#else +//#include // 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(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 diff --git a/Source_Files/fileoperation.cpp b/Source_Files/fileoperation.cpp new file mode 100644 index 0000000..c9eddc0 --- /dev/null +++ b/Source_Files/fileoperation.cpp @@ -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 +#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(); +} diff --git a/Source_Files/sbgbuffer.cpp b/Source_Files/sbgbuffer.cpp new file mode 100644 index 0000000..50bc5b3 --- /dev/null +++ b/Source_Files/sbgbuffer.cpp @@ -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; +} diff --git a/Source_Files/sbgcrc.cpp b/Source_Files/sbgcrc.cpp new file mode 100644 index 0000000..dd34185 --- /dev/null +++ b/Source_Files/sbgcrc.cpp @@ -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; +} diff --git a/Source_Files/sbglogparse.cpp b/Source_Files/sbglogparse.cpp new file mode 100644 index 0000000..99ef341 --- /dev/null +++ b/Source_Files/sbglogparse.cpp @@ -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); +} + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Source_Files/sbgrecorder.cpp b/Source_Files/sbgrecorder.cpp new file mode 100644 index 0000000..1a1c6a4 --- /dev/null +++ b/Source_Files/sbgrecorder.cpp @@ -0,0 +1,845 @@ +#include "Header_Files/sbgrecorder.h" +#include +#include +#include + +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!"< infos = QSerialPortInfo::availablePorts(); + + //std::cout<<"number of availablePorts:"<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<<"波特率被成功设置为:"<baudRate()<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;irxBufferSize > 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!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<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: ------------------------------------------------------"<>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"<(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<(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--------------:"<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"<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!"<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 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代表启动系统!"<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<<"系统已经开始采集!"<getImagerState()>=1 && m_imager->getImagerState()<=4) + { + m_imager->stopRecord(); + } + + break; + } + case 5:// + { + if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3) + { + std::cout<<"5代表设置帧率!"<setFramerate(datagramList[1].toFloat()); + } + + break; + } + case 6:// + { + if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3) + { + std::cout<<"6代表自动曝光!"<autoExposure(); + } + + break; + } + case 7: + { + if(datagramList[1].toInt()==1) + { + std::cout<<"拷贝数据!"<start(); + } + else if(datagramList[1].toInt()==0) + { + std::cout<<"8-0: 停止磁场矫正!"<m_iMagCalibStopControl=0; + } + + break; + } + case 9: + { + std::cout<<"9代表仅采集影像!"<stopRecord(); + } + + break; + } + default: + std::cout<<">=9没有意义!"<(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 <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::sendSbgMagCalibState(int SbgMagCalibState) +{ + std::cout<<"UdpServer::sendSbgMagCalibState---------------------:"<< SbgMagCalibState <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::sendSbgSolutionModeState(int SolutionMode) +{ + std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::sendSbgAccuracyState(int Accuracy) +{ +// std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::sendXimeaImageStatus(int ximeaImageStatus) +{ + std::cout<<"UdpServer::sendXimeaImageStatus---------------------:"<< ximeaImageStatus <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::sendCopyFileStatus(int fileStatus) +{ + std::cout<<"UdpServer::sendCopyFileStatus---------------------:"<< fileStatus <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); +} + +void UdpServer::onRecordFinished() +{ + std::cout<<"UdpServer::onRecordFinished----------------:影像停止采集"<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 < + +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;ia[j]) +// swap(a+j-1,a+j); + + //排序实现2 + int j,k,flag; + + flag = n; + while(flag>0) + { + k=flag; + flag=0; + for(j=1;ja[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; +} diff --git a/Source_Files/ximeaimager.cpp b/Source_Files/ximeaimager.cpp new file mode 100644 index 0000000..224ac71 --- /dev/null +++ b/Source_Files/ximeaimager.cpp @@ -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:正在打开相机!"<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: "<