rfly_mavlink.h

#pragma once
 
#include "./mavlink/common/mavlink.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <vector>
#include <algorithm> // std::transform
#include <cctype>
#include "rfly_data.h"
#include "rfly_udp.h"
 
#define MAX_BUF_LEN 300 // More than max mavlink msg len
 
 
enum PX4_FLIGHTMODE{
    PX4_FLIGHTMODE_MANUAL = 1,
    PX4_FLIGHTMODE_ALT,
    PX4_FLIGHTMODE_POS,
    PX4_FLIGHTMODE_TAKEOFF,
    PX4_FLIGHTMODE_LAND,
    PX4_FLIGHTMODE_MISSION,
    PX4_FLIGHTMODE_RTL,
    PX4_FLIGHTMODE_STABILIZED,
    PX4_FLIGHTMODE_OFFBOARD,
    PX4_FLIGHTMODE_AUTO_READY,
    PX4_FLIGHTMODE_AUTO_LOITER
};
 
enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
    PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};
 
enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
    PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
};
 
enum PX4_CUSTOM_SUB_MODE_POSCTL {
    PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
    PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
};
 
union px4_custom_mode {
 struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
 float data_float;
 struct {
        uint16_t reserved_hl;
        uint16_t custom_mode_hl;
    };
};
 
 
 
struct MavStateData{
 bool isHeartRec=false;
 bool isLocalNed=false;
 bool isGlobPos=false;
 bool isGpsHome=false;
 int selfCheckState=0; //-1:faild, 0 inited, 1:uav connected, 2. get local pos, 3. can offboard, 4. can armed, 5. checkPassed
 
    uint8_t sysID=255;
    uint8_t compID=1;
    uint8_t TargetSysID;
    uint8_t TargetCompID;
 
    uint32_t custom_mode; /*<  A bitfield for use for autopilot-specific flags*/
    uint8_t base_mode; /*<  System mode bitmap.*/
    uint8_t system_status; /*<  System status flag.*/
    uint32_t time_boot_ms;
 //float initYaw;
 //float yawOffset;
 double batteryInfo[6];
 double healthInfo[10];
 
 bool isFailSafeEn=false;
 bool isArmed=false;
 bool isStanby=false;
 bool isActive=false;
    mavlink_sys_status_t sysStatus;
    uint8_t landed_state=0;
 
 bool isLowBatt=false;
 bool isHighLoad=false;
 bool isDropComm=false;
 bool isOffboard=false;
    PX4_FLIGHTMODE flyMode=PX4_FLIGHTMODE_MANUAL;
 double obstacle[4]; // [0] id for obstacle, [1-3] x,y,z position for obstacle
};
 
class RflyMavlink {
 public:
 
 RflySimData data;
 MavStateData mavState;
 UDPServer udp;
 //MavHealthData phm;
    std::string TargetIP;
 int TargetPort;
 int CopterID;
 int RecvPort;
    std::string RecvIP;
 char buf[MAX_BUF_LEN+1]; 
 bool isUpdate=false;
 bool isRcc=false;
 double GpsOrin[3]={0};
 
 
 int SelfCheckMainState=0;
 int SelfCheckSubState=0;
 double lastTime=0;
 
 
 int offCtrlState=0;
 double offLastTime=0;
 
 //SOut2Simulator m_show3d;
    uint64_t m_start_time = 0;
 
 RflyMavlink(){
 
    }
 ~RflyMavlink(){
 
    }
 
 
 bool RecvNoblock(){
 int retry = 0;
 while(true){ // Read all data in the upd buf zone
 int recvlen=udp.RecvNoblock(buf,RecvIP,RecvPort,MAX_BUF_LEN);
 
 // Five times has no useful message, will break
 if (udp.needFilter && (RecvIP != TargetIP)){
                retry++;
 if (retry > 5)
 break;
 continue;
            }
            retry = 0;
 
 if(recvlen>0){
                mavlink_message_t message;
 mavlink_status_t status;
                uint8_t msgReceived = false;
 
 if ((uint8_t)(buf[0]) == 0xEF){
 // std::cout << "receive 0xEF" << std::endl;
                    onUdpMessage((uint8_t*)buf, recvlen);
                }
 else{
 for(int i = 0 ; i < recvlen ; ++i){
                        msgReceived = mavlink_parse_char(MAVLINK_COMM_1, (uint8_t)buf[i], &message, &status);
 if(msgReceived){
                            onMavLinkMessage(message);
                            isUpdate=true;
                        }
                    }
                }
            }else{
 break;
            }
        }
 return isUpdate;
    }
 
