ArduPilot开源代码之AP_IOMCU

1. 源由

之前我们讨论了FMU + IMU的硬件架构，该设计主要是由于STM32外设IO、MCU计算资源不够应付外设的情况下，采用的一种折中的方案。

-ArduPilot开源代码之FMU+IOMCU设计
-Ardupilot开源飞控之IOMCU设计细节

随着技术的日益发展，更高效，更多的IO资源的MCU芯片已经具备。当然在特殊应用领域或者方案，仍然可以考虑采用这一方案。

为此，我们研究下AP_IOMCU应用类：FMU与IOMCU通信实现。

注：参考Pixhawk4硬件飞控板。

2. 框架设计

hwdef.h头文件定义FMU端通信端口UART8.

#define HAL_UART_IOMCU_IDX 8
#define HAL_UART_IO_DRIVER ChibiOS::UARTDriver uart_io(HAL_UART_IOMCU_IDX)

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp初始化FMU串口实例.

#if HAL_WITH_IO_MCU
HAL_UART_IO_DRIVER;
#include <AP_IOMCU/AP_IOMCU.h>
AP_IOMCU iomcu(uart_io);
#endif

2.1 启动代码

AP_Vehicle::setup
 └──> AP_BoardConfig::init
     └──> AP_BoardConfig::board_setup
         └──> hal.rcout->init();
             └──> AP_IOMCU::init
                 └──> !hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void)

2.2 初始化

采用1.5Mbps，默认(测试)确保，当前连接在无CTS/RTS的方式下进行通信无异常。

void AP_IOMCU::init(void)
{
    // uart runs at 1.5MBit, 不知道为何，因为后面`thread_main`里面仍然要初始化一遍。难道是为了`check_crc`
    uart.begin(1500*1000, 128, 128);
    uart.set_unbuffered_writes(true);

#if IOMCU_DEBUG_ENABLE
    crc_is_ok = true;
#else
    AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
    if ((!boardconfig || boardconfig->io_enabled() == 1) && !hal.util->was_watchdog_reset()) {
        check_crc();
    } else {
        crc_is_ok = true;
    }
#endif

    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
                                      1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
        AP_HAL::panic("Unable to allocate IOMCU thread");
    }
    initialised = true;
}

2.3 动态过程

