Ardupilot移植经验分享(3)

最新推荐文章于 2023-04-23 19:47:50 发布

wenbodong

最新推荐文章于 2023-04-23 19:47:50 发布

阅读量938

点赞数 8

分类专栏： ardupilot移植经验分享文章标签：嵌入式单片机

本文链接：https://blog.csdn.net/wenbodong/article/details/115435523

版权

ardupilot移植经验分享专栏收录该内容

4 篇文章 10 订阅

订阅专栏

接Ardupilot移植经验分享(2)

深入细节

是时候深入具体的HAL接口了。笔者并不打算一一讲解所有的接口，而是挑选一些有代表性的来分析，主要的内容是：

分析HAL接口的含义，包括功能，入参及返回值的具体含义。
分析HAL_PX4的实现，看看有没有可借鉴之处。

调度接口

AP_HAL::Scheduler提供了程序调度相关的接口。主要分为两类：

延时函数
注册回调

延时函数有3个，1个毫秒级延时，2个微秒级延时。这里的延时可不是死等，而是睡眠一段时间，在此期间让出CPU的使用权以执行其他的线程。

virtual void     delay(uint16_t ms) = 0;
virtual void     delay_microseconds(uint16_t us) = 0;
virtual void     delay_microseconds_boost(uint16_t us) { delay_microseconds(us); }

看似简单的delay

大家可能会觉得，delay是最简单的，delay_microseconds会比较复杂。因为通常来说，毫秒级延时是RTOS的基础API，比如RT-Thread的rt_thread_mdelay。

rt_err_t rt_thread_mdelay(rt_int32_t ms);

不过Scheduler::delay除了简单的睡眠延时，还多了一项任务。这就要先提到注册回调接口中的一个：

typedef void(*Proc)(void);
virtual void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms) = 0;

这接口的功能是：注册一个延时回调。当某个线程调用delay进行延时时，若延时的时间大于min_time_ms，则delay函数将会调用这个延时回调。

这延时回调的意义是，当你睡眠的时候，正好可以执行某一个指定的任务，不要浪费时间。你可能会想，RTOS的延时本就会让出CPU，本就会让别的线程得以执行，何必多此一举呢？我们看看是谁注册了这个回调。

其中之一是mavlink模块。

hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);

在这里插入图片描述

注释里面说明了原因，这是为了在长时间的初始化函数（setup）中能进行MAVLink交互。主要的MAVLink任务是在loop()中的AP_Scheduler调度系统中执行，就是红圈部分（不过红圈里面没提到MAVLink，不要介意哈）。setup()是顺序执行一系列的初始化函数，想在它里面去进行MAVLink任务，就靠这register_delay_callback了。
在这里插入图片描述
我们再看看PX4的实现。延时的功能依赖于一个微秒级延时的接口delay_microseconds_semaphore，不过它每次只延时1000微秒，多余的时间会去执行延时回调。

微秒级延时delay_microseconds

笔者当初看到这接口时，有些头疼。因为RT-Thread并没有微秒级别的延时函数，再强调一遍，这不是死等，是要睡眠让权的。

直接看PX4的实现：

void PX4Scheduler::delay_microseconds(uint16_t usec)
{
    perf_begin(_perf_delay);
    delay_microseconds_semaphore(usec);
    perf_end(_perf_delay);
}

这是上节提到的delay_microseconds_semaphore的马甲，脱了它：
在这里插入图片描述
该函数使用hrt_call_after和信号量来完成微秒级睡眠的功能。hrt是High-resolution timer的缩写，高精度定时器。

调用hrt_call_after注册一个定时器回调，定时时间是usec，单位微秒。回调函数是信号量发送函数sem_post，回调参数是信号量wait_semaphore。
使用sem_wait等待信号量wait_semaphore，此时当前线程会堵塞，进入睡眠状态。
到达定时时间后，底层就执行这个回调，即sem_post(&wait_semaphore)。
sem_wait接收到了信号量wait_semaphore，该线程被唤醒。

结合时序图来理解：
在这里插入图片描述
微秒级的延时函数，依赖于微秒级的定时回调，也就是hrt_call_after：

