|
|
#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
|