AP_IOMCU::thread_main(void)
|
|-- 初始化部分
|   |-- thread_ctx = chThdGetSelfX()
|   |-- chEvtSignal(thread_ctx, initial_event_mask)
|   |-- uart.begin(1500*1000, 128, 128)
|   |-- uart.set_unbuffered_writes(true)
|
|-- 条件编译（DSHOT 相关配置）
|   |-- HAL_WITH_IO_MCU_BIDIR_DSHOT
|       |-- erpm_period_ms = 10
|       |-- HAVE_AP_BLHELI_SUPPORT
|           |-- blh = AP_BLHeli::get_singleton()
|           |-- erpm_period_ms = constrain_int16(1000 / blh->get_telemetry_rate(), 1, 1000)
|
|-- 触发初始化事件
|   |-- trigger_event(IOEVENT_INIT)
|
|-- 主循环
    |
    |-- 检查与 IOMCU 的通信
    |   |-- now_ms = AP_HAL::millis()
    |   |-- last_reg_access_ms 检查
    |       |-- INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset)
    |
    |-- 等待事件
    |   |-- mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10))
    |
    |-- 处理各类 IO 事件
        |
        |-- IOEVENT_SEND_PWM_OUT
        |   |-- send_servo_out()
        |
        |-- IOEVENT_INIT
        |   |-- 读取协议版本
        |       |-- read_registers()
        |       |-- is_chibios_backend 赋值
        |       |-- DEV_PRINTF
        |       |-- modify_register()
        |       |-- AP_IOMCU_FORCE_ENABLE_HEATER
        |           |-- modify_register()
        |
        |-- IOEVENT_MIXING
        |   |-- write_registers(PAGE_MIXING, ...)
        |
        |-- IOEVENT_FORCE_SAFETY_OFF
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)
        |
        |-- IOEVENT_FORCE_SAFETY_ON
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)
        |
        |-- IOEVENT_SET_RATES
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq)
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask)
        |
        |-- IOEVENT_ENABLE_SBUS
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, rate.sbus_rate_hz)
        |   |-- modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_SBUS1_OUT)
        |
        |-- IOEVENT_SET_HEATER_TARGET
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)
        |
        |-- IOEVENT_SET_DEFAULT_RATE
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)
        |
        |-- IOEVENT_SET_DSHOT_PERIOD
        |   |-- write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSHOT_PERIOD, ...)
        |
        |-- IOEVENT_SET_ONESHOT_ON
        |   |-- modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)
        |
        |-- IOEVENT_SET_BRUSHED_ON
        |   |-- modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)
        |
        |-- IOEVENT_SET_OUTPUT_MODE
        |   |-- write_registers(PAGE_SETUP, PAGE_REG_SETUP_OUTPUT_MODE, ...)
        |
        |-- IOEVENT_SET_CHANNEL_MASK
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_CHANNEL_MASK, pwm_out.channel_mask)
        |
        |-- IOEVENT_SET_SAFETY_MASK
        |   |-- write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)
        |
        |-- is_chibios_backend
        |   |-- IOEVENT_GPIO
        |       |-- write_registers(PAGE_GPIO, ...)
        |
        |-- IOEVENT_DSHOT
            |-- dshot_command_queue.pop(dshot)
            |-- write_registers(PAGE_DSHOT, ...)
    |
    |-- 定时事件检查
        |
        |-- 读取 RC 输入
        |   |-- now - last_rc_read_ms > 20
        |       |-- read_rc_input()
        |       |-- last_rc_read_ms = AP_HAL::millis()
        |
        |-- 读取状态
        |   |-- now - last_status_read_ms > 50
        |       |-- read_status()
        |       |-- last_status_read_ms = AP_HAL::millis()
        |       |-- write_log()
        |
        |-- 读取伺服输出
        |   |-- now - last_servo_read_ms > 50
        |       |-- read_servo()
        |       |-- last_servo_read_ms = AP_HAL::millis()
        |
        |-- 读取 ERPM 和遥测
        |   |-- HAL_WITH_IO_MCU_BIDIR_DSHOT
        |       |-- now - last_erpm_read_ms > erpm_period_ms
        |           |-- read_erpm()
        |           |-- last_erpm_read_ms = AP_HAL::millis()
        |       |-- now - last_telem_read_ms > 100
        |           |-- read_telem()
        |           |-- last_telem_read_ms = AP_HAL::millis()
        |
        |-- 更新安全选项
        |   |-- now - last_safety_option_check_ms > 1000
        |       |-- update_safety_options()
        |       |-- last_safety_option_check_ms = now
        |
        |-- 更新故障安全 PWM
            |-- pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent
                |-- write_registers(PAGE_FAILSAFE_PWM, ...)
                |-- pwm_out.failsafe_pwm_sent = set
    |
    |-- 发送 RC 协议
        |-- send_rc_protocols()
|
|-- 关闭线程标记
    |-- done_shutdown = true

3. 底层协议

+--------------------+
|    IOPacket        |
+--------------------+
| uint8_t count:6    | <-- 6 bits (范围：0-63)
+--------------------+
| uint8_t code:2     | <-- 2 bits (范围：0-3)
+--------------------+
| uint8_t crc        | <-- 8 bits (校验码)
+--------------------+
| uint8_t page       | <-- 8 bits (页码)
+--------------------+
| uint8_t offset     | <-- 8 bits (偏移量)
+--------------------+
| uint16_t regs[]    | <-- 16位寄存器数组，长度为 `PKT_MAX_REGS`
| (0 到 count-1)     |
+--------------------+

3.1 AP_IOMCU::read_registers

AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs)
|
|-- While 循环: 如果要读取的寄存器数量超过每包最大寄存器数
|   |-- 递归调用自身处理每包最大寄存器数
|   |-- 更新 offset, count 和 regs 指针
|
|-- 定义 IOPacket pkt 变量
|
|-- 丢弃 UART 输入缓冲区
|   |-- discard_input()
|
|-- 初始化 pkt 结构体
|   |-- memset(pkt.regs, 0, count*2)
|   |-- pkt.code = CODE_READ
|   |-- pkt.count = count
|   |-- pkt.page = page
|   |-- pkt.offset = offset
|   |-- pkt.crc = 0
|
|-- 计算 pkt_size
|   |-- if (is_chibios_backend)
|       |-- pkt_size = 4
|   |-- else
|       |-- pkt_size = pkt.get_size()
|
|-- 计算 pkt 的 CRC
|   |-- pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size)
|
|-- 通过 UART 写入 pkt 数据
|   |-- ret = write_wait((uint8_t *)&pkt, pkt_size)
|   |-- if (ret != pkt_size)
|       |-- debug("write failed1 %u %u %u\n", pkt_size, page, offset)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 等待 UART 超时或收到期望的字节数
|   |-- if (!uart.wait_timeout(count*2+4, 10))
|       |-- debug("t=%lu timeout read page=%u offset=%u count=%u avail=%u\n",
|                  AP_HAL::millis(), page, offset, count, uart.available())
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 检查 UART 可用数据长度
|   |-- if (n < offsetof(struct IOPacket, regs))
|       |-- debug("t=%lu small pkt %u\n", AP_HAL::millis(), n)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 检查 pkt 大小
|   |-- if (pkt.get_size() != n)
|       |-- debug("t=%lu bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size())
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 读取 UART 数据到 pkt
|   |-- uart.read(b, MIN(n, sizeof(pkt)))
|
|-- 验证 CRC
|   |-- got_crc = pkt.crc
|   |-- pkt.crc = 0
|   |-- expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size())
|   |-- if (got_crc != expected_crc)
|       |-- debug("t=%lu bad crc %02x should be %02x n=%u %u/%u/%u\n",
|                  AP_HAL::millis(), got_crc, expected_crc, n, page, offset, count)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 检查 pkt.code
|   |-- if (pkt.code != CODE_SUCCESS)
|       |-- debug("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 检查 pkt.count
|   |-- if (pkt.count < count)
|       |-- debug("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 将 pkt 中的寄存器值复制到 regs
|   |-- memcpy(regs, pkt.regs, count*2)
|
|-- 检查失败次数是否超过最大值
|   |-- if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES)
|       |-- handle_repeated_failures()
|
|-- 更新错误计数器和访问计数器
|   |-- total_errors += protocol_fail_count
|   |-- protocol_fail_count = 0
|   |-- protocol_count++
|   |-- last_reg_access_ms = AP_HAL::millis()
|
|-- 返回 true 表示读取成功

3.2 AP_IOMCU::write_registers

AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
|
|-- While 循环: 处理寄存器过多的情况（除 PAGE_SETUP 页面外）
|   |-- 递归调用自身处理最大可写寄存器数
|   |-- 更新 offset, count 和 regs 指针
|
|-- 定义 IOPacket pkt 变量
|
|-- 丢弃 UART 输入缓冲区
|   |-- discard_input()
|
|-- 初始化 pkt 结构体
|   |-- memset(pkt.regs, 0, count*2)
|   |-- pkt.code = CODE_WRITE
|   |-- pkt.count = count
|   |-- pkt.page = page
|   |-- pkt.offset = offset
|   |-- pkt.crc = 0
|   |-- memcpy(pkt.regs, regs, 2*count)
|   |-- pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size())
|
|-- 写入 pkt 到 UART
|   |-- pkt_size = pkt.get_size()
|   |-- ret = write_wait((uint8_t *)&pkt, pkt_size)
|   |-- if (ret != pkt_size)
|       |-- debug("write failed2 %u %u %u %u\n", pkt_size, page, offset, ret)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 等待 UART 应答
|   |-- if (!uart.wait_timeout(4, 10))
|       |-- debug("no reply for %u/%u/%u\n", page, offset, count)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 读取 UART 数据到 pkt
|   |-- uint8_t *b = (uint8_t *)&pkt
|   |-- uint8_t n = uart.available()
|   |-- for (uint8_t i=0; i<n; i++)
|       |-- if (i < sizeof(pkt))
|           |-- b[i] = uart.read()
|
|-- 验证 pkt.code
|   |-- if (pkt.code != CODE_SUCCESS)
|       |-- debug("bad code %02x write %u/%u/%u %02x/%02x n=%u\n", pkt.code, page, offset, count, pkt.page, pkt.offset, n)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 验证 CRC
|   |-- got_crc = pkt.crc
|   |-- pkt.crc = 0
|   |-- expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size())
|   |-- if (got_crc != expected_crc)
|       |-- debug("bad crc %02x should be %02x\n", got_crc, expected_crc)
|       |-- protocol_fail_count++
|       |-- return false
|
|-- 检查失败次数是否超过最大值
|   |-- if (protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES)
|       |-- handle_repeated_failures()
|
|-- 更新错误计数器和访问计数器
|   |-- total_errors += protocol_fail_count
|   |-- protocol_fail_count = 0
|   |-- protocol_count++
|
|-- 更新最后一次访问寄存器的时间
|   |-- last_reg_access_ms = AP_HAL::millis()
|
|-- 返回 true 表示写入成功

