【CrazySwarm】CrazySwarm代码解读-Crazyradio

于 2025-03-24 19:57:35 发布
阅读量636 收藏 20
点赞数 29
分类专栏： CrazySwarm 文章标签： 无人机集群 集群 Crazyswarm
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/weixin_45761762/article/details/146484743
版权

目录

文章目录


Crazyflie Radio 2 power setting location?

image-20250218205737300

其上是有一个功率放大器的。

今天核心任务是梳理清楚Crazyradio的构造过程和发送packet的核心机理。

class Crazyradio: public ITransport, public USBDevice
{
public:
    enum Datarate
    {
        Datarate_250KPS = 0,
        Datarate_1MPS = 1,
        Datarate_2MPS = 2,
    };
    
    enum Power 
    {
        Power_M18DBM = 0,
        Power_M12DBM = 1,
        Power_M6DBM = 2,
        Power_0DBM = 3,
    };
}

从上述Crazyradio定义中可以看出，该类有两个基类ITransport和USBDevice，两个枚举变量Datarate和Power

  1. ITransport：
    1. 一个抽象基类，其中有统一的数据传输规范和日志管理
    2. Crazyradio 类需要实现其中的两个纯虚接口来完成数据的收发操作。
  2. USBDevice：
    1. USB 设备的基类，Crazyradio 作为一个 USB 设备，继承该类可以复用一些 USB 设备的通用功能
    2. 主要是打开USB设备以及向USB设备发送数据

文档CrazySwarm代码梳理3-Crazyradio基类中对上述两个类进行了深扒。本文档主要研究Crazyradio。

class Crazyradio: public ITransport, public USBDevice
{
private:
    uint8_t m_channel;
    uint64_t m_address;
    Datarate m_datarate;
    bool m_ackEnable;
}

​ 每个Crazyradio类主要维护信道m_channel，通信地址m_address，传输速率m_datarate，使能ackm_ackEnable,这几个私有变量。

class Crazyradio: public ITransport, public USBDevice
{
public:
    Crazyradio(uint32_t devid);
    virtual ~Crazyradio();
    static uint32_t numDevices();
    float version() const {return m_version;}
    /*设置Crazyradio的接口*/
    void setChannel(uint8_t channel){sendVendorSetup(SET_RADIO_CHANNEL, channel, 0, NULL, 0);}
    uint8_t getChannel() const {return m_channel;}

    void setAddress(uint64_t address);
    uint64_t getAddress() const {return m_address;} 

    void setDatarate(Datarate datarate){sendVendorSetup(SET_DATA_RATE, datarate, 0, NULL, 0);}
    Datarate getDatarate() const {return m_datarate;}

    void setPower(Power power);

    void setArc(uint8_t arc);
    void setArdTime(uint8_t us);
    void setArdBytes(uint8_t nbytes);

    void setAckEnable(bool enable);
    bool getAckEnable() const {return m_ackEnable;}

    void setContCarrier(bool active);
		/*通过crazyradio的发送数据包接口*/
    virtual void sendPacket(
        const uint8_t* data,
        uint32_t length,
        ITransport::Ack& result);

    virtual void sendPacketNoAck(
        const uint8_t* data,
        uint32_t length);

    virtual void send2PacketsNoAck(
        const uint8_t* data,
        uint32_t totalLength);

}

上述Crazyradio的几个设置参数的接口都是调用了USBDevice基类中的sendVendorSetup，配置Crazyradio的参数。

核心的发送数据包的接口如下：

