Ardupilot开源飞控之IOMCU设计细节

1. 源由

• ArduPilot开源代码之FMU+IOMCU设计
• DShot on IOMCU - it’s here!
• Bi-directional DShot on IOMCU - it’s here!

鉴于最近研读了下FMU+IOMCU的设计，那么小伙伴们肯定要问，这个代码在哪里，怎么编啊？

2. 硬件设计

2.1 框图设计

注：以iomcu-f103-dshot为例！

2.2 硬件资源

主要硬件资源：

• SBUS (invertor) - UART3_RX
• RSSI_IN - ADC
• DSM_IN - UART1_RX
• FMU Communication - UART2 1.5Mbps
• 8 MAIN Outputs - GPIO x 8
• Saftey LED - GPIO x 1
• Safety Switch - GPIO x 1
• SPEKTRUM power Enable - GPIO x 1
• VDD Servo sense - GPIO x 1
• IO Status LED - GPIO x 1
• Debug pins

2.3 引脚定义

#define HAL_GPIO_PIN_AMBER_LED            PAL_LINE(GPIOB,15U)
#define HAL_GPIO_PIN_HEATER               PAL_LINE(GPIOB,14U)
#define HAL_GPIO_PIN_IO_HW_DETECT1        PAL_LINE(GPIOC,14U)
#define HAL_GPIO_PIN_IO_HW_DETECT2        PAL_LINE(GPIOC,15U)  // Not used
#define HAL_GPIO_PIN_RING_LED             PAL_LINE(GPIOA,11U)
#define HAL_GPIO_PIN_SAFETY_INPUT         PAL_LINE(GPIOB,5U)
#define HAL_GPIO_PIN_SAFETY_LED           PAL_LINE(GPIOB,13U)
#define HAL_GPIO_PIN_SBUS_OUT_EN          PAL_LINE(GPIOB,4U)
#define HAL_GPIO_PIN_SPEKTRUM_PWR_EN      PAL_LINE(GPIOC,13U)  // Not used
#define HAL_GPIO_PIN_TIM2_CH1             PAL_LINE(GPIOA,0U)
#define HAL_GPIO_PIN_TIM2_CH2             PAL_LINE(GPIOA,1U)
#define HAL_GPIO_PIN_TIM3_CH1             PAL_LINE(GPIOA,6U)
#define HAL_GPIO_PIN_TIM3_CH2             PAL_LINE(GPIOA,7U)
#define HAL_GPIO_PIN_TIM3_CH3             PAL_LINE(GPIOB,0U)
#define HAL_GPIO_PIN_TIM3_CH4             PAL_LINE(GPIOB,1U)
#define HAL_GPIO_PIN_TIM4_CH3             PAL_LINE(GPIOB,8U)
#define HAL_GPIO_PIN_TIM4_CH4             PAL_LINE(GPIOB,9U)
#define HAL_GPIO_PIN_USART1_RX            PAL_LINE(GPIOA,10U)
#define HAL_GPIO_PIN_USART1_TX            PAL_LINE(GPIOA,9U)
#define HAL_GPIO_PIN_USART2_RX            PAL_LINE(GPIOA,3U)
#define HAL_GPIO_PIN_USART2_TX            PAL_LINE(GPIOA,2U)
#define HAL_GPIO_PIN_USART3_RX            PAL_LINE(GPIOB,11U)
#define HAL_GPIO_PIN_USART3_TX            PAL_LINE(GPIOB,10U)
#define HAL_GPIO_PIN_VRSSI                PAL_LINE(GPIOA,5U)
#define HAL_GPIO_PIN_VSERVO               PAL_LINE(GPIOA,4U)

3. 通信协议

3.1 协议格式

+--------------------+
|    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)     |
+--------------------+

struct PACKED IOPacket {
    uint8_t 	count:6;
    uint8_t 	code:2;
    uint8_t 	crc;
    uint8_t 	page;
    uint8_t 	offset;
    uint16_t	regs[PKT_MAX_REGS];

    // get packet size in bytes
    uint8_t get_size(void) const
    {
        return count*2 + 4;
    }
};

3.2 字段说明

1. count (6 bits)

   • 描述：寄存器数组中有效寄存器的数量。因为是 6 bits，所以其值范围是 0 到 63。

