rfly_data.h

#pragma once
 
#include "rfly_wgs84.h"
 
typedef signed char        int8_t;
typedef short              int16_t;
typedef int                int32_t;
// typedef long long          int64_t;
typedef unsigned char      uint8_t;
typedef unsigned short     uint16_t;
typedef unsigned int       uint32_t;
// typedef unsigned long long uint64_t;
 
 
struct outHILStateShort{
 int checksum;
    int32_t gpsHome[3];     //Home GPS position, lat&long: deg*1e7, alt: m*1e3 and up is positive
 float AngEular[3];    //Estimated Euler angle, unit: rad
 float localPos[3];    //Estimated locoal position, NED, unit: m
 float localVel[3];    //Estimated locoal velocity, NED, unit: m/s
 outHILStateShort() {
        reset();
    }
 void reset(){
        checksum = 0;
 for(int i=0;i<3;i++){
            gpsHome[i]=0;
            AngEular[i]=0;
            localPos[i]=0;
            localVel[i]=0;
        }
    }
};
 
struct outHILStateData{
    uint32_t time_boot_ms; //Timestamp of the message  消息时间ćˆ?
    uint32_t copterID;     //Copter ID, start from 1
    int32_t GpsPos[3];     //Estimated GPS position��lat&long: deg*1e7, alt: m*1e3 and up is positive
    int32_t GpsVel[3];     //Estimated GPS velocity, NED, m/s*1e2->cm/s
    int32_t gpsHome[3];     //Home GPS position, lat&long: deg*1e7, alt: m*1e3 and up is positive
    int32_t relative_alt;  //alt: m*1e3 and up is positive
    int32_t hdg;           //Course angle, NED,deg*1000, 0~360
    int32_t satellites_visible; //GPS Raw data, sum of satellite
    int32_t fix_type;     //GPS Raw data, Fixed type, 3 for fixed (good precision)
    int32_t resrveInit;       //Int, reserve for the future use
 float AngEular[3];    //Estimated Euler angle, unit: rad
 float localPos[3];    //Estimated locoal position, NED, unit: m
 float localVel[3];    //Estimated locoal velocity, NED, unit: m/s
 float pos_horiz_accuracy;   //GPS horizontal accuracy, unit: m
 float pos_vert_accuracy; //GPS vertical accuracy, unit: m
 float resrveFloat;      //float,reserve for the future use
 outHILStateData() {
        reset();
    }
 void load(outHILStateShort hil) {
        reset();
 for(int i=0;i<3;i++){
            gpsHome[i]=hil.gpsHome[i];
            AngEular[i]=hil.AngEular[i];
            localPos[i]=hil.localPos[i];
            localVel[i]=hil.localVel[i];
        }
    }
 void reset(){
        time_boot_ms = 0;
 for(int i=0;i<3;i++){
            GpsPos[i]=0;
        }
 for(int i=0;i<3;i++){
            GpsVel[i]=0;
        }
        relative_alt=0;
        hdg=0;
 for(int i=0;i<3;i++){
            AngEular[i]=0;
        }
 for(int i=0;i<3;i++){
            localPos[i]=0;
        }
 for(int i=0;i<3;i++){
            localVel[i]=0;
        }
 for(int i=0;i<3;i++){
            gpsHome[i]=0;
        }
        pos_horiz_accuracy=0;
        pos_vert_accuracy=0;
        satellites_visible=0;
        fix_type=0;
    }
};
 
