wangchongwu
/
StitchVideo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'cac2db5ae0'
${ noResults }
StitchVideo/tests/S7215/commondefine.h

1466 lines
78 KiB
Raw Normal View History Unescape

S7215 载荷1 ts流解析
2 weeks ago
#ifndef COMMONDEFINE_H
#define COMMONDEFINE_H
#include <QString>
#include <vector>
//设备管理信息
#define MAX_IP_LEN 16
#define MAX_LOGIN_LEN 256
//#define MAX_SHOW_WINDOW_NUM (64) //最大显示窗口数
//#define MAX_DEV_NUM (18) //最大设备个数
//#define MAX_SHOW_WINDOW_NUM (8) //最大显示窗口数
//#define MAX_DEV_NUM (8) //最大设备个数
#define MAX_SHOW_WINDOW_NUM (9) //最大显示窗口数
#define MAX_DEV_NUM (9) //最大设备个数
#define IS_OLD_7215 (0) //是否为旧7215程序，1为是，0为不是
#define GUIDEANGLERATIO_FLOAT (0.0003017485)
/*
* @brief: 以下结构对齐方式为1个字节对齐
*/
#pragma pack(push,1)
/*
* @breif: 下传的测控数据
*
* 采用1个字节对齐的结构表示下传的测控数据
* 该结构既包括数据还包括对应的方法
*/
struct telemetryData
{
/*
* @brief: 测控数据中的标识，采用一个结构来表示
*
* 该结构中的数据为一个unsigned int类型，共定义32个标识位(含保留位)
* 每一个标识位用一个bit标识
*/
struct telemFlags
{
public:
/*
* @brief: telemFlags的默认构造函数，对结构中的数据进行默认初始化
*/
telemFlags():mTimeStamp(0),
mPlatformType(0),
mPlatformID(0),
mPlatformRotation(0),
mPlatformPosition(0),
mPlatformSpeed(0),
mSensorType(0),
mSensorID(0),
mSensorSelectionCB(0),
mSensorFOV(0),
mSensorMod(0),
mSensorRotation(0),
mLaserDistance(0),
mMarkPosition(0),
mComposedAngle(0),
mInnerNavigation(0)
{}
/*
* @brief: 获取各个标识位的状态
*
*/
inline bool isTimeStamp() const { return mTimeStamp == 1; }
inline bool isPlatformType() const { return mPlatformType == 1; }
inline bool isPlatformID() const { return mPlatformID == 1; }
inline bool isPlatformRotation() const { return mPlatformRotation == 1; }
inline bool isPlatformPosition() const { return mPlatformPosition == 1; }
inline bool isPlatformSpeed() const { return mPlatformSpeed == 1; }
inline bool isSensorType() const { return mSensorType == 1; }
inline bool isSensorID() const { return mSensorID == 1; }
inline bool isSensorSelectionCB() const { return mSensorSelectionCB == 1; }
inline bool isSensorFOV() const { return mSensorFOV == 1; }
inline bool isSensorMod() const { return mSensorMod == 1; }
inline bool isSensorRotation() const { return mSensorRotation == 1; }
inline bool isLaserDistance() const { return mLaserDistance == 1; }
inline bool isMarkPosition() const { return mMarkPosition == 1; }
inline bool isFrameRotation() const { return mFrameRotation == 1; }
inline bool isComposedAngle() const { return mComposedAngle == 1; }
inline bool isInnerNavigation() const { return mInnerNavigation == 1; }
/*
* @brief: 设置各个标识为的状态
*/
inline void setTimestamp(bool value)
{
mTimeStamp = value ? 1 : 0;
}
inline void setPlatformType(bool value)
{
mPlatformType = value ? 1 : 0;
}
inline void setPlatformID(bool value)
{
mPlatformID = value ? 1 : 0;
}
inline void setPlatformRotation(bool value)
{
mPlatformRotation = value ? 1 : 0;
}
inline void setPlatformPosition(bool value)
{
mPlatformPosition = value ? 1 : 0;
}
inline void setPlatformSpeed(bool value)
{
mPlatformSpeed = value ? 1 : 0;
}
inline void setSensorType(bool value)
{
mSensorType = value ? 1 : 0;
}
inline void setSensorID(bool value)
{
mSensorID = value ? 1 : 0;
}
inline void setSensorSelectionCB(bool value)
{
mSensorSelectionCB = value ? 1 : 0;
}
inline void setSensorFOV(bool value)
{
mSensorFOV = value ? 1 : 0;
}
inline void setSensorMod(bool value)
{
mSensorMod = value ? 1 : 0;
}
inline void setSensorRotation(bool value)
{
mSensorRotation = value ? 1 : 0;
}
inline void setLaserDistance(bool value)
{
mLaserDistance = value ? 1 : 0;
}
inline void setMarkPosition(bool value)
{
mMarkPosition = value ? 1 : 0;
}
inline void setFrameRotation(bool value)
{
mFrameRotation = value ? 1 : 0;
}
inline void setComposedAngle(bool value)
{
mComposedAngle = value ? 1 : 0;
}
inline void setInnerNavigation(bool value)
{
mInnerNavigation = value ? 1 : 0;
}
protected:
unsigned int mTimeStamp : 1; ///< 测控数据中是否存在时间标签，用1个bit标识；
unsigned int mPlatformType : 1; ///< 测控数据中是否存在平台类型，用1个bit标识；
unsigned int mPlatformID : 1; ///< 测控数据中是否存在平台编号，用1个bit标识；
unsigned int mPlatformRotation : 1; ///< 测控数据中是否存在平台旋转角度信息，用1个bit标识；
unsigned int mPlatformPosition : 1; ///< 测控数据中是否存在平台的位置信息，用1个bit标识；
unsigned int mPlatformSpeed : 1; ///< 测控数据中是否存在平台速度信息，用1个bit标识；
unsigned int mSensorType : 1; ///< 测控数据中是否存在传感器类型，用1个bit标识；
unsigned int mSensorID : 1; ///< 测控数据中是否存在传感器编号，用1个bit标识；
unsigned int mSensorSelectionCB : 1; ///< 测控数据中是否存在传感器选择回报，用1个bit标识；
unsigned int mSensorFOV : 1; ///< 测控数据中是否存在传感器FOV信息，用1个bit标识；
unsigned int mSensorMod : 1; ///< 测控数据中是否存在传感器工作模式信息，用1个bit标识；
unsigned int mSensorRotation : 1; ///< 测控数据中是否存在传感器旋转角度信息，用1个bit标识；
unsigned int mLaserDistance : 1; ///< 测控数据中是否存在激光测距信息，用1个bit标识；
unsigned int mMarkPosition : 1; ///< 测控数据中是否存在Mark经纬高，用1个bit标识；
unsigned int mFrameRotation : 1; ///< 测控数据中是否存在框架角，用1个bit标识；
unsigned int mComposedAngle : 1; ///< 测控数据中是否存在综合光轴角，用1个bit标识；
unsigned int mInnerNavigation : 1; ///< 测控数据中是否存在内置惯导的角度，用1个bit标识；
private:
unsigned int mReserved : 15; ///< 标志位为32个bit，已用17个，保留15个bit
};
/*
* @brief: 测控数据默认构造函数，默认不包含任何信息，需要手工设置包含了那些信息字段
*/
telemetryData() { memset(this, 0, sizeof(telemetryData)); }
public:
telemFlags mFlags; ///< 测控参数标识
///< 时间采用UTC时间
unsigned short mYear; ///< 必填项，生成飞控参数时对应的年，LSB=1，有效范围[2000,5000]
unsigned char mMonth; ///< 必填项，生成飞控参数时对应的月，LSB=1，有效范围[1,12]
unsigned char mDay; ///< 必填项，生成飞控参数时对应的日，LSB=1，有效范围[1,31]
unsigned char mHour; ///< 必填值，生成飞控参数时对应的时,24小时制，LSB=1，有效范围[0,24)
unsigned char mMinute; ///< 必填值，生成飞控参数时对应的分，LSB=1，有效范围[0,59]
unsigned char mSecond; ///< 必填值，生成飞控参数时对应的秒，LSB=1，有效范围[0,59]
unsigned short mMillSecond; ///< 必填值，生成飞控参数时对应的毫秒，LSB=1，有效范围[0,999]
unsigned char mDroneType; ///< 选填值，平台类型代号，LSB=1，由其他协议确定代号含义
unsigned short mDroneNum; ///< 必填值，平台编号，LSB=1，由平台提供，具体含义由其他协议确定
float mDroneYaw; ///< 必填值，平台航向角，默认为0，表示平台在头部朝北时北东地右手坐标系下的旋转角度
///< 角度定义为欧拉角，单位为度，0度为北，90度为东，180度为南，270度为西
///< LSB=360.0/ 65535.0，即360.0/(2^16 - 1)，编码转换为unsigned short型，有效范围[0.0,360.0)
float mDronePitch; ///< 必填值，平台俯仰角，默认为0，表示平台在北东地右手坐标系下的旋转角度
///< 角度定义为欧拉角，单位为度，规定头部水平时俯仰角为0度，并且向上为正，向下为负，
///< LSB=180.0/ 32767.0，即360.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mDroneRoll; ///< 必填值，平台滚转角，默认为0，表示平台在北东地右手坐标系下的旋转角度
///< 角度定义为欧拉角，单位为度，规定从平台尾部看向头部时右翼水平为0，并且向下为正，向上为负
///< LSB=180.0/ 32767.0，即360.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
double mDroneLongtitude; ///< 必填值，平台经度，WGS84椭球参考系。当传感器自带卫星导航设备时，
///< 该值为传感器自身携带的卫星导航设备位置值，单位用度表示，
///< LSB=180.0/2147483647.0，即180.0/(2^31 - 1)，编码转换为int型，有效范围为[-180.0,180.0]
double mDroneLatitude; ///< 必填值，平台纬度，WGS84椭球参考系。当传感器自带卫星导航设备时，
///< 该值为传感器自身携带的卫星导航设备位置值，单位用度表示，
///< LSB=90.0/2147483647.0，即90.0/(2^31 - 1)，编码转换为int型，有效范围为[-90.0,90.0]
float mDroneAltitude; ///< 必填值，平台高度，用大地高度（卫星导航系统测量得到的高度）表示，单位为米，
///< LSB=15000.0/32767.0，即15000.0/(2^15-1)，编码转换为short型之后的有效范围为[-100.0,15000]
float mDroneSpeedEast; ///< 必填值，平台东向速度分量，默认为0，表示东北地坐标系下平台东向速度分量，
///< 向东为正，向西为负，单位为米/秒，LSB=1024.0/32767.0，即1024/(2^15-1)，
///< 编码转换为short型，有效范围为[-1024.0,1024.0]
float mDroneSpeedNorth; ///< 必填值，平台北向速度分量，默认为0，表示东北地坐标系下平台北向速度分量
///< 向北为正，向南为负，单位为米/秒，LSB=1024.0/32767.0，即1024/(2^15-1)，
///< 编码转换为short型，有效范围为[-1024.0,1024.0]
float mDroneSpeedSky; ///< 必填值，平台天向速度分量，默认为0，表示东北地坐标系下平台天向速度分量
///< 向天为负，向地为正，单位为米/秒，LSB=1024.0/32767.0，即1024/(2^15-1)，
///< 编码转换为short型，有效范围为[-1024.0,1024.0]
unsigned char mSensorType; ///< 必填值，传感器类型代号，LSB=1，由其他协议确定代号含义
unsigned short mSensorID; ///< 必填项，传感器编号，LSB=1，由其他协议确定编号含义
unsigned char mSensorSelectionCB; ///< 选填值，传感器选择回报代号，LSB=1，由其他协议确定代号含义
float mSensorHFOV; ///< 必填值，传感器水平视场角，用度表示，LSB=180.0/65535.0，即180.0/(2^16 - 1)，
///< 编码转换为short型，有效范围为[0.0,180.0]，当传感器调整焦距时，需要更新水平和垂直FOV
float mSensorVFOV; ///< 必填值，传感器垂直视场角，用度表示，LSB=180.0/65535.0，即180.0/(2^16 - 1)，
///< 编码转换为short型，有效范围为[0.0,180.0]，当传感器调整焦距时，需要更新水平和垂直FOV
unsigned char mSensorMod; ///< 必填值，传感器工作模式代号，LSB=1，由其他协议确定代号含义
float mFrameYaw; ///< 必填值，特指搜索相机稳定平台的框架角中用于补偿平台航向角偏转的角度，若没有
///< 补偿平台航向角偏转，则此项为0。航向框架角LSB=360.0/ 65535.0，
///< 即360.0/(2^16 - 1)，编码转换为unsigned short型，有效范围[0.0,360.0)
float mFramePitch; ///< 必填值，特指搜索相机稳定平台的框架角中用于补偿平台俯仰角偏转的角度，若没有
///< 补偿平台俯仰角偏转，则此项为0。俯仰框架角LSB=180.0/ 32767.0，
///< 即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mFrameRoll; ///< 必填值，特指搜索相机稳定平台的框架角中用于补偿平台滚转角偏转的角度，若没有
///< 补偿平台滚转角偏转，则此项为0。滚转框架角LSB=180.0/ 32767.0，
///< 即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mSensorYaw; ///< 必填值，传感器光轴相对于安装平面的航向角，角度定义为欧拉角,
///< 当传感器为搜索相机和跟踪相机时，安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。传感器坐标系定义为相对于安装平面的坐标，初始坐标与安装平面坐标系一致。
///< 航向角LSB=360.0/ 65535.0，即360.0/(2^16 - 1)，编码转换为unsigned short型，有效范围[0.0,360.0)
float mSensorPitch; ///< 必填值，传感器光轴相对于安装平面的俯仰角，角度定义为欧拉角,
///<当传感器为搜索相机和跟踪相机时，安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。传感器坐标系定义为相对于安装平面的坐标，初始坐标与安装平面坐标系一致。
///< LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mSensorRoll; ///< 必填值，传感器光轴相对于安装平面的滚转角，角度定义为欧拉角,
///<当传感器为搜索相机和跟踪相机时，安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。传感器坐标系定义为相对于安装平面的坐标，初始坐标与安装平面坐标系一致。
///< LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mComposedYaw; ///< 必填值，传感器光轴在北东地坐标系下的综合角度，定义为综合航向角，角度定义为欧拉角
///< 综合角度指综合考虑平台旋转角度、框架补偿角、相机安装角等多个因素，得到的相机在北东地右手坐标系下的
///< 角度。综合航向角LSB=360.0/ 65535.0，即360.0/(2^16 - 1)，编码转换为unsigned short型，有效范围[0.0,360.0)
float mComposedPitch; ///< 必填值，传感器光轴在北东地坐标系下的综合角度，定义为综合俯仰角，角度定义为欧拉角
///< 综合角度指综合考虑平台旋转角度、框架补偿角、相机安装角等多个因素，得到的相机在北东地右手坐标系下的
///< 角度。综合俯仰角LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mComposedRoll; ///< 必填值，传感器光轴在北东地坐标系下的综合角度，定义为综合滚转角，角度定义为欧拉角
///< 综合角度指综合考虑平台旋转角度、框架补偿角、相机安装角等多个因素，得到的相机在北东地右手坐标系下的
///< 角度。综合滚转角LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mLaserDistance; ///< 选填值，激光测距值，单位为米，LSB=10000.0/65535.0，即10000.0/(2^16 - 1)
///< 编码转换为unsigned short型，有效范围为[0.0,10000.0]
double mMarkLongtitude; ///< 选填值，Mark的经度，WGS84椭球参考系，表示测量获得的目标的经度，
///< 单位用度表示，LSB=180.0/2147483647.0，即180.0/(2^31 - 1)，
///< 编码转换为int型，有效范围为[-180.0,180.0]
double mMarkLatitude; ///< 选填值，Mark的纬度，WGS84椭球参考系，表示测量获得的目标的纬度，
///< 单位用度表示，LSB=90.0/2147483647.0，即90.0/(2^31 - 1)，
///< 编码转换为int型，有效范围为[-90.0,90.0]
float mMarkAltitude; ///< 选填值，Mark的高度，WGS84椭球参考系，表示测量获得的目标的高度，
///< 单位用米表示，LSB=15000.0/32767.0，即15000.0/(2^15-1)，
///< 编码转换为short型，有效范围为[-100.0,15000]
float mInnerInertiaYaw; ///< 必填值，内置惯导测量得到的传感器安装平面的航向角，角度定义为欧拉角,
///< 安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。内置惯导固连在安装平面上，坐标系与安装平面坐标系平行。
///< 航向角LSB=360.0/ 65535.0，即360.0/(2^16 - 1)，编码转换为unsigned short型，有效范围[0.0,360.0)
float mInnerInertiaPitch; ///< 必填值，内置惯导测量得到的传感器安装平面的俯仰角，角度定义为欧拉角,
///< 安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。内置惯导固连在安装平面上，坐标系与安装平面坐标系平行。
///< 俯仰角LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
float mInnerInertiaRoll; ///< 必填值，内置惯导测量得到的传感器安装平面的滚转角，角度定义为欧拉角,
///< 安装平面初始坐标与北东地右手坐标一致，即X轴指向正北，Y轴指向正东
///< Z轴垂直指向地面。内置惯导固连在安装平面上，坐标系与安装平面坐标系平行。
///< 滚转角LSB=180.0/ 32767.0，即180.0/(2^15 - 1)，编码转换为short型，有效范围[-180.0,180.0]
};
#pragma pack(pop)
/*
* @brief: CRC 编码表,表长度为256个字节+1(1为结尾标识符)，该表不允许修改!!
*/
static const unsigned int telem_crc_table[257] = {
0x00000000, 0xB71DC104, 0x6E3B8209, 0xD926430D, 0xDC760413, 0x6B6BC517,
0xB24D861A, 0x0550471E, 0xB8ED0826, 0x0FF0C922, 0xD6D68A2F, 0x61CB4B2B,
0x649B0C35, 0xD386CD31, 0x0AA08E3C, 0xBDBD4F38, 0x70DB114C, 0xC7C6D048,
0x1EE09345, 0xA9FD5241, 0xACAD155F, 0x1BB0D45B, 0xC2969756, 0x758B5652,
0xC836196A, 0x7F2BD86E, 0xA60D9B63, 0x11105A67, 0x14401D79, 0xA35DDC7D,
0x7A7B9F70, 0xCD665E74, 0xE0B62398, 0x57ABE29C, 0x8E8DA191, 0x39906095,
0x3CC0278B, 0x8BDDE68F, 0x52FBA582, 0xE5E66486, 0x585B2BBE, 0xEF46EABA,
0x3660A9B7, 0x817D68B3, 0x842D2FAD, 0x3330EEA9, 0xEA16ADA4, 0x5D0B6CA0,
0x906D32D4, 0x2770F3D0, 0xFE56B0DD, 0x494B71D9, 0x4C1B36C7, 0xFB06F7C3,
0x2220B4CE, 0x953D75CA, 0x28803AF2, 0x9F9DFBF6, 0x46BBB8FB, 0xF1A679FF,
0xF4F63EE1, 0x43EBFFE5, 0x9ACDBCE8, 0x2DD07DEC, 0x77708634, 0xC06D4730,
0x194B043D, 0xAE56C539, 0xAB068227, 0x1C1B4323, 0xC53D002E, 0x7220C12A,
0xCF9D8E12, 0x78804F16, 0xA1A60C1B, 0x16BBCD1F, 0x13EB8A01, 0xA4F64B05,
0x7DD00808, 0xCACDC90C, 0x07AB9778, 0xB0B6567C, 0x69901571, 0xDE8DD475,
0xDBDD936B, 0x6CC0526F, 0xB5E61162, 0x02FBD066, 0xBF469F5E, 0x085B5E5A,
0xD17D1D57, 0x6660DC53, 0x63309B4D, 0xD42D5A49, 0x0D0B1944, 0xBA16D840,
0x97C6A5AC, 0x20DB64A8, 0xF9FD27A5, 0x4EE0E6A1, 0x4BB0A1BF, 0xFCAD60BB,
0x258B23B6, 0x9296E2B2, 0x2F2BAD8A, 0x98366C8E, 0x41102F83, 0xF60DEE87,
0xF35DA999, 0x4440689D, 0x9D662B90, 0x2A7BEA94, 0xE71DB4E0, 0x500075E4,
0x892636E9, 0x3E3BF7ED, 0x3B6BB0F3, 0x8C7671F7, 0x555032FA, 0xE24DF3FE,
0x5FF0BCC6, 0xE8ED7DC2, 0x31CB3ECF, 0x86D6FFCB, 0x8386B8D5, 0x349B79D1,
0xEDBD3ADC, 0x5AA0FBD8, 0xEEE00C69, 0x59FDCD6D, 0x80DB8E60, 0x37C64F64,
0x3296087A, 0x858BC97E, 0x5CAD8A73, 0xEBB04B77, 0x560D044F, 0xE110C54B,
0x38368646, 0x8F2B4742, 0x8A7B005C, 0x3D66C158, 0xE4408255, 0x535D4351,
0x9E3B1D25, 0x2926DC21, 0xF0009F2C, 0x471D5E28, 0x424D1936, 0xF550D832,
0x2C769B3F, 0x9B6B5A3B, 0x26D61503, 0x91CBD407, 0x48ED970A, 0xFFF0560E,
0xFAA01110, 0x4DBDD014, 0x949B9319, 0x2386521D, 0x0E562FF1, 0xB94BEEF5,
0x606DADF8, 0xD7706CFC, 0xD2202BE2, 0x653DEAE6, 0xBC1BA9EB, 0x0B0668EF,
0xB6BB27D7, 0x01A6E6D3, 0xD880A5DE, 0x6F9D64DA, 0x6ACD23C4, 0xDDD0E2C0,
0x04F6A1CD, 0xB3EB60C9, 0x7E8D3EBD, 0xC990FFB9, 0x10B6BCB4, 0xA7AB7DB0,
0xA2FB3AAE, 0x15E6FBAA, 0xCCC0B8A7, 0x7BDD79A3, 0xC660369B, 0x717DF79F,
0xA85BB492, 0x1F467596, 0x1A163288, 0xAD0BF38C, 0x742DB081, 0xC3307185,
0x99908A5D, 0x2E8D4B59, 0xF7AB0854, 0x40B6C950, 0x45E68E4E, 0xF2FB4F4A,
0x2BDD0C47, 0x9CC0CD43, 0x217D827B, 0x9660437F, 0x4F460072, 0xF85BC176,
0xFD0B8668, 0x4A16476C, 0x93300461, 0x242DC565, 0xE94B9B11, 0x5E565A15,
0x87701918, 0x306DD81C, 0x353D9F02, 0x82205E06, 0x5B061D0B, 0xEC1BDC0F,
0x51A69337, 0xE6BB5233, 0x3F9D113E, 0x8880D03A, 0x8DD09724, 0x3ACD5620,
0xE3EB152D, 0x54F6D429, 0x7926A9C5, 0xCE3B68C1, 0x171D2BCC, 0xA000EAC8,
0xA550ADD6, 0x124D6CD2, 0xCB6B2FDF, 0x7C76EEDB, 0xC1CBA1E3, 0x76D660E7,
0xAFF023EA, 0x18EDE2EE, 0x1DBDA5F0, 0xAAA064F4, 0x738627F9, 0xC49BE6FD,
0x09FDB889, 0xBEE0798D, 0x67C63A80, 0xD0DBFB84, 0xD58BBC9A, 0x62967D9E,
0xBBB03E93, 0x0CADFF97, 0xB110B0AF, 0x060D71AB, 0xDF2B32A6, 0x6836F3A2,
0x6D66B4BC, 0xDA7B75B8, 0x035D36B5, 0xB440F7B1, 0x00000001
};
/*
* @brief: CRC编码函数
*
*/
static unsigned int telemCRC(const unsigned char *buffer, unsigned int buffer_length)
{
unsigned int crc = 0;
const unsigned char *end = buffer + buffer_length;
while (buffer < end)
{
crc = telem_crc_table[((unsigned char)crc) ^ *buffer++] ^ (crc >> 8);
}
return crc;
}
///< 测控参数标识位，为固定值，不能更改
constexpr static const unsigned char telem_key[] = { '\x05', '\x7e','\x31','\x07','\x52','\x61','\x71','\x11','\x3e','\x03','\x33','\x41','\x53','\x1f','\x00','\x11' };
class telemCodec
{
public:
///< 角度，距离等计算时用到的量化精度和常量
constexpr static const double telem_LON_DEGREE_SAMPLE = (180.0 / 2147483647.0);
constexpr static const double telem_LAT_DEGREE_SAMPLE = (90.0 / 2147483647.0);
constexpr static const double telem_ALT_METERS_SAMPLE = (15000.0 / 32767.0);
constexpr static const double telem_YAW_DEGREE_SAMPLE = (360.0 / 65535.);
constexpr static const double telem_PITCH_DEGREE_SAMPLE = (180.0 / 32767.0);
constexpr static const double telem_ROLL_DEGREE_SAMPLE = (180.0 / 32767.0);
constexpr static const double telem_SPEED_EAST_SAMPLE = (1024.0 / 32767.0);
constexpr static const double telem_SPEED_NORTH_SAMPLE = (1024.0 / 32767.0);
constexpr static const double telem_SPEED_SKY_SAMPLE = (1024.0 / 32767.0);
constexpr static const double telem_FOV_H_DEGREES_SAMPLE = (180.0 / 65535.);
constexpr static const double telem_FOV_V_DEGREES_SAMPLE = (180.0 / 65535.);
constexpr static const double telem_SENSOR_YAW_DEGREES_SAMPLE = (360.0 / 65535.0);
constexpr static const double telem_SENSOR_PITCH_DEGREES_SAMPLE = (180.0 / 32767.0);
constexpr static const double telem_SENSOR_ROLL_DEGREES_SAMPLE = (180.0 / 32767.0);
constexpr static const double telem_LASER_DIST_METERS_SAMPLE = (10000.0 / 65535.0);
constexpr static const double telem_INV_LON_DEGREE_SAMPLE = 1.0 / telem_LON_DEGREE_SAMPLE;
constexpr static const double telem_INV_LAT_DEGREE_SAMPLE = 1.0 / telem_LAT_DEGREE_SAMPLE;
constexpr static const double telem_INV_ALT_METERS_SAMPLE = 1.0 / telem_ALT_METERS_SAMPLE;
constexpr static const double telem_INV_YAW_DEGREE_SAMPLE = 1.0 / telem_YAW_DEGREE_SAMPLE;
constexpr static const double telem_INV_PITCH_DEGREE_SAMPLE = 1.0 / telem_PITCH_DEGREE_SAMPLE;
constexpr static const double telem_INV_ROLL_DEGREE_SAMPLE = 1.0 / telem_ROLL_DEGREE_SAMPLE;
constexpr static const double telem_INV_SPEED_EAST_SAMPLE = 1.0/ telem_SPEED_EAST_SAMPLE;
constexpr static const double telem_INV_SPEED_NORTH_SAMPLE = 1.0/ telem_SPEED_NORTH_SAMPLE;
constexpr static const double telem_INV_SPEED_SKY_SAMPLE = 1.0/ telem_SPEED_SKY_SAMPLE;
constexpr static const double telem_INV_FOV_H_DEGREES_SAMPLE = 1.0 / telem_FOV_H_DEGREES_SAMPLE;
constexpr static const double telem_INV_FOV_V_DEGREES_SAMPLE = 1.0 / telem_FOV_V_DEGREES_SAMPLE;
constexpr static const double telem_INV_SENSOR_YAW_DEGREES_SAMPLE = 1.0 / telem_SENSOR_YAW_DEGREES_SAMPLE;
constexpr static const double telem_INV_SENSOR_PITCH_DEGREES_SAMPLE = 1.0 / telem_SENSOR_PITCH_DEGREES_SAMPLE;
constexpr static const double telem_INV_SENSOR_ROLL_DEGREES_SAMPLE = 1.0 / telem_SENSOR_ROLL_DEGREES_SAMPLE;
constexpr static const double telem_INV_LASER_DIST_METERS_SAMPLE = 1.0/ telem_LASER_DIST_METERS_SAMPLE;
constexpr static const double telem_FRAME_DEGREE_SAMPLE = (0.08 / 32767.);
/*
* @brief: 测控参数解码函数,将整数类型的数据通过LSB转换为double,float等类型，在解码前进行CRC校验
* 若校验码与存储在测控参数中的校验码不一致，表明数据被篡改，则返回错误
* @param: const unsigned char *pBuffer，输入，需要解码的测控数据指针，类型为unsigned char
* @param: const unsigned int &size，输入，需要解码的数据测控数据长度，类型为unsigned int
* @param: telemetryData &oData，输出，解码后的测控参数结构，类型为telemetryData结构
*/
inline static bool telemDecode(const unsigned char *pInputBuffer, const unsigned int &bufferSize, telemetryData &outputTelemetry)
{
if (bufferSize > sizeof(telem_key))
{
auto b = sizeof(telem_key);
// 比较标识位
if (memcmp(telem_key, pInputBuffer, sizeof(telem_key)) == 0)
{
unsigned char *pBuf = (unsigned char *)pInputBuffer + sizeof(telem_key);
int structLengthSize = (*pBuf) > 128 ? (*pBuf) - 128 : (*pBuf);
int infoLength = 0;
if (structLengthSize == 1)
{
pBuf++;
infoLength = getSubStructLength(structLengthSize, pBuf);
}
else
{
pBuf++;
unsigned char *stemp = pBuf;
unsigned short fieldSize;
memcpy(&fieldSize, stemp, sizeof(fieldSize));
infoLength = fieldSize;
pBuf += structLengthSize;
}
int crcFieldSize = sizeof(unsigned int);
if (infoLength > 256)
return false;
unsigned char *pLastInfo = (unsigned char *)pInputBuffer + infoLength;
// 计算校验码
unsigned int tcrc = telemCRC(pInputBuffer, infoLength);
// 读取飞控编码字段后四个字节组成的CRC校验码，数据类型为unsigned int
unsigned int storedCRC = 0;
unsigned char * pCRCPos = (unsigned char *)pInputBuffer + infoLength;
memcpy(&storedCRC, pCRCPos, sizeof(unsigned int));
// 防止野指针
pCRCPos = nullptr;
// 判断存储的CRC校验码与计算的校验码是否一致，如果不一致则返回错误
if (storedCRC != tcrc)
{
// 指针归零
pLastInfo = nullptr;
pBuf = nullptr;
return false;
}
while (pBuf < pLastInfo)
{
unsigned char infoTag = (*pBuf);
pBuf++;
// 根据信息标识解不同的类型
switch (infoTag)
{
case 1:// 时间标签：unsigned short mYear
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mYear = swap_uint16(*((unsigned short *)pBuf));
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 2:// 时间标签：unsigned char mMonth
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMonth = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 3:// 时间标签：unsigned char mDay
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDay = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 4:// 时间标签：unsigned char mHour
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mHour = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 5:// 时间标签：unsigned char mMinute
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMinute = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 6:// 时间标签：unsigned char mSecond
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSecond = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 7:// 时间标签：unsigned short mMillSecond
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMillSecond = swap_uint16(*((unsigned short *)pBuf));
pBuf += len;
outputTelemetry.mFlags.setTimestamp(true);
break;
}
case 8:// 平台类型代号：unsigned char mDroneType
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneType = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setPlatformType(true);
break;
}
case 9:// 平台编号：unsigned short mDroneNum
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneNum = swap_uint16(*((unsigned short *)pBuf));
pBuf += len;
outputTelemetry.mFlags.setPlatformID(true);
break;
}
case 10:// 平台航向角：float mDroneYaw,0到360度，大于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneYaw = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_YAW_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformRotation(true);
break;
}
case 11:// 平台俯仰角：float mDronePitch,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDronePitch = (float)swap_int16(*((short *)pBuf)) * telem_PITCH_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformRotation(true);
break;
}
case 12:// 平台滚转角：float mDroneRoll,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneRoll = (float)swap_int16(*((short *)pBuf)) * telem_ROLL_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformRotation(true);
break;
}
case 13:// 平台经度：double mDroneLongtitude,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneLongtitude = (double)swap_int32(*((int *)pBuf)) * telem_LON_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformPosition(true);
break;
}
case 14:// 平台纬度：double mDroneLatitude,-90到90度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneLatitude = (double)swap_int32(*((int *)pBuf)) * telem_LAT_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformPosition(true);
break;
}
case 15:// 平台高度：float mDroneAltitude
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneAltitude = (float)swap_int16(*((short *)pBuf)) * telem_ALT_METERS_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformPosition(true);
break;
}
case 16:// 平台东向速度分量：float mDroneSpeedEast
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneSpeedEast = (float)swap_int16(*((short *)pBuf)) * telem_SPEED_EAST_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformSpeed(true);
break;
}
case 17:// 平台北向速度分量：float mDroneSpeedNorth
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneSpeedNorth = (float)swap_int16(*((short *)pBuf)) * telem_SPEED_NORTH_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformSpeed(true);
break;
}
case 18:// 平台天向速度分量：float mDroneSpeedSky
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mDroneSpeedSky = (float)swap_int16(*((short *)pBuf)) * telem_SPEED_SKY_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setPlatformSpeed(true);
break;
}
case 19:// 传感器类型代号：unsigned char mSensorType
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorType = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setSensorType(true);
break;
}
case 20:// 传感器编号：unsigned short mSensorID
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorID = swap_uint16(*((unsigned short *)pBuf));
pBuf += len;
outputTelemetry.mFlags.setSensorID(true);
break;
}
case 21:// 传感器选择回报代号：unsigned char mSensorSelectionCB
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorSelectionCB = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setSensorSelectionCB(true);
break;
}
case 22:// 传感器水平视场角：float mSensorHFOV,0到180度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorHFOV = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_FOV_H_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setSensorFOV(true);
break;
}
case 23:// 传感器垂直视场角：float mSensorVFOV,0到180度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorVFOV = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_FOV_V_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setSensorFOV(true);
break;
}
case 24:// 传感器工作模式代号：unsigned char mSensorMod
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorMod = *((unsigned char *)pBuf);
pBuf += len;
outputTelemetry.mFlags.setSensorMod(true);
break;
}
case 25:// 传感器光轴相对安装平面航向角：float mSensorYaw,0到360度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorYaw = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_SENSOR_YAW_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setSensorRotation(true);
break;
}
case 26:// 传感器光轴相对安装平面的俯仰角：float mSensorPitch,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorPitch = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_PITCH_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setSensorRotation(true);
break;
}
case 27:// 传感器光轴相对于安装平面的滚转角：float mSensorRoll,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mSensorRoll = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_ROLL_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setSensorRotation(true);
break;
}
case 28:// 激光测距值：float mLaserDistance
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mLaserDistance = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_LASER_DIST_METERS_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setLaserDistance(true);
break;
}
case 29:// Mark的经度：double mMarkLongtitude,-180到180度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMarkLongtitude = (double)swap_int32(*((int *)pBuf)) * telem_LON_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setMarkPosition(true);
break;
}
case 30:// Mark的纬度：double mMarkLatitude,-90到90度，可能小于0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMarkLatitude = (double)swap_int32(*((int *)pBuf)) * telem_LAT_DEGREE_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setMarkPosition(true);
break;
}
case 31:// Mark的高度：float mMarkAltitude
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mMarkAltitude = (float)swap_int16(*((short *)pBuf)) * telem_ALT_METERS_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setMarkPosition(true);
break;
}
case 32:// 传感器框架在航向上的补偿角度，float mFrameYaw，0到360.0度，若没有航向上的补偿，则此项为0
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mFrameYaw = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_SENSOR_YAW_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setFrameRotation(true);
break;
}
case 33:// 传感器框架在俯仰方向上的补偿角度，float mFramePitch，-180.0到180.0度，
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mFramePitch = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_PITCH_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setFrameRotation(true);
break;
}
case 34:// 传感器框架在滚转方向上的补偿角度，float mFrameRoll，-180.0到180.0度，
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mFrameRoll = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_ROLL_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setFrameRotation(true);
break;
}
case 35:// 传感器的综合航向角，float mComposedYaw，0到360.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mComposedYaw = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_SENSOR_YAW_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setComposedAngle(true);
break;
}
case 36:// 传感器的综合俯仰角，float mComposedPitch，-180.0到180.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mComposedPitch = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_PITCH_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setComposedAngle(true);
break;
}
case 37:// 传感器的综合滚转角，float mComposedRoll，-180.0到180.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mComposedRoll = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_ROLL_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setComposedAngle(true);
break;
}
case 38:// 相机安装平面上的惯导测量得到的航向角，float mInnerInertiaYaw，0到360.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mInnerInertiaYaw = (float)swap_uint16(*((unsigned short *)pBuf)) * telem_SENSOR_YAW_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setInnerNavigation(true);
break;
}
case 39:// 相机安装平面上的惯导测量得到的俯仰角，float mInnerInertiaPitch，-180.0到180.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mInnerInertiaPitch = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_PITCH_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setInnerNavigation(true);
break;
}
case 40:// 相机安装平面上的惯导测量得到的滚转角，float mInnerInertiaRoll，-180.0到180.0度
{
int len = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
outputTelemetry.mInnerInertiaRoll = (float)swap_int16(*((short *)pBuf)) * telem_SENSOR_ROLL_DEGREES_SAMPLE;
pBuf += len;
outputTelemetry.mFlags.setInnerNavigation(true);
break;
}
default:
{
if (infoTag > 40)
{
pBuf = pLastInfo;
}
else
{
int valueLength = (*pBuf) > 128 ? getSubStructLength((*pBuf) - 128, pBuf) : getSubStructLength(pBuf);
pBuf += valueLength;
}
break;
}
}
}
// 防止野指针
pLastInfo = nullptr;
pBuf = nullptr;
return true;
}
}
return false;
}
/*
* @brief: 测控参数编码函数,将double,float等类型转换为整数类型
* @param: const telemetryData &inputTelemData，输入，需要编码的测控参数
* @param: std::vector<unsigned char> &outputBuffer，输出，编码之后的数据
* @param: unsigned int &outputBufferSize，输出，编码之后数据的大小
*/
inline static bool telemEncode(const telemetryData &inputTelemData, std::vector<unsigned char> &outputBuffer, unsigned int &outputBufferSize)
{
// 编码时平台的位置和姿态是必须填写的
if (inputTelemData.mFlags.isPlatformPosition() && inputTelemData.mFlags.isSensorRotation())
{
outputBuffer.resize(4096);
unsigned char *pBuf = &outputBuffer[0];
// 填入标识符
memcpy(pBuf, telem_key, sizeof(telem_key));
// 偏移
pBuf += sizeof(telem_key);
(*pBuf) = 130;
pBuf++;
unsigned int infoSize = (unsigned int)(pBuf - &outputBuffer[0]);
unsigned char *pSizeBuf = pBuf;
*pBuf = 0;
pBuf++;
*pBuf = 0;
pBuf++;
if(inputTelemData.mFlags.isTimeStamp())
{
// 1: 时间标签：unsigned short mYear
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 1;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16(inputTelemData.mYear);
memcpy(pBuf, &temp, fieldSize);
pBuf += fieldSize;
}
// 2: 时间标签：unsigned char mMonth
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 2;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mMonth), fieldSize);
pBuf += fieldSize;
}
//3: 时间标签：unsigned char mDay
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 3;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mDay), fieldSize);
pBuf += fieldSize;
}
// 4: 时间标签：unsigned char mHour
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 4;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mHour), fieldSize);
pBuf += fieldSize;
}
// 5: 时间标签：unsigned char mMinute
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 5;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mMinute), fieldSize);
pBuf += fieldSize;
}
// 6: // 时间标签：unsigned char mSecond
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 6;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mSecond), fieldSize);
pBuf += fieldSize;
}
// 7: 时间标签：unsigned short mMillSecond
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 7;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16(inputTelemData.mMillSecond);
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
// 8: 平台类型代号：unsigned char mDroneType
if (inputTelemData.mFlags.isPlatformType())
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 8;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mDroneType), fieldSize);
pBuf += fieldSize;
}
// 9: 平台编号：unsigned short mDroneNum
if (inputTelemData.mFlags.isPlatformID())
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 9;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16(inputTelemData.mDroneNum);
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
if (inputTelemData.mFlags.isPlatformRotation())
{
// 10: // 平台航向角：float mDroneYaw 0到360度，大于0
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 10;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mDroneYaw *telem_INV_YAW_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 11: 平台俯仰角：float mDronePitch，-180到180度，可能小于0
{
int fieldSize = sizeof(short);
(*pBuf) = 11;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDronePitch *telem_INV_PITCH_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 12: 平台滚转角：float mDroneRoll，-180到180度，可能小于0
{
int fieldSize = sizeof(short);
(*pBuf) = 12;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDroneRoll * telem_INV_ROLL_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
if (inputTelemData.mFlags.isPlatformPosition())
{
// 13: 平台经度：double mDroneLongtitude，-180到180度，可能小于0
{
int fieldSize = sizeof(int);
(*pBuf) = 13;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
int temp = swap_int32((int)(inputTelemData.mDroneLongtitude * telem_INV_LON_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 14: 平台纬度：double mDroneLatitude，-90到90度，可能小于0
{
int fieldSize = sizeof(int);
(*pBuf) = 14;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
int temp = swap_int32((int)(inputTelemData.mDroneLatitude * telem_INV_LAT_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 15: 平台高度：float mDroneAltitude
{
int fieldSize = sizeof(short);
(*pBuf) = 15;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDroneAltitude * telem_INV_ALT_METERS_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
if (inputTelemData.mFlags.isPlatformSpeed())
{
// 16: 平台东向速度分量：float mDroneSpeedEast
{
int fieldSize = sizeof(short);
(*pBuf) = 16;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDroneSpeedEast * telem_INV_SPEED_EAST_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 17: 平台北向速度分量：float mDroneSpeedNorth
{
int fieldSize = sizeof(short);
(*pBuf) = 17;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDroneSpeedNorth * telem_INV_SPEED_NORTH_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 18: 平台天向速度分量：float mDroneSpeedSky
{
int fieldSize = sizeof(short);
(*pBuf) = 18;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mDroneSpeedSky * telem_INV_SPEED_SKY_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
// 19: 传感器类型代号：unsigned char mSensorType
if (inputTelemData.mFlags.isSensorType())
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 19;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mSensorType), fieldSize);
pBuf += fieldSize;
}
// 20: 传感器编号：unsigned short mSensorID
if (inputTelemData.mFlags.isSensorID())
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 20;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16(inputTelemData.mSensorID);
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 21: 传感器选择回报代号：unsigned char mSensorSelectionCB
if (inputTelemData.mFlags.isSensorSelectionCB())
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 21;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mSensorSelectionCB), fieldSize);
pBuf += fieldSize;
}
if (inputTelemData.mFlags.isSensorFOV())
{
//22: 传感器水平视场角：float mSensorHFOV,0到180度,大于0
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 22;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mSensorHFOV * telem_INV_FOV_H_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 23: 传感器垂直视场角：float mSensorVFOV，0到180度，大于0
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 23;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mSensorVFOV * telem_INV_FOV_V_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
// 24: 传感器工作模式代号：unsigned char mSensorMod
if (inputTelemData.mFlags.isSensorMod())
{
int fieldSize = sizeof(unsigned char);
(*pBuf) = 24;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
memcpy(pBuf, &(inputTelemData.mSensorMod), fieldSize);
pBuf += fieldSize;
}
if (inputTelemData.mFlags.isSensorRotation())
{
// 25: 传感器光轴相对安装平面航向角：float mSensorYaw,0到360度，大于0
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 25;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mSensorYaw * telem_INV_SENSOR_YAW_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 26: 传感器光轴相对安装平面的俯仰角：float mSensorPitch,-180到180度，可能小于0
{
int fieldSize = sizeof(short);
(*pBuf) = 26;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mSensorPitch * telem_INV_SENSOR_PITCH_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 27: 传感器光轴相对于安装平面的滚转角：float mSensorRoll,-180到180度，可能小于0
{
int fieldSize = sizeof(short);
(*pBuf) = 27;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mSensorRoll * telem_INV_SENSOR_ROLL_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
// 28: 激光测距值：float mLaserDistance
if (inputTelemData.mFlags.isLaserDistance())
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 28;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mLaserDistance * telem_INV_LASER_DIST_METERS_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
if (inputTelemData.mFlags.isMarkPosition())
{
// 29: Mark的经度：double mMarkLongtitude,-180到180度，可能小于0
{
int fieldSize = sizeof(int);
(*pBuf) = 29;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
int temp = swap_int32((int)(inputTelemData.mMarkLongtitude *telem_INV_LON_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 30: Mark的纬度：double mMarkLatitude,-90到90度，可能小于0
{
int fieldSize = sizeof(int);
(*pBuf) = 30;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
int temp = swap_int32((int)(inputTelemData.mMarkLatitude * telem_INV_LAT_DEGREE_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 31: Mark的高度：float mMarkAltitude
{
int fieldSize = sizeof(short);
(*pBuf) = 31;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mMarkAltitude *telem_INV_ALT_METERS_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
if (inputTelemData.mFlags.isFrameRotation())
{
// 32: 传感器框架角在航向上的补偿角度，float mFrameYaw，0到360.0度，若没有航向上的补偿，则此项为0
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 32;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mFrameYaw * telem_INV_SENSOR_YAW_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 33: 传感器框架角在俯仰方向上的补偿角度，float mFramePitch，-180.0到180.0度，
{
int fieldSize = sizeof(short);
(*pBuf) = 33;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mFramePitch * telem_INV_SENSOR_PITCH_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 34: 传感器框架角在滚转方向上的补偿角度，float mFrameRoll，-180.0到180.0度，
{
int fieldSize = sizeof(short);
(*pBuf) = 34;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mFrameRoll * telem_INV_SENSOR_ROLL_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
if (inputTelemData.mFlags.isComposedAngle())
{
// 35: 传感器的综合航向角，float mComposedYaw，0到360.0度
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 35;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mComposedYaw * telem_INV_SENSOR_YAW_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 36: 传感器的综合俯仰角，float mComposedPitch，-180.0到180.0度
{
int fieldSize = sizeof(short);
(*pBuf) = 36;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mComposedPitch * telem_INV_SENSOR_PITCH_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 37: 传感器的综合滚转角，float mComposedRoll，-180.0到180.0度
{
int fieldSize = sizeof(short);
(*pBuf) = 37;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mComposedRoll * telem_INV_SENSOR_ROLL_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
if (inputTelemData.mFlags.isInnerNavigation())
{
// 38: 相机安装平面上的惯导测量得到的航向角，float mInnerInertiaYaw，0到360.0度
{
int fieldSize = sizeof(unsigned short);
(*pBuf) = 38;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
unsigned short temp = swap_uint16((unsigned short)(inputTelemData.mInnerInertiaYaw * telem_INV_SENSOR_YAW_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 39: 相机安装平面上的惯导测量得到的俯仰角，float mInnerInertiaPitch，-180.0到180.0度
{
int fieldSize = sizeof(short);
(*pBuf) = 39;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mInnerInertiaPitch * telem_INV_SENSOR_PITCH_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
// 40: 相机安装平面上的惯导测量得到的滚转角，float mInnerInertiaRoll，-180.0到180.0度
{
int fieldSize = sizeof(short);
(*pBuf) = 40;
pBuf++;
(*pBuf) = (unsigned char)(fieldSize);
pBuf++;
short temp = swap_int16((short)(inputTelemData.mInnerInertiaRoll * telem_INV_SENSOR_ROLL_DEGREES_SAMPLE));
memcpy(pBuf, &(temp), fieldSize);
pBuf += fieldSize;
}
}
// 计算CRC校验码的长度，目前是四个字节
int crcFieldSize = sizeof(unsigned int);
// 未增加校验码的缓存长度
outputBufferSize = (unsigned int)(pBuf - &outputBuffer[0]);
// 将未增加校验码的长度作为飞控编码长度，即飞控编码长度不包括校验码长度
unsigned short filedSize = (unsigned short)(outputBufferSize);
memcpy(pSizeBuf, &filedSize, sizeof(unsigned short));
// 计算CRC，内容为从缓存开始到飞控编码结束，不包括校验码自身
const unsigned char * pCrcInput = &outputBuffer[0];
unsigned int tcrc = telemCRC(&outputBuffer[0], outputBufferSize);
memcpy(pBuf, &(tcrc), crcFieldSize);
// 整个缓存的长度需要增加校验码的长度
outputBufferSize += crcFieldSize;
//将pBuf等归零防止野指针
pBuf = nullptr;
pSizeBuf = nullptr;
return true;
}
return false;
}
private:
/*
* @brief: 获取测控参数结构中某一子结构的长度，将数据指针偏移到实际数据位置
*/
inline static int getSubStructLength(const int &lenSize, unsigned char *&pBuf)
{
int subStructLength = 0;
switch (lenSize)
{
case 1:// 子结构长度为字节类型
{
subStructLength = (int)(*pBuf);
pBuf++;
break;
}
case 2:// 子结构为short类型，需要交换高低字节得到长度
subStructLength = (int)swap_int16(*((short*)pBuf));
pBuf += sizeof(short);
break;
case 4:// 子结构为int类型，需要交换数据，得到真实长度
{
subStructLength = swap_int32(*((int*)pBuf));
pBuf += sizeof(int);
break;
}
}
return subStructLength;
}
/*
* @brief: 当子结构的长度用字节表示时，获取子结构的长度，将数据指针偏移到实际数据位置
*/
inline static int getSubStructLength(unsigned char *&pBuf)
{
int len = (*pBuf);
pBuf++;
return len;
}
/*
* @brief: 交换unsigned short类型数据的高低位
*/
inline static unsigned short swap_uint16(unsigned short value)
{
return (value << 8) | (value >> 8);
}
/*
* @brief: 交换short类型数据的高低位
*/
inline static short swap_int16(short value)
{
return (value << 8) | ((value >> 8) & 0xFF);
}
/*
* @brief: 交换unsigned int类型数据的高低位
*/
inline static unsigned int swap_uint32(unsigned int value)
{
value = ((value << 8) & 0xFF00FF00) | ((value >> 8) & 0xFF00FF);
return (value << 16) | (value >> 16);
}
/*
* @brief: 交换int类型数据的高低位
*/
inline static int swap_int32(int value)
{
value = ((value << 8) & 0xFF00FF00) | ((value >> 8) & 0xFF00FF);
return (value << 16) | ((value >> 16) & 0xFFFF);
}
/*
* @brief: 交换无符号8字节数据类型数据的高低位
*/
inline static unsigned long long swap_uint64(unsigned long long value)
{
value = ((value << 8) & 0xFF00FF00FF00FF00ULL) | ((value >> 8) & 0x00FF00FF00FF00FFULL);
value = ((value << 16) & 0xFFFF0000FFFF0000ULL) | ((value >> 16) & 0x0000FFFF0000FFFFULL);
return (value << 32) | (value >> 32);
}
};
#endif // COMMONDEFINE_H