2. code (2 bits)

   • 描述：数据包类型或状态码。因为是 2 bits，所以其值范围是 0 到 3。

3. crc (8 bits)

   • 描述：循环冗余校验码，用于验证数据包的完整性。

4. page (8 bits)

   • 描述：指示所操作的寄存器页。

5. offset (8 bits)

   • 描述：寄存器数组的起始位置偏移。

6. regs (数组，每个元素 16 bits)

   • 描述：实际的数据寄存器数组，元素个数由 count 确定。

3.3 结构体尺寸计算

根据 get_size() 函数，数据包的总尺寸计算方式如下：

• 固定部分：4 字节（count + code + crc + page + offset）
• 动态部分：count * 2 字节（每个寄存器占用 2 字节）

因此，数据包的总大小为 count * 2 + 4 字节。

4. 软件设计

4.1 源代码组成

根据工程脚本libraries/AP_IOMCU/iofirmware/wscript内容，源代码涉及以下内容：

 ├──> libraries/AP_IOMCU/iofirmware
 ├──> libraries/AP_Common
 ├──> libraries/AP_HAL
 ├──> libraries/AP_HAL_Empty
 ├──> libraries/AP_Math
 ├──> libraries/AP_RCProtocol
 ├──> libraries/AP_BoardConfig
 ├──> libraries/AP_ESC_Telem
 ├──> libraries/AP_SBusOut
 ├──> libraries/AP_Scripting
 └──> libraries/AP_HAL_ChibiOS

4.2 特性功能

• 与FMU采用通过UART2高速(1.5Mbps)通信
• 支持PWM/dshot/bdshot电调(8 MAIN ports)
• 支持SUBS输入/输出(带反向器74LVC2G240DP)
• 支持DSM输入
• 支持模拟RSSI输入
• 支持飞控加热
• 支持LED(AMBER_LED/RING_LED/SAFETY_LED/)
• 支持SAFETY开关
• 支持VDD电压监控

4.3 启动流程

setup
 │  /* RC in/out初始化 */
 ├──> hal.rcin->init();
 ├──> hal.rcout->init();
 │
 │  /* IOMCU初始化 */
 ├──> iomcu.init();
 │
 │  /* 固件CRC校验计算 */
 ├──> iomcu.calculate_fw_crc();
 │
 │  /* 启动与FMU之间的高速通信串口 */
 ├──> uartStart(&UARTD2, &uart_cfg);
 ├──> uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet);
 │
 │  /* 确保unlock状态 */
#if AP_HAL_SHARED_DMA_ENABLED
 │   iomcu.tx_dma_handle->unlock();
#endif
 │
 │
 │  /* 
 │   * disable the pieces from the UART which will get enabled later
 │   * 禁用 UART（通用异步收发传输器）的 DMA 发送功能；后面可能会使能。
 │   */ 
 ├──> chSysLock();
 ├──> UARTD2.usart->CR3 &= ~USART_CR3_DMAT;
 └──> chSysUnlock();

4.3.1 IOMCU启动

AP_IOMCU_FW::init()
 │ 
 │  /* 设置协议版本 */
 ├──> config.protocol_version = IOMCU_PROTOCOL_VERSION
 ├──> config.protocol_version2 = IOMCU_PROTOCOL_VERSION2
 │ 
 │  /* 读取 MCU ID */
 ├──> config.mcuid = (*(uint32_t *)DBGMCU_BASE)
 ├──> #if defined(STM32F103xB) || defined(STM32F103x8)
 │   └──> if (config.mcuid == 0)
 │       └──> config.mcuid = 0x20036410
 │ 
 │  /* 读取 CPU ID */
 ├──> config.cpuid = SCB->CPUID
 │ 
 │  /* 获取当前线程上下文 */
 ├──> thread_ctx = chThdGetSelfX()
 │ 
 ├──> /* #if AP_HAL_SHARED_DMA_ENABLED //创建和配置 DMA
 │   ├──> tx_dma_handle = NEW_NOTHROW ChibiOS::Shared_DMA(...)
 │   ├──> tx_dma_handle->lock()
 │   └──> tx_dma_deallocate(tx_dma_handle) //很特殊的情况，不知道为何？？？
 │ 
 │  /* 硬件检测和设置加热器 */
 ├──> if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 &&
 │   │    palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0)
 │   └──> has_heater = true
 │ 
 │  /* 设置加热器引脚模式 */
 ├──> if (heater_pwm_polarity)
 │   └──> palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_PUSHPULL)
 ├──> else
 │   └──> palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_OPENDRAIN)
 │ 
 │  /* ADC 和 RC 输入初始化 */
 ├──> adc_init()
 ├──> rcin_serial_init()
 │ 
 │  /* Spektrum 端口电源设置 */
 ├──> palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL)
 ├──> SPEKTRUM_POWER(1)
 │ 
 │  /* 记录可用内存 */
 ├──> reg_status.freemem = hal.util->available_memory()
 │ 
 │  /* 检查和处理看门狗安全状态 */
 └──> if (hal.util->was_watchdog_safety_off())
     ├──> hal.rcout->force_safety_off()
     └──> reg_status.flag_safety_off = true