4. API函数

• read/write one channel

void write_channel(uint8_t chan, uint16_t pwm);
uint16_t read_channel(uint8_t chan);

• cork/push output

void cork(void);
void push(void);

• set/get output frequency

void set_freq(uint16_t chmask, uint16_t freq);
uint16_t get_freq(uint16_t chan);

• get state of safety switch

AP_HAL::Util::safety_state get_safety_switch_state(void) const;

• force safety on/off

bool force_safety_on(void);
void force_safety_off(void);

• set mask of channels that ignore safety state

void set_safety_mask(uint16_t chmask);

• set PWM of channels when in FMU failsafe

void set_failsafe_pwm(uint16_t chmask, uint16_t period_us);

• enable sbus output

bool enable_sbus_out(uint16_t rate_hz);

• check for new RC input

bool check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_channels);

• Do DSM receiver binding

void bind_dsm(uint8_t mode);

• get the name of the RC protocol

const char *get_rc_protocol(void);

• get receiver RSSI

int16_t get_RSSI(void)

• get servo rail voltage / rssi voltage adc counts

uint16_t get_vservo_adc_count(void)
uint16_t get_vrssi_adc_count(void)

• set target for IMU heater

void set_heater_duty_cycle(uint8_t duty_cycle);

• set default output rate

void set_default_rate(uint16_t rate_hz);

• set to oneshot/brushed mode

void set_oneshot_mode(void);
void set_brushed_mode(void);

• set/get output mode

void set_output_mode(uint16_t mask, uint16_t mode);
AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const;

• set bi-directional mask

void set_bidir_dshot_mask(uint16_t mask);

• get MCUID/CPUID

uint32_t get_mcu_id()
uint32_t get_cpu_id()

• set dshot output period

void set_dshot_period(uint16_t period_us, uint8_t drate);

• set telem request mask

void set_telem_request_mask(uint32_t mask);

• set the dshot esc_type

void set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType dshot_esc_type);

• send a dshot command

void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority);

• enable/disable channels

void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);

• check if IO is healthy

bool healthy(void);

• shutdown IO protocol (for reboot)

void shutdown();
void soft_reboot();

• setup for FMU failsafe mixing

bool setup_mixing(RCMapper *rcmap, int8_t override_chan, float mixing_gain, uint16_t manual_rc_mask);

• GPIO operation

bool valid_GPIO_pin(uint8_t pin) const; // Check if pin number is valid and configured for GPIO
bool convert_pin_number(uint8_t& pin) const; // convert external pin numbers 101 to 108 to internal 0 to 7
void set_GPIO_mask(uint8_t mask); // set GPIO mask of channels setup for output
void write_GPIO(uint8_t pin, bool value); // write to a output pin
void toggle_GPIO(uint8_t pin); // toggle a output pin

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
【7】ArduPilot开源代码之AP_DAL研读系列