struct RflySimData{
    uint32_t time_boot_ms; //Timestamp of the message  消息时间ćˆ?
    uint32_t copterID;     //Copter ID, start from 1
    int32_t GpsPos[3];     //Estimated GPS position��lat&long: deg*1e7, alt: m*1e3 and up is positive
    int32_t GpsVel[3];     //Estimated GPS velocity, NED, m/s*1e2->cm/s
    int32_t gpsHome[3];     //Home GPS position, lat&long: deg*1e7, alt: m*1e3 and up is positive
    int32_t relative_alt;  //alt: m*1e3 and up is positive
    int32_t hdg;           //Course angle, NED,deg*1000, 0~360
    int32_t satellites_visible; //GPS Raw data, sum of satellite
    int32_t fix_type;     //GPS Raw data, Fixed type, 3 for fixed (good precision)
    int32_t resrveInit;       //Int, reserve for the future use
 float AngEular[3];    //Estimated Euler angle, unit: rad
 float AngRate[3];    //Estimated Euler angle, unit: rad
 float localPos[3];    //Estimated locoal position, NED, unit: m
 float localVel[3];    //Estimated locoal velocity, NED, unit: m/s
 float pos_horiz_accuracy;   //GPS horizontal accuracy, unit: m
 float pos_vert_accuracy; //GPS vertical accuracy, unit: m
 float resrveFloat;      //float,reserve for the future use
 double GlobalPos[3];
 
 WgsConversions wgs; //Class from rfly_wgs84.h for gps conversion
 RflySimData() {
        reset();
    }
 RflySimData(outHILStateShort hil) {
        load(hil);
    }
 RflySimData(outHILStateData hil) {
        load(hil);
    }
 void load(outHILStateShort hil) {
 //reset();
 for(int i=0;i<3;i++){
            gpsHome[i]=hil.gpsHome[i];
            AngEular[i]=hil.AngEular[i];
            localPos[i]=hil.localPos[i];
            localVel[i]=hil.localVel[i];
        }
    }
 void load(outHILStateData hil) {
 //reset();
        time_boot_ms = hil.time_boot_ms;
 for(int i=0;i<3;i++){
            GpsPos[i]=hil.GpsPos[i];
            GlobalPos[i]=0;
            AngRate[i]=0;
        }
 for(int i=0;i<3;i++){
            GpsVel[i]=hil.GpsVel[i];
        }
        relative_alt=hil.relative_alt;
        hdg=hil.hdg;
 for(int i=0;i<3;i++){
            AngEular[i]=hil.AngEular[i];
        }
 for(int i=0;i<3;i++){
            localPos[i]=hil.localPos[i];
        }
 for(int i=0;i<3;i++){
            localVel[i]=hil.localVel[i];
        }
 for(int i=0;i<3;i++){
            gpsHome[i]=hil.gpsHome[i];
        }
        pos_horiz_accuracy=hil.pos_horiz_accuracy;
        pos_vert_accuracy=hil.pos_vert_accuracy;
        satellites_visible=hil.satellites_visible;
        fix_type=hil.fix_type;
    } 
 void reset(){
        time_boot_ms = 0;
 for(int i=0;i<3;i++){
            GpsPos[i]=0;
            GlobalPos[i]=0;
            AngRate[i]=0;
        }
 for(int i=0;i<3;i++){
            GpsVel[i]=0;
        }
        relative_alt=0;
        hdg=0;
 for(int i=0;i<3;i++){
            AngEular[i]=0;
        }
 for(int i=0;i<3;i++){
            localPos[i]=0;
        }
 for(int i=0;i<3;i++){
            localVel[i]=0;
        }
 for(int i=0;i<3;i++){
            gpsHome[i]=0;
        }
        pos_horiz_accuracy=0;
        pos_vert_accuracy=0;
        satellites_visible=0;
        fix_type=0;
    }
 bool calcGlobalPos(double GpsOrin[3]){
 // Get global position according to GPS pos and given GPSOrin from SParamInfo
 if((fabs(gpsHome[0])<1&&fabs(gpsHome[1])<1)||(fabs(GpsOrin[0])<0.001&&fabs(GpsOrin[1])<0.001)){
            GlobalPos[0]=0;
            GlobalPos[1]=0;
            GlobalPos[2]=0;
 return false;
        }else{
 double GpsHomePos[3]={0};
            GpsHomePos[0] = (double)gpsHome[0] * 1e-7;
            GpsHomePos[1] = (double)gpsHome[1] * 1e-7;
            GpsHomePos[2] = (double)gpsHome[2] * 1e-3;
 double enu[3]={0};
            wgs.lla2enu(enu,GpsHomePos,GpsOrin);
            GlobalPos[0] = enu[1] + localPos[0];
            GlobalPos[1] = enu[0] + localPos[1];
            GlobalPos[2] = -enu[2] + localPos[2];
 return true;
        }
    }
};
 
 
 
 
 