4.3.2 ADC初始化

adc_init()
 │ 
 │  /* 启用 ADC1 */
 ├──> rccEnableADC1(true)
 │ 
 │  /* 配置 ADC1 控制寄存器 1 和 2 */
 ├──> ADC1->CR1 = 0
 ├──> ADC1->CR2 = ADC_CR2_ADON
 │ 
 │  /* 复位校准 */
 ├──> ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_RSTCAL
 ├──> while ((ADC1->CR2 & ADC_CR2_RSTCAL) != 0)
 │   └──> 等待校准复位完成
 │ 
 │  /* 开始校准 */
 ├──> ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_CAL
 ├──> while ((ADC1->CR2 & ADC_CR2_CAL) != 0)
 │   └──> 等待校准完成
 │ 
 │  /* 设置通道 4 和 5 的采样时间为 28.5us */
 ├──> ADC1->SMPR2 = ADC_SMPR2_SMP_AN4(ADC_SAMPLE_28P5) | ADC_SMPR2_SMP_AN5(ADC_SAMPLE_28P5)
 │ 
 │  /* 配置序列寄存器以每次采集一个样本 */
 ├──> ADC1->SQR1 = 0
 ├──> ADC1->SQR2 = 0
 │ 
 │  /* 启用 ADC1 的转换结束中断 */
 ├──> ADC1->CR1 |= ADC_CR1_EOCIE
 │ 
 │  /* 启用 NVIC 中对应 ADC1 的中断向量 */
 └──> nvicEnableVector(STM32_ADC1_NUMBER, STM32_ADC_ADC1_IRQ_PRIORITY)

4.3.3 RC输入初始化

AP_IOMCU_FW::rcin_serial_init()
 │ 
 │  /* 启动串口 SD1 和 SD3 */
 ├──> sdStart(&SD1, &dsm_cfg)
 ├──> sdStart(&SD3, &sbus_cfg)
 │ 
 │  /* 注册 SD3 事件源 */
 ├──> chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
 │                                 &sd3_listener,
 │                                 EVENT_MASK(1),
 │                                 SD_PARITY_ERROR)
 │ 
 │  /* SBUS 和 FPORT 协议禁用输入脉冲 */
 ├──> #if AP_RCPROTOCOL_SBUS_ENABLED
 │   └──> AP::RC().disable_for_pulses(AP_RCProtocol::SBUS)
 ├──> #if AP_RCPROTOCOL_SBUS_NI_ENABLED
 │   └──> AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI)
 └──> #if AP_RCPROTOCOL_FPORT_ENABLED
     └──> AP::RC().disable_for_pulses(AP_RCProtocol::FPORT)

4.4 动态流程

loop
 └──> AP_IOMCU_FW::update