void Crazyradio::sendPacket(
    const uint8_t* data,
    uint32_t length,
    Ack& result)
{
    result.ack = false;
    result.size = 0;

    int status;
    int transferred;

    if (!m_handle) 
        throw std::runtime_error("No valid device handle!");
		 /*使能m_enableLogging的话，会往日志文件里打包一份发送的数据*/。
    if (m_enableLogging) 
        logPacket(data, length);

    //使用libusb_bulk_transfer接口发送数据
    status = libusb_bulk_transfer(
        m_handle,
        /* endpoint*/ (0x01 | LIBUSB_ENDPOINT_OUT),
        (uint8_t*)data,  //将data发送出去
        length,
        &transferred,
        /*timeout*/ 100);
    if (status != LIBUSB_SUCCESS)
        throw std::runtime_error(libusb_error_name(status));
    if (length != (uint32_t)transferred) {
        std::stringstream sstr;
        sstr << "Did transfer " << transferred << " but " << length << " was requested!";
        throw std::runtime_error(sstr.str());
    }

    // 读取发送的结果？
    status = libusb_bulk_transfer(
        m_handle,
        /* endpoint*/ (0x81 | LIBUSB_ENDPOINT_IN),
        (unsigned char*)&result,  //将result接收回来
        sizeof(result) - 1,
        &transferred,
        /*timeout*/ 10);
    // 如果读取超时了就会直接返回
    if (status == LIBUSB_ERROR_TIMEOUT) 
        return;
    if (status != LIBUSB_SUCCESS) 
        throw std::runtime_error(libusb_error_name(status));

    result.size = transferred - 1;

    if (m_enableLogging)
        logAck(result);   // log的位置太奇怪了
}

​ 从上述代码中可以看出libusb_bulk_transfer是sendPacket的数据发送端的最底层发送接口，其是有发送状态检测和超时保护的，如果发送失败了就报错，如果没有报错说明就没有发送失败。读取结果也是同样的过程，所以发送过程阻塞似乎并不是一个合理的解释。

​ 但是其发完数据之后，立马就开始接收，且只有10s

​ 除了其中定了的发送数据和读取结果外，m_enableLogging可以看到并没有被代码使能，其来源于ITransport初始化时的false，所以使用Crazyradio进行收发的所有数据包都是没有被log记录下来的，反过来如说，如果使能了m_enableLogging，就可以看到文件中生成了transport.log，看到所有原始数据的收发过程。

void Crazyradio::sendPacketNoAck(
    const uint8_t* data,
    uint32_t length)
{
    int status;
    int transferred;

    if (!m_handle)
        throw std::runtime_error("No valid device handle!");
    if (m_enableLogging)
        logPacket(data, length);
    // Send data
    status = libusb_bulk_transfer(
        m_handle,
        /* endpoint*/ (0x01 | LIBUSB_ENDPOINT_OUT), // 数据传输的端点，从主机到设备
        (uint8_t*)data,
        length,
        &transferred,
        /*timeout*/ 100);
    if (status != LIBUSB_SUCCESS) 
        throw std::runtime_error(libusb_error_name(status));
    if (length != (uint32_t)transferred) {
        std::stringstream sstr;
        sstr << "Did transfer " << transferred << " but " << length << " was requested!";
        throw std::runtime_error(sstr.str());
    }
}

只是少了ACK返回值，少了一个读取飞机端数据的过程。

void Crazyradio::send2PacketsNoAck(
    const uint8_t* data,
    uint32_t totalLength)
{
    int status;
    int transferred;

    if (!m_handle) 
        throw std::runtime_error("No valid device handle!");
    if (m_enableLogging) 
        logPacket(data, totalLength);
    // Send data
    status = libusb_bulk_transfer(
        m_handle,
        /* endpoint*/ (0x01 | LIBUSB_ENDPOINT_OUT),
        (uint8_t*)data,
        totalLength,
        &transferred,
        /*timeout*/ 100);
    if (status != LIBUSB_SUCCESS) 
        throw std::runtime_error(libusb_error_name(status));
    if (totalLength != (uint32_t)transferred) {
        std::stringstream sstr;
        sstr << "Did transfer " << transferred << " but " << totalLength << " was requested!";
        throw std::runtime_error(sstr.str());
    }
}

怎么通过一个Crazyradio给多个Crazyfile通信呢？

在每个Crazyflie中都维护两个通信类

class Crazyflie
{
private:
  Crazyradio* m_radio;				// 这个指针被通往Crazyradio
  ITransport* m_transport;  // 这个指针被通往CrazyUSB
  int m_devId;
  uint8_t m_channel;
  uint64_t m_address;
  Crazyradio::Datarate m_datarate;
}

​ 那么在该Crazyflie类中，整个Crazyradio是怎么建立起来的呢？每一个CF都维护了一个Crazyradio指针m_radio，但是每一个Crazyflie对象又是如何初始化该Crazyradio的呢？