typedef struct _netDataShortShort {
 int tg;
 int        len;
 char       payload[112];
 _netDataShortShort() {
        memset(payload, 0, sizeof(payload));
    }
}netDataShortShort;
 
 
 
struct inHILCMDData{
    uint32_t time_boot_ms;
    uint32_t copterID;
    uint32_t modes;
    uint32_t flags;
 float ctrls[16];
 inHILCMDData() {
        reset();
    }
 void reset(){
        time_boot_ms = 0;
        copterID=0;
        flags = 0;
        modes=0;
 for(int i=0;i<16;i++){
            ctrls[i]=0;
        }
    }
};
 
 
struct inOffboardShortData{
 int checksum;
 int ctrlMode;
 float controls[4];
 inOffboardShortData() {
        reset();
    }
 void reset(){
        checksum = 0;
        ctrlMode=0;
 for(int i=0;i<4;i++){
            controls[i]=0;
        }
    }
};
 
 
struct inRealflyData{
    uint32_t cmdBitmap;
    uint16_t typeMask;
 float controls[20];
 inRealflyData() {
        reset();
    }
 void reset(){
        cmdBitmap = 0;
        typeMask = 0;
 for(int i=0;i<20;i++){
            controls[i]=0;
        }
    }
};
 
 
 
 
// Python解码标识4i24f7d，数据长度168字节
//解析方法：对比包长168，然后按4i24f7d编码进行解析
//解析示例详见：PX4MavCtrlV4.py/getTrueDataMsg()函数
//发送示例详见：UE4CtrlAPI.py/sendUE4PosFull()函数
//输出到模拟器的数据
//FULL完整模式，输出到RflySim3D的数据
struct SOut2Simulator {
 int checksum; // 校验码123456789，必须设定为本值，才会认为是有效数据
 int copterID;  //飞机ID序号
 int vehicleType;  //载具样式ID，对应UE的XML中ClassID
 int reserv; //备用标志位，Int型，将来用于表示碰撞、或大地图等标识
 float VelE[3];   //速度，北东地，单位米/s
 float AngEuler[3];  //欧拉角，滚转俯仰偏航，单位弧度
 float AngQuatern[4]; //姿态四元数向量，w x y z
 float MotorRPMS[8];  //执行器偏转量或转速，旋翼类对应RPM转每分
 float AccB[3];       //载具机体FRD坐标系下加速度
 float RateB[3];      //载具机体FRD坐标系下角速度，pqr，单位rad/s
 double runnedTime; //时间戳，仿真开始为0时刻。
 double PosE[3];   //北东地位置，单位米，z向下为正
 double PosGPS[3];    //载具纬度、经度、高度向量，单位度和米，高度向上为正
 SOut2Simulator() {
        reset();
    }
 void reset(){
        checksum=0;
        copterID = -1;
        vehicleType= -1;
        runnedTime = 0;
        reserv=0;
 
 for (int i = 0; i < 3; i++) {
            PosE[i] = 0;
            AngEuler[i] = 0;
            VelE[i]=0;
            PosGPS[i]=0;
            RateB[i]=0;
            AccB[i]=0;
            AngQuatern[i]=0;
        }
        AngQuatern[3]=0;
 
 for(int i=0;i<8;i++){
            MotorRPMS[i]=0;
        }
    }
};