AP_IOMCU_FW::update()
 │ 
 │  /* 等待事件 */
 ├──> eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END | IOEVENT_TX_BEGIN, TIME_US2I(1000))
 │ 
 │  /* 调试（可选） */
 ├──> #ifdef HAL_GPIO_LINE_GPIO107
 │   └──> TOGGLE_PIN_DEBUG(107)
 │ 
 │  /* 更新总计时和事件计数 */
 ├──> iomcu.reg_status.total_ticks++
 ├──> if (mask)
 │   └──> iomcu.reg_status.total_events++
 │ 
 │  /* DMA 共享处理（可选） */
 ├──> #if AP_HAL_SHARED_DMA_ENABLED
 │   ├──> if ((mask & IOEVENT_TX_BEGIN) && !(mask & IOEVENT_TX_END))
 │   │   └──> tx_dma_handle->lock()
 │   ├──> else if (!(mask & IOEVENT_TX_BEGIN) && (mask & IOEVENT_TX_END))
 │   │   └──> tx_dma_handle->unlock()
 │   └──> if (mask & IOEVENT_TX_BEGIN)
 │       ├──> chSysLock()
 │       ├──> setup_tx_dma(&UARTD2)
 │       └──> chSysUnlock()
 │ 
 │  /* 时间戳更新 */
 ├──> last_ms = AP_HAL::millis()
 ├──> loop_counter++
 │ 
 │  /* 检查重启条件 */
 ├──> if (do_reboot && (last_ms > reboot_time))
 │   └──> hal.scheduler->reboot(true)
 │ 
 │  /* PWM 输出更新 */
 ├──> if ((mask & IOEVENT_PWM) || (last_safety_off != reg_status.flag_safety_off))
 │   ├──> last_safety_off = reg_status.flag_safety_off
 │   └──> pwm_out_update()
 │ 
 │  /* 时间戳和事件处理 */
 ├──> uint32_t now = last_ms
 ├──> uint32_t now_us = AP_HAL::micros()
 ├──> reg_status.timestamp_ms = last_ms
 ├──> if (条件) 输出新的 SBUS 帧
 ├──> if (条件) 处理 FMU 故障保护
 ├──> if (条件) 更新状态页
 ├──> if (条件) 更新遥测数据
 │ 
 │  /* 快速循环函数 1Khz */
 ├──> if (now_us - last_fast_loop_us >= 1000)
 │   ├──> last_fast_loop_us = now_us
 │   ├──> heater_update()
 │   ├──> rcin_update()
 │   ├──> rcin_serial_update()
 │   └──> #ifdef HAL_WITH_BIDIR_DSHOT
 │       └──> erpm_update()
 │ 
 │  /* 慢速循环函数 100Hz */
 ├──> if (now - last_slow_loop_ms > 10)
 │   ├──> last_slow_loop_ms = now
 │   ├──> safety_update()
 │   ├──> rcout_config_update()
 │   ├──> hal.rcout->timer_tick()
 │   ├──> if (dsm_bind_state)
 │   │   └──> dsm_bind_step()
 │   ├──> GPIO_write()
 │   └──> #if CH_DBG_ENABLE_STACK_CHECK == TRUE
 │       └──> stackCheck(reg_status.freemstack, reg_status.freepstack)
 │ 
 │  /* DMA 共享处理（可选） */
 ├──> #if AP_HAL_SHARED_DMA_ENABLED
 │   └──> mask = chEvtGetAndClearEvents(IOEVENT_TX_END)
 │       └──> if (mask)
 │           └──> tx_dma_handle->unlock()
 │ 
 │  /* 调试（可选） */
 └──> #ifdef HAL_GPIO_LINE_GPIO107
     └──> TOGGLE_PIN_DEBUG(107)

4.4.1 快速循环

快速循环函数 1Khz

4.4.1.1 heater_update

AP_IOMCU_FW::heater_update()
|
+-- 获取当前时间
|   |
|   +-- uint32_t now = last_ms
|
+-- 判断是否有加热器
    |
    +-- if (!has_heater)
        |
        +-- 蓝色LED作为心跳指示器
        |   |
        |   +-- if (now - last_blue_led_ms > (override_active?125:500))
        |       |
        |       +-- BLUE_TOGGLE()
        |       +-- last_blue_led_ms = now
        |
        +-- else if (reg_setup.heater_duty_cycle == 0 || (now - last_heater_ms > 3000UL))
        |   |
        |   +-- 关闭加热器
        |   |   |
        |   |   +-- HEATER_SET(!heater_pwm_polarity)
        |
        +-- else
            |
            +-- 使用伪随机序列来抖动加热器循环
                |
                +-- bool heater_on = (get_random16() < uint32_t(reg_setup.heater_duty_cycle) * 0xFFFFU / 100U)
                +-- 设置加热器状态
                    |
                    +-- HEATER_SET(heater_on ? heater_pwm_polarity : !heater_pwm_polarity)

4.4.1.2 rcin_update

