rfly_unitree.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 UnitreeDogMode{
    Damp = 1,
    BalanceStand,
    StopMove,
    PStandUp,
    StandDown,
    RecoveryStand,
    Sit,
    RiseSit,
    SwitchGait,
    BodyHeight,
    FootRaiseHeight,
    SpeedLevel,
    Hello,
    Stretch,
    SwitchJoystick,
    ContinuousGait,
    Wallow,
    Pose,
    Scrape,
    FrontFlip,
    FrontJump,
    FrontPounce,
    Dance
};
 
class RflyUnitree {
 public:
 
 RflySimData data;
 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};
 bool isReal=false;
 
 
 int SelfCheckMainState=0;
 int SelfCheckSubState=0;
 double lastTime=0;
 
 
 int offCtrlState=0;
 double offLastTime=0;
 
    uint64_t m_start_time = 0;
 GenericInOut28 m_cmd;
 GenericInOut28 m_recv;
 SOut2Simulator m_recv_ue;
 Ue4ExtMsg m_send_ue;
 
 // Dog status, the output of S func
 double m_global_pos[3] = {0};
 double m_ang_euler[3] = {0};
 double m_local_pos[3] = {0};
 double m_local_vel[3] = {0};
 double m_battery_info = 100;
 
    RflyUnitree(){
        m_cmd.ioSilInts[0] = 5;
        m_send_ue.checksum = 1234567894;
    }
    ~RflyUnitree(){
 
    }
 
 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){
                onUdpMessage((uint8_t*)buf, recvlen);
                isUpdate=true;
            }else{
 break;
            }
        }
 return isUpdate;
    }
 
 void onUdpMessage(uint8_t *buf, int recvlen){
 if(isReal){
 if (recvlen == sizeof(GenericInOut28)){
                unpackStruct(buf, m_recv);
 for (int i = 0; i < 3; i++){
                    m_global_pos[i] =  m_recv.ioSilFloats[i];
                    m_ang_euler[i] =  m_recv.ioSilFloats[i+3];
                    m_local_pos[i] =  m_recv.ioSilFloats[i+6];
                    m_local_vel[i] = m_recv.ioSilFloats[i+9];
                }
                m_battery_info = m_recv.ioSilFloats[12];
            }
 
        }
 else{
 if (recvlen == sizeof(SOut2Simulator)){
                unpackStruct(buf, m_recv_ue);
 for (int i = 0; i < 3; i++){
                    m_global_pos[i] =  m_recv_ue.PosE[i];
                    m_ang_euler[i] =  m_recv_ue.AngEuler[i];
                    m_local_pos[i] =  m_recv_ue.PosE[i];
                    m_local_vel[i] = m_recv_ue.VelE[i];
                }
            }
        }
    }
 
 void SendMsg(){
 unsigned int length;
 if (isReal){
            length = packStruct(m_cmd, (uint8_t*)buf);
        }
 else{
            length = packStruct(m_send_ue, (uint8_t*)buf);
        }
 
 if(length>0){
            buf[length]='0';
            udp.SendTo((const char *)buf,length,TargetIP,TargetPort);
 // std::cout << "send message to TargetIP \t"<<TargetIP << "TargetPort\t" << TargetPort << std::endl;
        }
    }
 
 void setBit(int &num, int n) {
 // 生成掩码：第n位为1，其余位为0
 int mask = 1 << n;
 // 执行按位或操作，将第n位设为1
        num = num | mask;
    }
 
 
 void SendSetAction(int mode)
    {
        m_cmd.ioSilInts[1] = 0;
        setBit(m_cmd.ioSilInts[1], 0);
        setBit(m_cmd.ioSilInts[1], 5);
        m_cmd.ioSilInts[2] = mode;
        SendMsg();
    }
 
 void SendMove(float vx, float vy, float yaw_rate)
    {
        m_cmd.ioSilInts[1] = 0;
        setBit(m_cmd.ioSilInts[1], 0);
        setBit(m_cmd.ioSilInts[1], 19);
        m_cmd.ioSilFloats[3] = vx;
        m_cmd.ioSilFloats[4] = vy;
        m_cmd.ioSilFloats[14] = yaw_rate;
        SendMsg();
    }
 
 bool isBitTrue(uint32_t value, int bitIndex) {  
 return (value & (1U << bitIndex)) != 0;
    }
 
 // input vector, input dim, sim time (s)
 void SendMavlinkReal(double **u, double t){
 
 int cmdBitmap = (int)*u[1];
 int typeMask = (int)*u[2];
 
 if (isBitTrue(cmdBitmap, 0)){
 if (isBitTrue(cmdBitmap, 5)){
 // std::cout << "SendSetAction: " <<typeMask <<  std::endl;
                SendSetAction(typeMask);
            }
        }
 if (isBitTrue(cmdBitmap, 19)){
 // std::cout << "SendMove: " <<(float)*u[11] << "\t" << (float)*u[12] << "\t" << (float)*u[22] <<  std::endl;
            SendMove((float)*u[11], (float)*u[12], (float)*u[22]);
        }
    }
 
 void SendUECtrl(double **u, double t){
 int cmdBitmap = (int)*u[1];
 int typeMask = (int)*u[2];
        m_send_ue.runnedTime = t;
 for (int i=0; i< 16; i++)
            m_send_ue.ExtToUE4[i] = 0;
 if (isBitTrue(cmdBitmap, 0)){
 if (isBitTrue(cmdBitmap, 5)){
                m_send_ue.ExtToUE4[0] = 2;
                m_send_ue.ExtToUE4[1] = typeMask;
                SendMsg();
            }
        }
 if (isBitTrue(cmdBitmap, 19)){
            m_send_ue.ExtToUE4[0] = 26112;
            m_send_ue.ExtToUE4[8] = -*u[22];
            m_send_ue.ExtToUE4[9] = *u[11];
            m_send_ue.ExtToUE4[10] = -*u[12];
            m_send_ue.ExtToUE4[14] = 2;
            SendMsg();
        }
 
    }
 
};