/**
 * Call callout(arg) after delay has elapsed.
 *
 * If callout is NULL, this can be used to implement a timeout by testing the call
 * with hrt_called().
 */
__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);

hrt_call_after属于pixhawk底层的接口。笔者一度以为是Nuttx提供的功能，并且为RT-Thread没有相应功能而烦恼。关于定时器，RT-Thread有类似的功能，那就是rt_timer，不过这是毫秒级的。

rt_timer_t rt_timer_create(const char *name,
                           void (*timeout)(void *parameter),
                           void       *parameter,
                           rt_tick_t   time,
                           rt_uint8_t  flag)

可能你会想，一般单片机里面不是都有硬件定时器吗？实现微秒级的定时功能很简单啊。确实简单，也不简单。因为hrt_call_after提供的定时功能是要支持并发的，千言万语不如一图：
在这里插入图片描述

至于pixhawk的实现以及笔者的移植，将专门出一篇文章来讲解。

delay_microseconds_boost

这个同样依赖于hrt_call_after，只是在睡眠的时候提高了优先级，以使得自己可在第一时间被唤醒。

注册回调

virtual void     register_timer_process(AP_HAL::MemberProc) = 0;
virtual void     register_io_process(AP_HAL::MemberProc) = 0;

相对来说，这两接口就简单的多了。它们用于注册在timer线程和io线程中运行的回调函数。
在这里插入图片描述
注册函数将回调指针加入到数组中。

在相应线程中，定时的一一执行。
在这里插入图片描述
那么，这线程是怎么创建的呢？PX4Scheduler的初始化函数中，会创建许多线程。

void PX4Scheduler::init()
{
    _main_task_pid = getpid();

    // setup the timer thread - this will call tasks at 1kHz
    pthread_attr_t thread_attr;
    struct sched_param param;

    pthread_attr_init(&thread_attr);
    pthread_attr_setstacksize(&thread_attr, 2048);

    param.sched_priority = APM_TIMER_PRIORITY;
    (void)pthread_attr_setschedparam(&thread_attr, &param);
    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);

    pthread_create(&_timer_thread_ctx, &thread_attr, &PX4Scheduler::_timer_thread, this);

    // the UART thread runs at a medium priority
    pthread_attr_init(&thread_attr);
    pthread_attr_setstacksize(&thread_attr, 2048);

    param.sched_priority = APM_UART_PRIORITY;
    (void)pthread_attr_setschedparam(&thread_attr, &param);
    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);

    pthread_create(&_uart_thread_ctx, &thread_attr, &PX4Scheduler::_uart_thread, this);

    // the IO thread runs at lower priority
    pthread_attr_init(&thread_attr);
    pthread_attr_setstacksize(&thread_attr, 2048);

    param.sched_priority = APM_IO_PRIORITY;
    (void)pthread_attr_setschedparam(&thread_attr, &param);
    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);

    pthread_create(&_io_thread_ctx, &thread_attr, &PX4Scheduler::_io_thread, this);

    // the storage thread runs at just above IO priority
    pthread_attr_init(&thread_attr);
    pthread_attr_setstacksize(&thread_attr, 1024);

    param.sched_priority = APM_STORAGE_PRIORITY;
    (void)pthread_attr_setschedparam(&thread_attr, &param);
    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);

    pthread_create(&_storage_thread_ctx, &thread_attr, &PX4Scheduler::_storage_thread, this);
}

pthread_attr_init，pthread_create，这些是POSIX的线程接口，由Nuttx提供。POSIX 是 “Portable Operating System Interface”（可移植操作系统接口）的缩写，POSIX 是 IEEE Computer Society 为了提高不同操作系统的兼容性和应用程序的可移植性而制定的一套标准。

好消息是，RT-Thread从3.0版本开始提供POSIX接口。所以，当我们移植的时候，很多地方可以参考AP_HAL_PX4的代码，甚至是直接复制它的代码。

串口驱动

AP_HAL的串口驱动框架由4个类组成，层层分工明确。