AP_IOMCU_FW::rcin_update()
|
+-- 定时器更新
|   |
|   +-- ((ChibiOS::RCInput *)hal.rcin)->_timer_tick()
|
+-- 检查新的RC输入
|   |
|   +-- if (hal.rcin->new_input())
|       |
|       +-- 获取RC信息
|       |   |
|       |   +-- const auto &rc = AP::RC()
|       |   +-- rc_input.count = hal.rcin->num_channels()
|       |   +-- rc_input.flags_rc_ok = true
|       |   +-- hal.rcin->read(rc_input.pwm, IOMCU_MAX_RC_CHANNELS)
|       |   +-- rc_last_input_ms = last_ms
|       |   +-- rc_input.rc_protocol = (uint16_t)rc.protocol_detected()
|       |   +-- rc_input.rssi = rc.get_RSSI()
|       |   +-- rc_input.flags_failsafe = rc.failsafe_active()
|       |
|       +-- else if (last_ms - rc_last_input_ms > 200U)
|           |
|           +-- rc_input.flags_rc_ok = false
|
+-- 检查是否需要更新输出频率
|   |
|   +-- if (update_rcout_freq)
|       |
|       +-- hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate)
|       +-- update_rcout_freq = false
|
+-- 检查是否需要更新默认频率
|   |
|   +-- if (update_default_rate)
|       |
|       +-- hal.rcout->set_default_rate(reg_setup.pwm_defaultrate)
|       +-- update_default_rate = false
|
+-- 检查覆盖通道
|   |
|   +-- bool old_override = override_active
|   +-- if (mixing.enabled &&
|       |    mixing.rc_chan_override > 0 &&
|       |    rc_input.flags_rc_ok &&
|       |    mixing.rc_chan_override <= IOMCU_MAX_RC_CHANNELS)
|       |   |
|       |   +-- override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1750)
|       |
|       +-- else
|           |
|           +-- override_active = false
|
+-- 处理覆盖通道变化
    |
    +-- if (old_override != override_active)
        |
        +-- if (override_active)
        |   |
        |   +-- fill_failsafe_pwm()
        |
        +-- chEvtSignal(thread_ctx, IOEVENT_PWM)

4.4.1.3 rcin_serial_update

AP_IOMCU_FW::rcin_serial_update()
|
+-- 变量声明和初始化
|   |
|   +-- uint8_t b[16]
|   +-- uint32_t n
|   +-- uint32_t now = AP_HAL::millis()
|   +-- auto &rc = AP::RC()
|
+-- 检查是否需要搜索RC信号
|   |
|   +-- if (rc.should_search(now))
|       |
|       +-- rc_state = RC_SEARCHING
|
+-- 从DSM端口读取
|   |
|   +-- if ((rc_state == RC_SEARCHING || rc_state == RC_DSM_PORT) &&
|           (n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0)
|       |
|       +-- n = MIN(n, sizeof(b))
|       +-- if (sd3_config == 0)
|           |
|           +-- rc_stats.num_dsm_bytes += n
|           +-- for (uint8_t i=0; i<n; i++)
|               |
|               +-- if (rc.process_byte(b[i], 115200))
|                   |
|                   +-- rc_stats.last_good_ms = now
|                   +-- if (!rc.should_search(now))
|                       |
|                       +-- rc_state = RC_DSM_PORT
|           +-- //BLUE_TOGGLE()
|
+-- 从SBUS端口读取
|   |
|   +-- if ((rc_state == RC_SEARCHING || rc_state == RC_SBUS_PORT) &&
|           (n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0)
|       |
|       +-- eventflags_t flags
|       +-- if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR))
|           |
|           +-- rc_stats.sbus_error = flags
|           +-- rc_stats.num_sbus_errors++
|       +-- else
|           |
|           +-- n = MIN(n, sizeof(b))
|           +-- rc_stats.num_sbus_bytes += n
|           +-- for (uint8_t i=0; i<n; i++)
|               |
|               +-- if (rc.process_byte(b[i], sd3_config==0?100000:115200))
|                   |
|                   +-- rc_stats.last_good_ms = now
|                   +-- if (!rc.should_search(now))
|                       |
|                       +-- rc_state = RC_SBUS_PORT
|
+-- 处理SBUS输出
    |
    +-- const bool sbus_out_enabled = (reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) != 0
    +-- if (rc_state == RC_SEARCHING &&
            now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled))
        |
        +-- rc_stats.last_good_ms = now
        +-- sd3_config ^= 1
        +-- sdStop(&SD3)
        +-- sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg)

