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};
 
 
 int SelfCheckMainState=0;
 int SelfCheckSubState=0;
 double lastTime=0;
 
 
 int offCtrlState=0;
 double offLastTime=0;
 
 //SOut2Simulator m_show3d;
    uint64_t m_start_time = 0;
 GenericInOut28 m_cmd;
 GenericInOut28 m_recv;
 
    RflyUnitree(){
        m_cmd.ioSilInts[0] = 5;
    }
    ~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);
            }else{
 break;
            }
        }
 return isUpdate;
    }
 
 void onUdpMessage(uint8_t *buf, int recvlen){
 if(recvlen==112){
            unpackStruct(buf, m_recv);
        }
    }
 
 void SendMsg(){
 unsigned int length = packStruct(m_cmd, (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]);
        }
    }
 
};