 void onUdpMessage(uint8_t *buf, int recvlen){
 if (recvlen < 33){
            std::cout << "expect recvlen >=33, receive udp msg with length "<< recvlen << std::endl;
 return;
        }
 // std::cout << "received obstacle position: ";
 for (int i = 0; i < 4; i++){
 double received_double;
            memcpy(&received_double, &(buf[1+8*i]), sizeof(double));
            mavState.obstacle[i] = received_double;
 // std::cout << received_double << "\t";
        }
 // std::cout << std::endl;
    }
 
 
 void onMavLinkMessage(mavlink_message_t message){
 
 switch (message.msgid){
 case MAVLINK_MSG_ID_HEARTBEAT:{
                mavlink_heartbeat_t MavHartt;
                mavlink_msg_heartbeat_decode(&message, &MavHartt);
 // ignore QGC msg
 if (MavHartt.custom_mode == 0)
 break;
 if (!mavState.isHeartRec){
                    mavState.isHeartRec = true;
                    mavState.TargetSysID = message.sysid;
                    mavState.TargetCompID = message.compid;
                    std::cout<<"Drone #"<<CopterID<<": "<<"Heartbeat for SysID "<<std::to_string(message.sysid)<<" and CompID "<<std::to_string(message.compid)<<std::endl;
 //isLocalNed
                }
                mavState.base_mode = MavHartt.base_mode;
                mavState.custom_mode = MavHartt.custom_mode;
                mavState.system_status = MavHartt.system_status;
 
 if(mavState.system_status==3){
                    mavState.isStanby=true;
                }else{
                    mavState.isStanby=false;
                }
 
 if(mavState.system_status==4){
                    mavState.isActive=true;
                }else{
                    mavState.isActive=false;
                }
 
 if ((mavState.base_mode & static_cast<uint8_t>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0) {
 if(!mavState.isArmed){
                        mavState.isArmed = true;
                        std::cout<<"Drone #"<<CopterID<<": "<<"PX4 Armed "<<std::endl;
                    }
                }else{
 if(mavState.isArmed){
                        mavState.isArmed = false;
                        std::cout<<"Drone #"<<CopterID<<": "<<"PX4 Disamed. "<<std::endl;
                    }
                }
 
                PX4_FLIGHTMODE fm;
                getFlightMode_px4(MavHartt.custom_mode,fm);
 if(fm!=mavState.flyMode){
                    std::string qstr=getFlightModeName_px4(fm);
                    std::cout<<"Drone #"<<CopterID<<": "<<qstr<<std::endl;
                    mavState.flyMode=fm;
                }
 if(fm==PX4_FLIGHTMODE_OFFBOARD){
                    mavState.isOffboard=true;
                }
 else{
                    mavState.isOffboard=false;
                }
 
 break;
            }
 // MAVLINK_MSG_ID_SYS_STATUS may not have
 case MAVLINK_MSG_ID_SYS_STATUS:{
                mavlink_msg_sys_status_decode(&message, &mavState.sysStatus);
                mavState.batteryInfo[0] = mavState.sysStatus.battery_remaining;
                mavState.batteryInfo[1] = mavState.sysStatus.voltage_battery;
                mavState.batteryInfo[2] = mavState.sysStatus.current_battery;
 
 if(mavState.sysStatus.battery_remaining>=0 && mavState.sysStatus.battery_remaining<10){
 if (!mavState.isLowBatt){
                        mavState.isLowBatt=true;
                        std::cout<<"Drone #"<<CopterID<<": "<<"Low battery."<<std::endl;
                    }
                }
 if(mavState.sysStatus.load>900){
 if (!mavState.isHighLoad){
                        mavState.isHighLoad=true;
                        std::cout<<"Drone #"<<CopterID<<": "<<"High CPU load."<<std::endl;
                    }
                }
 
 if(mavState.sysStatus.drop_rate_comm>80){
 if (!mavState.isDropComm){
                        mavState.isDropComm=true;
                        std::cout<<"Drone #"<<CopterID<<": "<<"Communication high drop rate."<<std::endl;
                    }
                }
 break;
            }
 
 case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:{
                mavlink_extended_sys_state_t mss;
                mavlink_msg_extended_sys_state_decode(&message, &mss);
 //inCopterData[4]=mss.vtol_state;
 //inCopterData[5]=mss.landed_state;
 if(mavState.landed_state!=mss.landed_state){
                    mavState.landed_state=mss.landed_state;
 switch(mss.landed_state){
 case MAV_LANDED_STATE_ON_GROUND:
                            std::cout<<"Drone #"<<CopterID<<": "<<"On ground."<<std::endl;
 break;
 case MAV_LANDED_STATE_IN_AIR:
                            std::cout<<"Drone #"<<CopterID<<": "<<"In air."<<std::endl;
 break;
 case MAV_LANDED_STATE_TAKEOFF:
                            std::cout<<"Drone #"<<CopterID<<": "<<"Takeoff."<<std::endl;
 break;
 case MAV_LANDED_STATE_LANDING:
                            std::cout<<"Drone #"<<CopterID<<": "<<"Landing."<<std::endl;
 break;
                    }
                }
 break;
            }
 
 case MAVLINK_MSG_ID_LOCAL_POSITION_NED:{
                mavlink_local_position_ned_t lpn;
                mavlink_msg_local_position_ned_decode(&message, &lpn);
                data.localPos[0]=lpn.x;
                data.localPos[1]=lpn.y;
                data.localPos[2]=lpn.z;
                data.localVel[0] = lpn.vx;
                data.localVel[1] = lpn.vy;
                data.localVel[2] = lpn.vz;
                data.time_boot_ms = lpn.time_boot_ms;
 
 if(!mavState.isLocalNed){
                    mavState.isLocalNed=true;
                    std::cout<<"Drone #"<<CopterID<<": Got local pos x,y,z "<<data.localPos[0]<<","<<data.localPos[1]<<","<<data.localPos[2]<<","<<std::endl;
                }
 
 //isUpdate=true;
 
 if(mavState.isGpsHome){
 if(data.calcGlobalPos(GpsOrin)){
 if(!mavState.isGlobPos){
                            mavState.isGlobPos=true;
                            std::cout<<"Drone #"<<CopterID<<": Got global pos x,y,z "<<data.GlobalPos[0]<<","<<data.GlobalPos[1]<<","<<data.GlobalPos[2]<<","<<std::endl;
                        }
                    }
                }
 
 break;
            }
 case MAVLINK_MSG_ID_ATTITUDE:{
                mavlink_attitude_t att;
                mavlink_msg_attitude_decode(&message, &att);
 //data.time_boot_ms = RunnedTime;
 //data.copterID = copterID;
                data.AngEular[0]=att.roll;
                data.AngEular[1]=att.pitch;
                data.AngEular[2]=att.yaw;
                data.AngRate[0]=att.rollspeed;
                data.AngRate[1]=att.pitchspeed;
                data.AngRate[2]=att.yawspeed;
                data.time_boot_ms = att.time_boot_ms;
 // if (!mavState.initYaw && currentTime > 1){
 //     mavState.yawOffset = mavState.yaw - data[5];
 //     cout <<"sysID: "<<(int)(mavState.sysID) <<"\tyawOffset: "<< mavState.yawOffset << endl;
 //     mavState.initYaw = true;
 // }
 //isUpdate=true;
 break;
            }
 case MAVLINK_MSG_ID_BATTERY_STATUS:{
                mavlink_battery_status_t bat;
                mavlink_msg_battery_status_decode(&message, &bat);
                mavState.batteryInfo[0] = (double)bat.battery_remaining;
 break;
            }
 case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:{
                mavlink_global_position_int_t gp;
                mavlink_msg_global_position_int_decode(&message, &gp);
 //outHILStateData outHilData;
 //data.time_boot_ms = gp.time_boot_ms;
 //data.time_boot_ms = RunnedTime;
                data.GpsPos[0]=gp.lat;
                data.GpsPos[1]=gp.lon;
                data.GpsPos[2]=gp.alt;
                data.relative_alt = gp.relative_alt;
                data.GpsVel[0]=gp.vx;
                data.GpsVel[1]=gp.vy;
                data.GpsVel[2]=gp.vz;
                data.hdg = gp.hdg;
 break;
            }
 case MAVLINK_MSG_ID_GPS_RAW_INT:{
 //qDebug()<<"State Received!";
                mavlink_gps_raw_int_t gri;
                mavlink_msg_gps_raw_int_decode(&message, &gri);
 //data.time_boot_ms = gri.time_usec;
                data.fix_type = gri.fix_type;
 //qDebug()<<data.fix_type;
                data.satellites_visible = gri.satellites_visible;
 
 break;
            }
 case MAVLINK_MSG_ID_ESTIMATOR_STATUS:{
 
 //qDebug()<<"State Received!";
                mavlink_estimator_status_t es;
                mavlink_msg_estimator_status_decode(&message, &es);
 //data.time_boot_ms = RunnedTime;
 //data.time_boot_ms = es.time_usec;
                data.pos_horiz_accuracy = es.pos_horiz_accuracy;
                data.pos_vert_accuracy = es.pos_vert_accuracy;
 
 //qDebug()<<"Status: "<<data.pos_horiz_accuracy<<","<<es.pos_horiz_ratio;
 break;
            }
 
 case  MAVLINK_MSG_ID_HOME_POSITION:{
 
 //if(!m_flightModel.isSkipGps){
                mavlink_home_position_t hm;
                mavlink_msg_home_position_decode(&message, &hm);
                data.gpsHome[0] = hm.latitude;
                data.gpsHome[1] = hm.longitude;
                data.gpsHome[2] = hm.altitude;
 //data.time_boot_ms = RunnedTime;
                mavState.isGpsHome=true;
 break;
            }
 case MAVLINK_MSG_ID_STATUSTEXT:{
                mavlink_statustext_t mavStatusText;
                mavlink_msg_statustext_decode(&message, &mavStatusText);
                std::string mystr(mavStatusText.text);
                std::cout<<"Drone #"<<CopterID<<" STATUSTEXT: "<< mystr << std::endl;
                stringToLower(mystr);
 if (mystr.find("failsafe mode enabled") != std::string::npos) {
                    mavState.isFailSafeEn=true;
                }
 break;
            }
 case MAVLINK_MSG_ID_COMMAND_ACK:{
                mavlink_command_ack_t mck;
                mavlink_msg_command_ack_decode(&message, &mck);
                std::string qstrt;
 
 switch(mck.result){
 case 0:
                        qstrt="ACCEPTED";
 break;
 case 1:
                        qstrt="TEMPORARILY_REJECTED";
 break;
 case 2:
                        qstrt="DENIED";
 break;
 case 3:
                        qstrt="UNSUPPORTED";
 break;
 case 4:
                        qstrt="FAILED";
 break;
 case 5:
                        qstrt="IN_PROGRESS";
 break;
                }
 
                std::string cmdName;
 switch(mck.command){
 case 400:
                        cmdName = "ARM/DISARM";
 break;
 case 176:
                        cmdName = "SET_MODE";
 break;
 case 246:
                        cmdName = "REBOOT/SHUTDOWN";
 break;
 case 519:
                        cmdName = "REQUEST_MAVLINK_VERSION";
 break;
 case 520:
                        cmdName = "REQUEST_AUTOPILOT_VERSION";
 break;
 case 92:
                    cmdName = "NAV_GUIDED_ENABLE";
 break;
 case 21:
                        cmdName = "NAV_LAND";
 break;
 case 22:
                        cmdName = "NAV_TAKEOFF";
 break;
 case 192:
                        cmdName = "DO_REPOSITION";
 break;
 default:
                        cmdName = "ID: "+ std::to_string(mck.command);
 break;
                }
 
                std::cout<<"Drone #"<<CopterID<<": "<<"Command "<<cmdName<<" "<<qstrt<<std::endl;
 break;
            }
 
 
        }
    }
 
 
 void stringToLower(std::string& str)
    {
        std::transform(str.begin(), str.end(), str.begin(), [](char& c){
            return std::tolower(c);
        });
    }
 
 void SendMsg(mavlink_message_t mess){
 unsigned int length = mavlink_msg_to_send_buffer((uint8_t*)buf, &mess);
 if(length>0){
            buf[length]='0';
            udp.SendTo((const char *)buf,length,TargetIP,TargetPort);
        }
 
    }
 
 void SendHeartbeat(){
        mavlink_message_t mess;
        mavlink_heartbeat_t MavHartt;
        MavHartt.autopilot = MAV_AUTOPILOT_INVALID;
        MavHartt.base_mode = 0;
        MavHartt.custom_mode = 0;
        MavHartt.system_status = MAV_STATE_ACTIVE;
        MavHartt.type = MAV_TYPE_GCS;
        mavlink_msg_heartbeat_encode(mavState.sysID, mavState.compID, &mess, &MavHartt);
        SendMsg(mess);
    }
 
 void SendMavCmdLong(uint16_t command, float param1, float param2=0, float param3=0,float param4=0,float param5=0,float param6=0,float param7=0)
    {
        mavlink_message_t mess;
        mavlink_command_long_t command_long;
        command_long.target_system = mavState.TargetSysID;
        command_long.target_component = mavState.TargetCompID;
        command_long.confirmation = 0;
        command_long.command = command;
        command_long.param1 = param1;
        command_long.param2 = param2;
        command_long.param3 = param3;
        command_long.param4 = param4;
        command_long.param5 = param5;
        command_long.param6 = param6;
        command_long.param7 = param7;
        mavlink_msg_command_long_encode(mavState.sysID, mavState.compID, &mess, &command_long);
        SendMsg(mess);
    }
 
 
 void SendMavArm(int isArm)
    {
 // arm
 if (isArm) {
            SendMavCmdLong(MAV_CMD_COMPONENT_ARM_DISARM, 1);
        }
 // disarm 
 else {
            SendMavCmdLong(MAV_CMD_COMPONENT_ARM_DISARM, 0, 21196.0f);
        }
    }
 
 // Send MAVLink command to PX4 to change flight mode
 // https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MODE
 void SendSetMode(int mainMode, int customMode=0)
    {
        uint16_t baseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
        SendMavCmdLong(MAV_CMD_DO_SET_MODE, baseMode, mainMode, customMode);
    }
 
 void SendPositionTarget(uint16_t type_mask, uint8_t coordinate, float *data){
        mavlink_message_t mess;
 if (coordinate == MAV_FRAME_LOCAL_NED) {
            mavlink_set_position_target_local_ned_t m_target_ned;
 //m_target_ned.time_boot_ms = SGetCurrentTime();
            m_target_ned.target_system = mavState.TargetSysID;
            m_target_ned.target_component = mavState.TargetCompID;
            m_target_ned.coordinate_frame = coordinate;
            m_target_ned.type_mask = type_mask;
            m_target_ned.x = data[0];
            m_target_ned.y = data[1];
 // from vision frame to ned
            m_target_ned.z = data[2];
            m_target_ned.vx = data[3];
            m_target_ned.vy = data[4];
 // from vision frame to ned
            m_target_ned.vz = data[5];
            m_target_ned.afx = data[6];
            m_target_ned.afy = data[7];
 // from vision frame to ned
            m_target_ned.afz = data[8];
            m_target_ned.yaw = data[11];
 // from vision frame to ned
            m_target_ned.yaw_rate = data[14];
 //  from vision frame to ned
 // mavState.rotate(m_target_ned.x, m_target_ned.y);
 // mavState.rotate(m_target_ned.vx, m_target_ned.vy);
 // mavState.rotate(m_target_ned.afx, m_target_ned.afy);
 
            mavlink_msg_set_position_target_local_ned_encode(mavState.sysID, mavState.compID, &mess, &m_target_ned);
        }
 else if (coordinate == MAV_FRAME_GLOBAL_INT){
            mavlink_set_position_target_global_int_t m_target_global;
 //m_target_global.time_boot_ms = SGetCurrentTime();
            m_target_global.target_system = mavState.TargetSysID;
            m_target_global.target_component = mavState.TargetCompID;
            m_target_global.coordinate_frame = coordinate;
            m_target_global.type_mask = type_mask;
            m_target_global.lat_int = (int32_t)(data[0]*1e7);
            m_target_global.lon_int = (int32_t)(data[1]*1e7);
            m_target_global.alt = data[2];
            m_target_global.vx = data[3];
            m_target_global.vy = data[4];
            m_target_global.vz = data[5];
            m_target_global.afx = data[6];
            m_target_global.afy = data[7];
            m_target_global.afz = data[8];
            m_target_global.yaw = data[11];
            m_target_global.yaw_rate = data[14];
            mavlink_msg_set_position_target_global_int_encode(mavState.sysID, mavState.compID, &mess, &m_target_global);
        }
        SendMsg(mess);
    }
 
 
 void SendAttitudeTarget(uint8_t type_mask, float *data){
        mavlink_message_t mess;
        mavlink_set_attitude_target_t m_target_att;
 //m_target_att.time_boot_ms = SGetCurrentTime();
        m_target_att.target_system = mavState.TargetSysID;
        m_target_att.target_component = mavState.TargetCompID;
        m_target_att.type_mask = type_mask;
 
 float roll = data[9];
 float pitch = data[10];
 float yaw = data[11];
 float sp2 = sin(pitch/2);
 float sy2 = sin(yaw/2);
 float cr2 = cos(roll/2);
 float cp2 = cos(pitch/2);
 float cy2 = cos(yaw/2);
 float sr2 = sin(roll/2);
 
 float qx = sp2*sy2*cr2+cp2*cy2*sr2;
 float qy = sp2*cy2*cr2+cp2*sy2*sr2;
 float qz = cp2*sy2*cr2-sp2*cy2*sr2;
 float qw = cp2*cy2*cr2-sp2*sy2*sr2;
 
        m_target_att.q[0] = qw;
        m_target_att.q[1] = qx;
        m_target_att.q[2] = qy;
        m_target_att.q[3] = qz;
        m_target_att.body_roll_rate = data[12];
        m_target_att.body_pitch_rate = data[13];
        m_target_att.body_yaw_rate = data[14];
        m_target_att.thrust = data[15];
        mavlink_msg_set_attitude_target_encode(mavState.sysID, mavState.compID, &mess, &m_target_att);
        SendMsg(mess);
    }
 
 void SendParamInt(std::string id, int value){
        mavlink_message_t mess;
        mavlink_param_set_t param;
 
        param.target_system = mavState.TargetSysID;
        param.target_component = mavState.TargetCompID;
        param.param_value = float(value)/7.13622e+44; // A Change is required.
        param.param_type = MAV_PARAM_TYPE_INT32;
        strcpy(param.param_id, id.c_str());
 
        mavlink_msg_param_set_encode(mavState.sysID, mavState.compID, &mess, &param);
        SendMsg(mess);
    }
 
 void SendParamFloat(std::string id, float value){
        mavlink_message_t mess;
        mavlink_param_set_t param;
 
        param.target_system = mavState.TargetSysID;
        param.target_component = mavState.TargetCompID;
        param.param_value = value;
        param.param_type = MAV_PARAM_TYPE_REAL32;
        strcpy(param.param_id, id.c_str());
 
        mavlink_msg_param_set_encode(mavState.sysID, mavState.compID, &mess, &param);
        SendMsg(mess);
    }
 
 void SendRccTarget(float *data)
    {
        mavlink_message_t mess;
        mavlink_rc_channels_override_t rcc;
        uint16_t middle_rc = 1500;
 
        rcc.target_system = mavState.TargetSysID;
        rcc.target_component = mavState.TargetCompID;
        rcc.chan1_raw = (uint16_t)data[13];
        rcc.chan2_raw = (uint16_t)data[14];
        rcc.chan3_raw = (uint16_t)data[16];
        rcc.chan4_raw = (uint16_t)data[15];
 //cout << rcc.chan1_raw <<"\t" << rcc.chan3_raw << endl;
        rcc.chan5_raw = middle_rc;
        rcc.chan6_raw = middle_rc;
        rcc.chan7_raw = middle_rc;
        rcc.chan8_raw = middle_rc;
        mavlink_msg_rc_channels_override_encode(mavState.sysID, mavState.compID, &mess, &rcc);
 
        SendMsg(mess);
 
        mavlink_manual_control_t manual;
        manual.target = mavState.sysID;
        manual.x = (rcc.chan2_raw-1500)*2;
        manual.y = (rcc.chan1_raw-1500)*2;
        manual.z = rcc.chan3_raw-1000;
        manual.r = (rcc.chan4_raw-1500)*2;
        manual.buttons = 0;
        mavlink_msg_manual_control_encode(mavState.sysID, mavState.compID, &mess, &manual);
        SendMsg(mess);
    }
 
 
 
 void SendVisionPosition(uint64_t time_usec, float x, float y, float z, float roll, float pitch, float yaw)
    {
        mavlink_message_t mess;
        mavlink_vision_position_estimate_t vision_position;
        vision_position.usec = time_usec;
        vision_position.x = x;
        vision_position.y = y;
        vision_position.z = z;
        vision_position.roll = roll;
        vision_position.pitch = pitch;
        vision_position.yaw = yaw;
 
        mavlink_msg_vision_position_estimate_encode(mavState.sysID, mavState.compID, &mess, &vision_position);
 //std::cout<<std::to_string(time_usec)<<", "<<std::to_string(mavState.sysID)<<", "<<std::to_string(mavState.compID)<<", "<<std::to_string(mess.len)<<", "<<std::to_string(mess.seq)<<", "<<vision_position.x<<", "<<vision_position.y<<", "<<vision_position.z<<", "<<vision_position.yaw<<std::endl;
        SendMsg(mess);
    }
 
 
 // y0 -> Time stamp sec for pos msg
 // y1 -> Time stamp usec for pos msg
 // t_pos_msg = y0 + y1 / 1000000.0
 // y2-y4 -> Pos_x,Pos_y,Pos_z in ned, unit m
 // y5-y8 -> Quat (w,x,y,z) for vehicle attitude
 // y9 -> Time stamp sec for vel msg
 // y10 -> Time stamp usec for vel msg
 // t_vel_msg = y9 + y10 / 1000000.0
 // y11-y13 -> vel_x,vel_y,vel_z in ned, unit m/s
 // y14 -> Time stamp sec for acc msg
 // y15 -> Time stamp usec for acc msg
 // y16-y18 -> acc_x,acc_y,acc_z in ned, unit m/s^2
 // y19-y21 -> Euler (roll pitch yaw) ang from vehicle Quat
 // y22 -> tracker->has_data, is there any vrpn message received.
 // y23 -> tracker->has_pos, is the vrpn pos message received.
 // 24-29 -> reserve for future use
 void Send3DShow(double y[30])
    {
 SOut2Simulator m_show3d;
        uint64_t time_usec = y[0]*1000000+y[1];
 if(m_start_time==0){
            m_start_time=time_usec;
        }
 double runTime = (time_usec-m_start_time)/1000000.0; // 时间戳
        m_show3d.checksum=123456789;
        m_show3d.copterID = CopterID;
        m_show3d.runnedTime = runTime;
        m_show3d.vehicleType = 600251; // 显示飞机
 for (int i = 0; i < 3; i++) {
            m_show3d.PosE[i] = y[2+i];
            m_show3d.AngEuler[i] = y[19+i];
            m_show3d.VelE[i]= y[11+i];
            m_show3d.PosGPS[i]=0;
            m_show3d.RateB[i]=0;
            m_show3d.AccB[i]=y[16+i];
            m_show3d.AngQuatern[i]=y[5+i];
        }
        m_show3d.AngQuatern[3]=y[8];
 
 // 仿真超过1s，且高度大于0.2，才开启螺旋桨
 if(m_show3d.runnedTime>1 && m_show3d.PosE[2]<-0.1){
 for (int i = 0; i < 4; i++) {
                m_show3d.MotorRPMS[i] = 500; //单位RPM
                m_show3d.MotorRPMS[4+i] = 0;
            }
        }
 
 // 发送到RflySim3D用于三维显示
        udp.SendTo((const char *)&m_show3d,sizeof(m_show3d),"127.0.0.1",20010);
 
    }
 
 
 // void SendVisionOdometry(uint64_t time_usec, float pos[3],float vel[3],float quat[4],float rate[3], bool isReset=false)
 // {
 //     mavlink_message_t mess;
 //     mavlink_odometry_t odo;
 
 //     static int resetCont=0;
 
 //     /**
 //      * PX4 expects the body's linear velocity in the local frame,
 //      * the linear velocity is rotated from the odom child_frame to the
 //      * local NED frame. The angular velocity needs to be expressed in the
 //      * body (fcu_frd) frame.
 //      */
 //     uint8_t frame_id=MAV_FRAME_LOCAL_FRD;
 
 
 //     /**
 //      * Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD
 //      * The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION,
 //      * MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP
 //      *
 //      * @note Regarding the local frames of reference, the appropriate EKF_AID_MASK
 //      * should be set in order to match a frame aligned (NED) or not aligned (FRD)
 //      * with true North
 //      */
 //     uint8_t child_frame_id=MAV_FRAME_LOCAL_FRD;
 
 //     odo.time_usec = time_usec;
 //     odo.x=pos[0];/*< [m] X Position*/
 //     odo.y=pos[1];/*< [m] Y Position*/
 //     odo.z=pos[2];/*< [m] Z Position*/
 
 //     for(int i=0;i<4;i++){
 //         odo.q[i]=quat[i]; /*<  Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)*/
 //     }
 
 //     odo.vx=vel[0]; /*< [m/s] X linear speed*/
 //     odo.vy=vel[1]; /*< [m/s] Y linear speed*/
 //     odo.vz=vel[2]; /*< [m/s] Z linear speed*/
 
 //     odo.rollspeed=rate[0]; /*< [rad/s] Roll angular speed*/
 //     odo.pitchspeed=rate[1]; /*< [rad/s] Pitch angular speed*/
 //     odo.yawspeed=rate[2]; /*< [rad/s] Yaw angular speed*/
 
 //     float covariance[21]={0};
 //     covariance[0]=NAN; //set NAN to disable pose_covariance
 
 //     odo.estimator_type=MAV_ESTIMATOR_TYPE_VIO;
 
 //     if(isReset){ //self add if need reset.
 //         resetCont++;
 //     }
 //     odo.reset_counter=resetCont;
 
 //     mavlink_msg_odometry_encode(mavState.sysID, mavState.compID, &mess, &odo);
 //     SendMsg(mess);
 // }
 
 void setFlightMode_px4(PX4_FLIGHTMODE newMode, uint32_t& outCustomMode){
 px4_custom_mode outdata;
        outdata.data = 0;
 switch (newMode)
        {
 case PX4_FLIGHTMODE_MANUAL:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
            outdata.sub_mode = 0;
 break;
 case PX4_FLIGHTMODE_ALT:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
            outdata.sub_mode = 0;
 break;
 case PX4_FLIGHTMODE_POS:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
            outdata.sub_mode = 0;
 break;
 case PX4_FLIGHTMODE_TAKEOFF:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
 break;
 case PX4_FLIGHTMODE_LAND:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 break;
 case PX4_FLIGHTMODE_MISSION:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
 break;
 case PX4_FLIGHTMODE_RTL:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 break;
 
 case PX4_FLIGHTMODE_STABILIZED:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
            outdata.sub_mode = 0;
 break;
 
 case PX4_FLIGHTMODE_OFFBOARD:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
            outdata.sub_mode = 0;
 break;
 
 case PX4_FLIGHTMODE_AUTO_READY:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 break;
 
 case PX4_FLIGHTMODE_AUTO_LOITER:
            outdata.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
            outdata.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 break;
 
 default:
 break;
        }
        outCustomMode = outdata.data;
    }
 
 void getFlightMode_px4(uint32_t inCustomMode, PX4_FLIGHTMODE& outMode){
 px4_custom_mode indata;
        indata.data = inCustomMode;
 //qDebug()<<inCustomMode<<indata.main_mode<<indata.sub_mode;
 if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL){
            outMode = PX4_FLIGHTMODE_MANUAL;
        }
 else if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL){
            outMode = PX4_FLIGHTMODE_ALT;
        }
 else if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL){
            outMode = PX4_FLIGHTMODE_POS;
        }
 else if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD){
            outMode = PX4_FLIGHTMODE_OFFBOARD;
        }
 else if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED){
            outMode = PX4_FLIGHTMODE_STABILIZED;
        }
 else if (indata.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO)
        {
 if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION){
                outMode = PX4_FLIGHTMODE_MISSION;
            }
 else if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF){
                outMode = PX4_FLIGHTMODE_TAKEOFF;
            }
 else if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND){
                outMode = PX4_FLIGHTMODE_LAND;
            }
 else if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL){
                outMode = PX4_FLIGHTMODE_RTL;
            }
 else if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY){
                outMode = PX4_FLIGHTMODE_AUTO_READY;
            }
 else if (indata.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER){
                outMode = PX4_FLIGHTMODE_AUTO_LOITER;
            }
        }
    }
 
    std::string getFlightModeName_px4(PX4_FLIGHTMODE fm){
 
        std::string qstr="";
 switch (fm)
        {
 case 1:
            qstr="Manual Mode";
 break;
 case 2:
            qstr="Altitude Mode";
 break;
 case 3:
            qstr="Position Mode";
 break;
 case 4:
            qstr="Takeoff Mode";
 break;
 case 5:
            qstr="Land Mode";
 break;
 case 6:
            qstr="Auto Mode";
 break;
 case 7:
            qstr="RTL Mode";
 break;
 case 8:
            qstr="Stablized Mode";
 break;
 case 9:
            qstr="Offboard Mode";
 break;
 case 10:
            qstr="Auto Ready Mode";
 break;
 case 11:
            qstr="Auto Loiter Mode";
 break;
 default:
            qstr="Other Mode";
 break;
        }
 return qstr;
 
    }
 
 bool isBitTrue(uint32_t value, int bitIndex) {  
 return (value & (1U << bitIndex)) != 0;
    }
 
 // input vector, input dim, sim time (s)
 void SendMavlinkFull(double **u, double t){
 
 if(offCtrlState==0){
            SendVelNED(0,0,0,0);
            offLastTime=t;
            offCtrlState=1;
        }
 if(offCtrlState==1){
            SendVelNED(0,0,0,0);
 if(t-offLastTime>1.1){
                sendMavEnableGuided(true);
                offLastTime=t;
                offCtrlState=2;
            }
        }
 if(offCtrlState==2){
 if(t-offLastTime>1.1){
                SendMavArm(true);
                offCtrlState=3;
            }
        }
 if(offCtrlState==3){
 
 inHILCMDData hil;
 //inData.time_boot_ms = 123;
            hil.time_boot_ms = (uint32_t)*u[0];
            hil.copterID=(uint32_t)*u[1];
            hil.modes=(uint32_t)*u[2];
            hil.flags=(uint32_t)*u[3];
 for(int j=0;j<11;j++){
                hil.ctrls[j]=(float)*u[4+j];
            }
            uint16_t type_mask =  hil.modes & 0xFFF;
            uint8_t coordinate_frame = hil.flags & 0xFF;
            sendMavOffboardCmd(type_mask,coordinate_frame,hil.ctrls[0],hil.ctrls[1],hil.ctrls[2],hil.ctrls[3],hil.ctrls[4],hil.ctrls[5],hil.ctrls[6],hil.ctrls[7],hil.ctrls[8],hil.ctrls[9],hil.ctrls[10]);
 
        }
 
    }
 
 
 // input vector, input dim, sim time (s)
 void SendMavlinkSimple(double **u, double t){
 
 if(offCtrlState==0){
            SendVelNED(0,0,0,0);
            offLastTime=t;
            offCtrlState=1;
        }
 if(offCtrlState==1){
            SendVelNED(0,0,0,0);
 if(t-offLastTime>1.1){
                sendMavEnableGuided(true);
                offLastTime=t;
                offCtrlState=2;
            }
        }
 if(offCtrlState==2){
 if(t-offLastTime>1.1){
                SendMavArm(true);
                offCtrlState=3;
            }
        }
 if(offCtrlState==3){
 int ctrlMode=(int)*u[0];
 float ctrls[4];
 
 if(ctrlMode <0){ // no Data send mode
 return;
            }
 
 // the input len is numIn
 for(int j=0; j<4; j++){ // numIn-2 for controls
                ctrls[j] = (float)*u[1+j];
            }
 
            SendOffSimp(ctrlMode,ctrls);
        }
 
    }
 
 // idx=0 1 2 for rfly_ctrl rfly_ctrl1 rfly_ctrl2 msg to Simulink Controller
 // u[1], flags, related to isEnCtrl
 // u[2], mode, related to idx
 // u[8-23], controls, total 16 elements
 // The meaning of u[1], u[8-23] can be user defined
 void sendHILCtrlMessage(double **u, double t)
    {
        uint8_t modes=1;
        uint64_t flags=(uint64_t)*u[1];
 double idx=(double)*u[2];
 if(idx<0.5){
            modes=1; // for rfly_ctrl msg
        }else if(idx<1.5){
            modes=101; // for rfly_ctrl1 msg
        }else{
            modes=201; // for rfly_ctrl2 msg
        }
 
        mavlink_hil_actuator_controls_t hilctrl;
        hilctrl.mode = modes;
        hilctrl.flags = flags;
 for(int i=0;i<16;i++){
            hilctrl.controls[i]=(float)*u[i+8];
        }
        mavlink_message_t mess;
        mavlink_msg_hil_actuator_controls_encode(mavState.sysID, mavState.compID, &mess, &hilctrl);
        SendMsg(mess);
    }
 
 
 // input vector, input dim, sim time (s)
 void SendMavlinkReal(double **u, double t){
 
 inRealflyData ird;
        ird.cmdBitmap = (uint32_t)*u[1];
        ird.typeMask = (uint16_t)*u[2];
 
 // the input len is numIn
 for(int j=0; j<20; j++){ // numIn-8 for controls
            ird.controls[j] = (float)*u[8+j];
        }
 
 //
 if (isBitTrue(ird.cmdBitmap, 0)){
 // 
 if (isBitTrue(ird.cmdBitmap, 2))
                SendMavArm(1);
 else if (isBitTrue(ird.cmdBitmap, 7))
                SendMavArm(0);
 
 // Offboard
 if (isBitTrue(ird.cmdBitmap, 3)){
                SendSetMode(PX4_CUSTOM_MAIN_MODE_OFFBOARD);
            }
 // Mannual
 if (isBitTrue(ird.cmdBitmap, 28)){
                SendSetMode(PX4_CUSTOM_MAIN_MODE_MANUAL);
            }
 // Alt
 if (isBitTrue(ird.cmdBitmap, 29)){
                SendSetMode(PX4_CUSTOM_MAIN_MODE_ALTCTL);
            }
 // Position
 if (isBitTrue(ird.cmdBitmap, 30)){
                SendSetMode(PX4_CUSTOM_MAIN_MODE_POSCTL);
            }
 // AUTO
 if (isBitTrue(ird.cmdBitmap, 31)) {
                SendSetMode(MAV_MODE_FLAG_AUTO_ENABLED, PX4_CUSTOM_SUB_MODE_AUTO_LOITER);
            }
        }
 
 // send Offboard
 if (isBitTrue(ird.cmdBitmap, 16)) {
            uint8_t coordinate = MAV_FRAME_LOCAL_NED;
 if (isBitTrue(ird.cmdBitmap, 18))
                coordinate = MAV_FRAME_GLOBAL_INT;
 else if (isBitTrue(ird.cmdBitmap, 19))
                coordinate = MAV_FRAME_BODY_NED;
            SendPositionTarget(ird.typeMask, coordinate, ird.controls);
        }
 else if (isBitTrue(ird.cmdBitmap, 17)){
            SendAttitudeTarget(ird.typeMask, ird.controls);
        }
 else if (isBitTrue(ird.cmdBitmap, 23)){
 if (!isRcc){
                SendParamInt("NAV_RCL_ACT", 0);
                SendParamInt("NAV_DLL_ACT", 0);
                isRcc = true;
                SendSetMode(PX4_CUSTOM_MAIN_MODE_MANUAL);
            }
            SendRccTarget(ird.controls);
        }
 
    }
 
 void mavlinkSetCmd(uint16_t command,float param1=0,float param2=0,float param3=0){
        SendMavCmdLong(command,param1,param2,param3);
    }
 
 void sendMavSetParam(char param_id[], float param_value, MAV_PARAM_TYPE param_type)
    {
 
        mavlink_param_set_t ps;
        ps.target_system = mavState.TargetSysID;
        ps.target_component = mavState.TargetCompID;
        ps.param_value = param_value;
        ps.param_type = param_type;
 for(int i=0;i<16;i++){
            ps.param_id[i] = param_id[i];
        }
        mavlink_message_t mess;
        mavlink_msg_param_set_encode(mavState.sysID, mavState.compID, &mess, &ps);
 
    }
 
 
 void sendMavEnableGuided(bool isEnable)
    {
 if(isEnable){
            setMavModel(PX4_CUSTOM_MAIN_MODE_OFFBOARD);
 char cmd[17] = "NAV_RCL_ACT";
            sendMavSetParam(cmd,0,MAV_PARAM_TYPE_INT32);
 char cmd1[17] = "NAV_DLL_ACT";
            sendMavSetParam(cmd1,0,MAV_PARAM_TYPE_INT32);
 char cmd2[17] = "COM_RCL_EXCEPT";
            sendMavSetParam(cmd2,4/7.1362e+44,MAV_PARAM_TYPE_INT32);
        }else{
 //mavlinkSetCmd(MAV_CMD_NAV_GUIDED_ENABLE,false);
            setMavModel(PX4_CUSTOM_MAIN_MODE_MANUAL);
        }
    }
 void setMavModel(PX4_CUSTOM_MAIN_MODE mianMode){
        uint8_t basemode=static_cast<uint8_t>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);
 
        mavlinkSetCmd(MAV_CMD_DO_SET_MODE,basemode,mianMode);
    }
 
 void setMavModel(PX4_CUSTOM_MAIN_MODE mianMode,PX4_CUSTOM_SUB_MODE_AUTO custMode){
        uint8_t basemode=static_cast<uint8_t>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);
        mavlinkSetCmd(MAV_CMD_DO_SET_MODE,basemode,mianMode,custMode);
    }
 
 void sendMavOffboardCmd(uint16_t type_mask, uint8_t coordinate_frame, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate)
    {
        mavlink_set_position_target_local_ned_t m_target_ned;
 //m_target_ned.time_boot_ms = QDateTime::currentDateTime().currentMSecsSinceEpoch();
        m_target_ned.target_system = mavState.TargetSysID;
        m_target_ned.target_component = mavState.TargetCompID;
        m_target_ned.coordinate_frame = coordinate_frame;
        m_target_ned.type_mask = type_mask;
        m_target_ned.x = x;
        m_target_ned.y = y;
        m_target_ned.z = z;
        m_target_ned.vx = vx;
        m_target_ned.vy = vy;
        m_target_ned.vz = vz;
        m_target_ned.afx = afx;
        m_target_ned.afy = afy;
        m_target_ned.afz = afz;
        m_target_ned.yaw = yaw;
        m_target_ned.yaw_rate = yaw_rate;
 
        mavlink_message_t mess;
        mavlink_msg_set_position_target_local_ned_encode(mavState.sysID, mavState.compID, &mess, &m_target_ned);
        SendMsg(mess);
    }
 
 
 void SendOffSimp(int ctrlMode,float controls[4]={0}){
 float ctrls[11] = {0};
        uint8_t coordinate_frame = 0;
        uint16_t type_mask = 0;
 switch (ctrlMode)
        {
 case 0: // Earth vel Ctrl
            coordinate_frame = MAV_FRAME_LOCAL_NED;
            type_mask = 7 | 7 << 6 | 1 << 10;
            ctrls[3] = controls[0];
            ctrls[4] = controls[1];
            ctrls[5] = controls[2];
            ctrls[10] = controls[3];
 break;
 case 1: //body vel ctrl
            coordinate_frame = MAV_FRAME_BODY_NED;
            type_mask = 7 | 7 << 6 | 1 << 10;
            ctrls[3] = controls[0];
            ctrls[4] = controls[1];
            ctrls[5] = controls[2];
            ctrls[10] = controls[3];
 break;
 
 case 2: // earth pos ctrl
            coordinate_frame = MAV_FRAME_LOCAL_NED;
            type_mask = 7 << 3 | 7 << 6 | 1 << 11;
            ctrls[0] = controls[0];
            ctrls[1] = controls[1];
            ctrls[2] = controls[2];
            ctrls[9] = controls[3];
 break;
 case 3: // body pos ctrl
            coordinate_frame = MAV_FRAME_BODY_NED;
            type_mask = 7 << 3 | 7 << 6 | 1 << 11;
            ctrls[0] = controls[0];
            ctrls[1] = controls[1];
            ctrls[2] = controls[2];
            ctrls[9] = controls[3];
 break;
 
 case 6: // body pos ctrl
            coordinate_frame = MAV_FRAME_LOCAL_NED;
            type_mask = 7 | 7 << 3 | 1 << 10 | 1 << 11;
            ctrls[6] = controls[0];
            ctrls[7] = controls[1];
            ctrls[8] = controls[2];
 break;
 
 case 7: // 
            coordinate_frame = MAV_FRAME_LOCAL_NED;
            type_mask = 7 | 7 << 3 | 1 << 11;
            ctrls[6] = controls[0];
            ctrls[7] = controls[1];
            ctrls[8] = controls[2];
            ctrls[9] = controls[3];
 break;
 
 case 8: // 
            coordinate_frame = MAV_FRAME_LOCAL_NED;
            type_mask = 7 | 7 << 3 | 1 << 10;
            ctrls[6] = controls[0];
            ctrls[7] = controls[1];
            ctrls[8] = controls[2];
            ctrls[10] = controls[3];
 break;
 
 case 13:
        {
            coordinate_frame = 1;
            type_mask = 448;
 float vel = controls[0];
 float alt = controls[1];
 float yaw = controls[2];
 
            ctrls[2] = alt;
            ctrls[3] = yaw;
            ctrls[4] = vel;
 break;
        }
 
 default:
 break;
        }
 
        sendMavOffboardCmd(type_mask,coordinate_frame,ctrls[0],ctrls[1],ctrls[2],ctrls[3],ctrls[4],ctrls[5],ctrls[6],ctrls[7],ctrls[8],ctrls[9],ctrls[10]);
 
    }
 
 void SendVelNED(float vx=0,float vy=0,float vz=0,float yawrate=0){
 float ctrls[4]={vx,vy,vz,yawrate};
        SendOffSimp(0,ctrls);
    }
 
 // Self check program...
 bool SelfCheckProgram(double t){
 //std::cout<<"checking"<<std::endl;
 //-1:faild, 0 inited, 2. get local pos, 3. can offboard, 4. can armed, 5. checkPassed
 
 // Send this can get px4 info for any valid local ip.
 // if (t-lastHeartbeatTime >= 1) {
 //     SendHeartbeat();
 //     lastHeartbeatTime = t;
 // }
 if((SelfCheckMainState==0) && t>2){ //Connection check
 if(mavState.isHeartRec){
                std::cout<<"Drone #"<<CopterID<<": Checking mavlink connection successfully."<<std::endl;
                mavState.selfCheckState=1;
                SelfCheckMainState=1; //go to mode 1.
            }
        }
 
 if(SelfCheckMainState==1){
 if(mavState.isLocalNed){
                std::cout<<"Drone #"<<CopterID<<": Checking local pos successfully."<<std::endl;
                mavState.selfCheckState=2;
                SelfCheckMainState=2; //go to mode 2.
            }
        }
 
 if(SelfCheckMainState==2){
 // send offboard vel=0
            SendVelNED(0,0,0,0);
 
 if(SelfCheckSubState==0){
                lastTime=t; //Record sim time.
                SelfCheckSubState=1;
            }
 if(SelfCheckSubState==1){
                sendMavEnableGuided(true);
                SelfCheckSubState=2;
                lastTime=t; //Record sim time.
            }
 if(SelfCheckSubState==2){
 if(t-lastTime>1){
 if(mavState.isOffboard){
                        std::cout<<"Drone #"<<CopterID<<": Checking offboard successfully."<<std::endl;
                        mavState.selfCheckState=3;
                        SelfCheckMainState=3; //go to mode 3.
                        SelfCheckSubState=0;
                    }
 else{
                        SelfCheckSubState=1;
                    }
                }
            }
        }
 
 if(SelfCheckMainState==3){
 // send offboard vel=0
            SendVelNED(0,0,0,0);
 
 if(SelfCheckSubState==0){
                lastTime=t; //Record sim time.
 // send arm cmd
                SendMavArm(true);
                SelfCheckSubState=1;
            }
 
 if(SelfCheckSubState==1){
 if(t-lastTime>1){
 if(mavState.isArmed){
                        std::cout<<"Drone #"<<CopterID<<": Checking arm successfully."<<std::endl;
                        mavState.selfCheckState=4;
                        SelfCheckMainState=4; //go to mode 4.
                    }
                    SelfCheckSubState = 0;
                }
            }
        }
 
 if(SelfCheckMainState==4){
            SendVelNED(0,0,0,0);
 
 if(SelfCheckSubState==0){
                lastTime=t; //Record sim time.
                SelfCheckSubState=1;
                sendMavEnableGuided(false);
                SendMavArm(false);
            }
 
 if(SelfCheckSubState==1){
 if(t-lastTime>1){
 if(!mavState.isArmed && !mavState.isOffboard){
                        std::cout<<"Drone #"<<CopterID<<": Checking passed."<<std::endl;
                        mavState.selfCheckState=5;
                        SelfCheckMainState=5; //go to mode 5
                    }
                    SelfCheckSubState=0;
                }
            }
        }
 
 if(SelfCheckMainState==5){
 return true;
        }
 return false;
    }
};