4.4.1.4 erpm_update

AP_IOMCU_FW::erpm_update()
|
+-- 变量声明和初始化
|   |
|   +-- uint32_t now_us = AP_HAL::micros()
|
+-- 检查是否有新的eRPM数据
    |
    +-- if (hal.rcout->new_erpm())
        |
        +-- dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_TELEM_CHANNELS)
        +-- last_erpm_us = now_us
    |
    +-- else if (now_us - last_erpm_us > ESC_RPM_DATA_TIMEOUT_US)
        |
        +-- dshot_erpm.update_mask = 0

4.4.2 慢速循环

慢速循环函数 100Hz

4.4.2.1 safety_update

RCOutput::safety_update()
|
+-- 获取当前时间
|   |
|   +-- uint32_t now = AP_HAL::millis()
|
+-- 检查是否需要更新安全状态
|   |
|   +-- if (now - safety_update_ms < 100)
|       |
|       +-- return
|   |
|   +-- safety_update_ms = now
|
+-- 获取 BoardConfig 的单例
|   |
|   +-- AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton()
|   |
|   +-- if (boardconfig)
|       |
|       +-- safety_mask = boardconfig->get_safety_mask()
|
+-- 处理安全按钮
|   |
|   +-- #ifdef HAL_GPIO_PIN_SAFETY_IN
|       |
|       +-- bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_IN)
|       |
|       +-- if (safety_pressed)
|           |
|           +-- AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton()
|           +-- if (safety_press_count < 255)
|               |
|               +-- safety_press_count++
|           +-- if (brdconfig && brdconfig->safety_button_handle_pressed(safety_press_count))
|               |
|               +-- if (safety_state == AP_HAL::Util::SAFETY_ARMED)
|                   |
|                   +-- safety_state = AP_HAL::Util::SAFETY_DISARMED
|               +-- else
|                   |
|                   +-- safety_state = AP_HAL::Util::SAFETY_ARMED
|       |
|       +-- else
|           |
|           +-- safety_press_count = 0
|   |
|   +-- #elif HAL_WITH_IO_MCU
|       |
|       +-- safety_state = _safety_switch_state()
|
+-- 设置 IOMCU 安全掩码
|   |
|   +-- #if HAL_WITH_IO_MCU
|       |
|       +-- iomcu.set_safety_mask(safety_mask)
|
+-- 更新安全 LED 状态
    |
    +-- #ifdef HAL_GPIO_PIN_LED_SAFETY
        |
        +-- led_counter = (led_counter+1) % 16
        +-- const uint16_t led_pattern = safety_state==AP_HAL::Util::SAFETY_DISARMED?0x5500:0xFFFF
        +-- palWriteLine(HAL_GPIO_PIN_LED_SAFETY, (led_pattern & (1U << led_counter))?0:1)

4.4.2.2 rcout_config_update