Print的功能，如其名称，负责打印输出。write系列为最底层的字节输出接口，需要由具体的HAL平台实现。print和println系列提供打印功能。所谓的打印，比如打印float，就是以字符格式输出float。笔者只列了几个print接口，实际上远比这个多。
Stream定义了输入接口，available返回接收缓存中的字节数，read用于读取输入。
BetterStream添加了格式化输出的接口，即大家熟悉的printf系列。
UARTDriver引入了与串口相关的接口，begin用于配置波特率、输入输出缓存，set_flow_control和get_flow_control与流控相关。

在这里插入图片描述

除了print系列函数由AP_HAL中的类实现，其他的功能，比如read, write, begin, flow_control，都要由具体的HAL平台实现。PX4UARTDriver就是具体的实现类。

串口绑定

Ardupilot有6个串口，定义在AP_HAL::HAL之中。

AP_HAL::UARTDriver* uartA;
AP_HAL::UARTDriver* uartB;
AP_HAL::UARTDriver* uartC;
AP_HAL::UARTDriver* uartD;
AP_HAL::UARTDriver* uartE;
AP_HAL::UARTDriver* uartF;

PX4UARTDriver构造函数和set_device_path用于与具体的底层串口相绑定。HAL_PX4_Class.c中定义了绑定关系：

// 3 UART drivers, for GPS plus two mavlink-enabled devices
static PX4UARTDriver uartADriver(UARTA_DEFAULT_DEVICE, "APM_uartA");
static PX4UARTDriver uartBDriver(UARTB_DEFAULT_DEVICE, "APM_uartB");
static PX4UARTDriver uartCDriver(UARTC_DEFAULT_DEVICE, "APM_uartC");
static PX4UARTDriver uartDDriver(UARTD_DEFAULT_DEVICE, "APM_uartD");
static PX4UARTDriver uartEDriver(UARTE_DEFAULT_DEVICE, "APM_uartE");
static PX4UARTDriver uartFDriver(UARTF_DEFAULT_DEVICE, "APM_uartF");

read和write的实现

大家可能认为串口的读写是非常简单的操作。其实不然，Ardupilot作为一个对实时要求很高的飞控程序，在应用层调用串口读写函数时，是不允许堵塞的。这需要一些额外的工作来实现。

PX4UARTDriver使用接收缓存和发送缓存来实现异步读写。

write是将数据写入到发送缓存_writebuf之中。_writebuf是一个队列，实质上是环形数组RingBuffer。

/*
   write one byte to the buffer
 */
size_t PX4UARTDriver::write(uint8_t c)
{
    if (_uart_owner_pid != getpid()){
        return 0;
    }
    if (!_initialised) {
        try_initialise();
        return 0;
    }

    while (_writebuf.space() == 0) {
        if (_nonblocking_writes) {
            return 0;
        }
        hal.scheduler->delay(1);
    }
    return _writebuf.write(&c, 1);
}

read从接收缓存中提取数据，若没有则返回-1。

/*
  read one byte from the read buffer
 */
int16_t PX4UARTDriver::read()
{
    if (_uart_owner_pid != getpid()){
        return -1;
    }
    if (!_initialised) {
        try_initialise();
        return -1;
    }

    uint8_t byte;
    if (!_readbuf.read_byte(&byte)) {
        return -1;
    }

    return byte;
}

将发送缓存的数据写入串口和从串口接收数据以填充接收缓存的工作，在PX4UARTDriver::_timer_tick函数中实现。而所有串口的_timer_tick由一个统一的串口线程来调度。

void *PX4Scheduler::_uart_thread(void *arg)
{
    PX4Scheduler *sched = (PX4Scheduler *)arg;

    pthread_setname_np(pthread_self(), "apm_uart");

    while (!sched->_hal_initialized) {
        poll(nullptr, 0, 1);
    }
    while (!_px4_thread_should_exit) {
        sched->delay_microseconds_semaphore(1000);

        // process any pending serial bytes
        ((PX4UARTDriver *)hal.uartA)->_timer_tick();
        ((PX4UARTDriver *)hal.uartB)->_timer_tick();
        ((PX4UARTDriver *)hal.uartC)->_timer_tick();
        ((PX4UARTDriver *)hal.uartD)->_timer_tick();
        ((PX4UARTDriver *)hal.uartE)->_timer_tick();
        ((PX4UARTDriver *)hal.uartF)->_timer_tick();
    }
    return nullptr;
}