Crazyradio* g_crazyradios[MAX_RADIOS];  // 这就是互斥锁保护的全局资源
std::mutex g_radioMutex[MAX_RADIOS];

Crazyflie::Crazyflie(
    const std::string& link_uri,
    Logger& logger,
    std::function<void(const char*)> consoleCb)
{
    int datarate, channel;
    char datarateType;
    bool success = false;
    /*从link_uri中取出devID, channel, datarate, m_address*/
    success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c/%" SCNx64,
                          &m_devId, &channel, &datarate,
                          &datarateType, &m_address) == 5;
    if (!success) {
        success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c",
                              &m_devId, &channel, &datarate,
                              &datarateType) == 4;
        m_address = 0xE7E7E7E7E7;
    }
    /*根据devId创建Crazyradion对象*/
    {
        // 利用一个新的作用域，是因为uniqui_lock对象，保护了一个全局资源
        std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
        if (!g_crazyradios[m_devId]) {
            g_crazyradios[m_devId] = new Crazyradio(m_devId);
            g_crazyradios[m_devId]->enableLogging(LOG_COMMUNICATION);//并未使能
            // g_crazyradios[m_devId]->setAckEnable(false);
            g_crazyradios[m_devId]->setAckEnable(true);
            g_crazyradios[m_devId]->setArc(0);
        }
        // 此处加锁是防止其他Crazyflie对象在这里影响通信链路。但实际上chooser.py运行的时候就跑了？
    }
    m_radio = g_crazyradios[m_devId];  	// 将这个全局资源的指针赋能给m_radio
}

​ 从上述信息可以看到，所有的CF都是公用的一个全局资源Crazyradio*的 g_crazyradios，所以当你调用了m_radio的通信接口的时候，如果仅有一个radio的话本质上都会调用同一个g_crazyradios。

​ 只不过在创建该对象的时候，需要有一些通信设置，在这个通信设置的过程中，需要使用互斥锁进行全局资源的保护。

void Crazyflie::sendPacketInternal(
    const uint8_t* data,
    uint32_t length,
    ITransport::Ack& ack,
    bool useSafeLink)
{
    static uint32_t numPackets = 0;
    static uint32_t numAcks = 0;

    numPackets++;

    if (m_radio) {
        std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
        if (m_radio->getAddress() != m_address)
            m_radio->setAddress(m_address);
        if (m_radio->getChannel() != m_channel)
            m_radio->setChannel(m_channel);
        if (m_radio->getDatarate() != m_datarate) 
            m_radio->setDatarate(m_datarate);
        if (!m_radio->getAckEnable()) 
            m_radio->setAckEnable(true);
        if (useSafeLink) {
            std::vector<uint8_t> dataCopy(data, data + length);
            dataCopy[0] &= 0xF3; //0x11110011B
            dataCopy[0] |= m_curr_up << 3 | m_curr_down << 2;
            m_radio->sendPacket(dataCopy.data(), length, ack);
            if (ack.ack && ack.size > 0 && (ack.data[0] & 0x04) == (m_curr_down << 2)) 
                m_curr_down = 1 - m_curr_down;
            if (ack.ack) 
                m_curr_up = 1 - m_curr_up;
        } else 
            m_radio->sendPacket(data, length, ack);
    }

​ 从上可知，在Crazyflie的最核心的通信接口 sendPacketInternal中在使用m_radio发送数据包sendPacket的时候，都对m_radio的资源进行互斥锁保护。非常多线程的时候是否会进行保护并不确定。

​ 但是假设有20个Crazyflie对象都同时调用了sendPacketInternal，其最后的数据出口都会被该unique_lock阻塞在门口，依次调用该接口进行数据发送，但是如果硬件允许的话，这些数据接口都应该是可以被依次发出的，尤其控制频率不是很高的情况下，个人觉得远远没有到其通信频率的极限，所以关于这个数据链，不止有通信频带频点一说，实际上应该还有通信带宽的说法，至于怎么确定通信带宽和通信硬件的极限还是需要进行学习。

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

后厂村路小狗蛋

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值