AP_IOMCU_FW::rcout_config_update()
|
+-- 检查 GPIO 通道掩码是否更改
|   |
|   +-- if (GPIO.channel_mask != last_GPIO_channel_mask)
|       |
|       +-- for (uint8_t i=0; i<8; i++)
|           |
|           +-- if ((GPIO.channel_mask & (1U << i)) != 0)
|           |   |
|           |   +-- hal.rcout->disable_ch(i)
|           |   +-- hal.gpio->pinMode(101+i, HAL_GPIO_OUTPUT)
|           |
|           +-- else
|               |
|               +-- hal.rcout->enable_ch(i)
|       |
|       +-- last_GPIO_channel_mask = GPIO.channel_mask
|
+-- 检查通道掩码是否更改
|   |
|   +-- if (last_channel_mask != reg_setup.channel_mask)
|       |
|       +-- for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++)
|           |
|           +-- if (reg_setup.channel_mask & 1U << i)
|           |   |
|           |   +-- hal.rcout->enable_ch(i)
|           |
|           +-- else
|               |
|               +-- hal.rcout->disable_ch(i)
|       |
|       +-- last_channel_mask = reg_setup.channel_mask
|       |
|       +-- uint32_t output_mask = 0
|       +-- reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask)
|       +-- reg_status.rcout_mask = uint8_t(0xFF & output_mask)
|
+-- 检查输出模式是否更改
|   |
|   +-- if ((last_output_mode_mask & mode_out.mask) == mode_out.mask
|   |   && (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask
|   |   && last_output_esc_type == mode_out.esc_type)
|   |   |
|   |   +-- return
|
+-- 根据模式设置输出
|   |
|   +-- switch (mode_out.mode)
|       |
|       +-- case AP_HAL::RCOutput::MODE_PWM_DSHOT150:
|       |   case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
|       |   #if defined(STM32F103xB) || defined(STM32F103x8)
|       |   case AP_HAL::RCOutput::MODE_PWM_DSHOT600:
|       |   #endif
|       |   #ifdef HAL_WITH_BIDIR_DSHOT
|       |       |
|       |       +-- hal.rcout->set_bidir_dshot_mask(mode_out.bdmask)
|       |       +-- hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type))
|       |   #endif
|       |   |
|       |   +-- hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode)
|       |   +-- reg_status.freemem = hal.util->available_memory()
|       |   +-- last_output_mode_mask |= mode_out.mask
|       |   +-- last_output_bdmask |= mode_out.bdmask
|       |   +-- last_output_esc_type = mode_out.esc_type
|       |
|       +-- case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
|       |   case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:
|       |   |
|       |   +-- hal.rcout->set_freq(mode_out.mask, 1)
|       |   +-- hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode)
|       |   +-- last_output_mode_mask |= mode_out.mask
|       |
|       +-- case AP_HAL::RCOutput::MODE_PWM_BRUSHED:
|       |   |
|       |   +-- hal.rcout->set_freq(mode_out.mask, 2000)
|       |   +-- hal.rcout->set_esc_scaling(1000, 2000)
|       |   +-- hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED)
|       |   +-- hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate)
|       |   +-- last_output_mode_mask |= mode_out.mask
|       |
|       +-- default
|           |
|           +-- break
|
+-- 更新输出模式和掩码
    |
    +-- uint32_t output_mask = 0
    +-- reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask)
    +-- reg_status.rcout_mask = uint8_t(0xFF & output_mask)

4.4.2.3 dsm_bind_step

AP_IOMCU_FW::dsm_bind_step()
|
+-- 获取当前时间 now = last_ms
|
+-- switch (dsm_bind_state)
    |
    +-- case 1:
    |   |
    |   +-- palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL)
    |   +-- SPEKTRUM_POWER(0)
    |   +-- palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL)
    |   +-- SPEKTRUM_SET(1)
    |   +-- dsm_bind_state = 2
    |   +-- last_dsm_bind_ms = now
    |
    +-- case 2:
    |   |
    |   +-- if (now - last_dsm_bind_ms >= 500)
    |       |
    |       +-- SPEKTRUM_POWER(1)
    |       +-- dsm_bind_state = 3
    |       +-- last_dsm_bind_ms = now
    |
    +-- case 3:
    |   |
    |   +-- if (now - last_dsm_bind_ms >= 72)
    |       |
    |       +-- delay_one_ms(now)
    |       +-- const uint8_t num_pulses = 9
    |       +-- for (uint8_t i=0; i<num_pulses; i++)
    |       |   |
    |       |   +-- delay_one_ms(now)
    |       |   +-- SPEKTRUM_SET(0)
    |       |   +-- delay_one_ms(now)
    |       |   +-- SPEKTRUM_SET(1)
    |       |
    |       +-- last_dsm_bind_ms = now
    |       +-- dsm_bind_state = 4
    |
    +-- case 4:
    |   |
    |   +-- if (now - last_dsm_bind_ms >= 50)
    |       |
    |       +-- palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_INPUT)
    |       +-- dsm_bind_state = 0
    |
    +-- default:
        |
        +-- dsm_bind_state = 0

4.4.2.4 GPIO_write

void AP_IOMCU_FW::GPIO_write()
{
    for (uint8_t i=0; i<8; i++) {
        if ((GPIO.channel_mask & (1U << i)) != 0) {
            hal.gpio->write(101+i, (GPIO.output_mask & (1U << i)) != 0);
        }
    }
}

4.4.2.5 stackCheck