看过前面高清大图的，应该对这个有印象：
在这里插入图片描述

SPI和I2C驱动

我们再看两个驱动，SPI驱动和I2C驱动。这两个驱动有很多共同之处：

都有总线的概念，一条总线挂接许多设备。
都有主从概念，每次传输由主机发起，由从机应答。

正是由于它们非常相似，Ardupilot提取出它们的共同之处，抽象成一个基类AP_HAL::Device。下图是Device的类图，并非包含其所有内容，仅列出了一些重要的元素。

在这里插入图片描述

UML图示说明

上图为UML类图。前面提到类图的语法，这里做一点补充。

变量和函数左边有颜色的符号表示访问权限，绿色圆圈是public，黄色菱形是protected。
斜体函数为纯虚函数，需要由子类实现。

transfer

Ardupilot的I2C和SPI驱动主要是用于与传感器通信，所以Device类提供了两个常用的接口： read_register和write_register，并且实现了它们。当然，这是基于transfer接口实现的，而transfer交由子类来实现，毕竟SPI和I2C的实现是不同的。

/*
 * Core transfer function. This does a single bus transaction which
 * sends send_len bytes and receives recv_len bytes back from the slave.
 *
 * Return: true on a successful transfer, false on failure.
 */
virtual bool transfer(const uint8_t *send, uint32_t send_len,
                      uint8_t *recv, uint32_t recv_len) = 0;

对于I2C来说，transfer实现的是先写后读。而对于SPI来说，transfer内部是同时读写。

SPIDevice和I2CDevice

在这里插入图片描述
它们添加了自身独有的接口。

SPIDevice添加了全双工的传输接口transfer_fullduplex，与transfer接口所不同之处在于发送和接收缓存的长度一致。
I2CDevice中，set_address用于设置地址，set_split_transfers指定在先写后读的中间是否传输停止位。

Periodic Callback

Device的功能远不只是为SPI和I2C定义了统一的transfer接口。最重要的，是实现了应用层访问总线的串行化。SPI和I2C都是由一条总线挂接许多设备，无论是SPI或I2C，都不允许在同一时刻访问多个设备。因此，Device提供了get_semaphore接口，以锁定总线。当然，这并不算是串行化，真正的串行化，是通过register_periodic_callback来实现。

virtual PeriodicHandle register_periodic_callback(uint32_t period_usec, PeriodicCb) = 0;

各传感器驱动通过register_periodic_callback注册定时回调，在回调之中访问对应的传感器。同一总线的所有定时回调是在同一个线程中被执行的，这就是串行化。

在这里插入图片描述
PX4在实现时，使用DeviceBus实现这个串行化的功能，其会为每一条总线创建一个线程。

其内部实现，无非是创建线程，将回调添加到一个链表之中。在函数中，POSIX接口的调用清晰可见。笔者的意思是，可以直接拿来用啦。
在这里插入图片描述

Manager

应用层通过Device来访问I2C和SPI设备，那么Device对象是哪来的呢？由I2CDeviceManager和SPIDeviceManager提供，而这两个Manager的实例可通过HAL引用访问。
在这里插入图片描述

小结

SPI和I2C传输的具体实现，没啥好说的。最值得说的，是Ardupilot抽象出了Device基类，为应用层提供串行化的访问功能。而这串行化，是靠创建线程和回调链表来实现。

是时候放一张高清大图了：
在这里插入图片描述

总结

到目前为止，我们看了调度接口，串口驱动，SPI和I2C驱动。调度接口中的微秒级延时接口非常关键，因为很多地方使用了它，并且它的实现有些困难。至于串口、SPI等驱动接口，只要我们理清了它们的层级关系，明确了各接口的作用，移植时不会有什么大问题。并且，这些驱动接口的实现，很多地方可以参考PX4的实现，甚至是直接复制过来用。