stackCheck(uint16_t& mstack, uint16_t& pstack)
|
+-- extern stkalign_t __main_stack_base__[]        // Declare external symbol for main stack base
+-- extern stkalign_t __main_stack_end__[]          // Declare external symbol for main stack end
|
+-- uint32_t stklimit = (uint32_t)__main_stack_end__  // Set stack limit to main stack end address
+-- uint32_t stkbase  = (uint32_t)__main_stack_base__ // Set stack base to main stack base address
+-- uint32_t *crawl   = (uint32_t *)stkbase           // Initialize crawl pointer to stack base
|
+-- while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit)
|   |
|   +-- crawl++                                      // Traverse stack, checking for 0x55555555 pattern
|
+-- uint32_t free = (uint32_t)crawl - stkbase        // Calculate free stack space
+-- chDbgAssert(free > 0, "mstack exhausted")       // Assert that free space is positive
+-- mstack = (uint16_t)free                          // Store free space in mstack
|
+-- extern stkalign_t __main_thread_stack_base__[]   // Declare external symbol for main thread stack base
+-- extern stkalign_t __main_thread_stack_end__[]     // Declare external symbol for main thread stack end
|
+-- stklimit = (uint32_t)__main_thread_stack_end__   // Set stack limit to main thread stack end address
+-- stkbase  = (uint32_t)__main_thread_stack_base__  // Set stack base to main thread stack base address
+-- crawl   = (uint32_t *)stkbase                    // Initialize crawl pointer to stack base
|
+-- while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit)
|   |
|   +-- crawl++                                      // Traverse stack, checking for 0x55555555 pattern
|
+-- free = (uint32_t)crawl - stkbase                // Calculate free stack space
+-- chDbgAssert(free > 0, "pstack exhausted")       // Assert that free space is positive
+-- pstack = (uint16_t)free                          // Store free space in pstack

1. 外部变量声明:

   • extern stkalign_t __main_stack_base__[]：声明主堆栈基地址的外部符号。
   • extern stkalign_t __main_stack_end__[]：声明主堆栈结束地址的外部符号。

2. 初始化主堆栈相关变量:

   • uint32_t stklimit = (uint32_t)__main_stack_end__：设置堆栈限制为主堆栈结束地址。
   • uint32_t stkbase = (uint32_t)__main_stack_base__：设置堆栈基地址为主堆栈基地址。
   • uint32_t *crawl = (uint32_t *)stkbase：初始化 crawl 指针为堆栈基地址。

3. 遍历主堆栈:

   • while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit)：遍历堆栈，查找不等于 0x55555555 的值。
     • crawl++：移动到下一个堆栈位置。

4. 计算主堆栈的空闲空间:

   • uint32_t free = (uint32_t)crawl - stkbase：计算空闲的堆栈空间。
   • chDbgAssert(free > 0, "mstack exhausted")：检查空闲空间是否大于 0，若不大于 0，触发断言。
   • mstack = (uint16_t)free：将空闲空间存储在 mstack 中。

5. 外部变量声明（线程堆栈）:

   • extern stkalign_t __main_thread_stack_base__[]：声明线程堆栈基地址的外部符号。
   • extern stkalign_t __main_thread_stack_end__[]：声明线程堆栈结束地址的外部符号。

6. 初始化线程堆栈相关变量:

   • stklimit = (uint32_t)__main_thread_stack_end__：设置堆栈限制为线程堆栈结束地址。
   • stkbase = (uint32_t)__main_thread_stack_base__：设置堆栈基地址为线程堆栈基地址。
   • crawl = (uint32_t *)stkbase：初始化 crawl 指针为线程堆栈基地址。

7. 遍历线程堆栈:

   • while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit)：遍历堆栈，查找不等于 0x55555555 的值。
     • crawl++：移动到下一个堆栈位置。

8. 计算线程堆栈的空闲空间:

   • free = (uint32_t)crawl - stkbase：计算空闲的堆栈空间。
   • chDbgAssert(free > 0, "pstack exhausted")：检查空闲空间是否大于 0，若不大于 0，触发断言。
   • pstack = (uint16_t)free：将空闲空间存储在 pstack 中。

4.5 工程编译

$ ./waf configure --board iomcu-f103-dshot
$ ./waf clean
$ ./waf build iofirmware

或者 Tools/scripts/build_iofirmware.py编译所有的IOMCU 固件：

$ Tools/scripts/build_iofirmware.py

5. 参考资料

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