Ardupilot chibios IO固件,IO与FMU通信,固件下载(3)

目录

摘要

---

本节主要记录自己学习：
(1)IO如何编译
(2)IO如何被FMU烧写固件
(3)IO代码运行分析；
(4)IO与FMU通信;
(5)FMU下载IO固件代码分析;

---

ardupilot最新固件采用chibios操作系统，最新飞控硬件：pixhawk_v5控制器：STM32F765IIT6和STM32F103C8T6, 其中F7也被称作FMU主处理器,F1被称作IO协处理器，F1与F7分别运行自己的Chibios代码，并且FMU与IO通过串口进行通信，共同完成飞行控制任务。

---

---

1. IO如何编译

---

---

编译命令：

./waf configure --board iomcu

生成IO的bin文件iofirmware：

./waf iofirmware

最终生成的固件在：ardupilot/build/iomcu/bin文件夹下

---

---

2. IO如何被FMU烧写固件

---

---

ardupilot最新固件采用chibios操作系统，最新飞控硬件：pixhawk_v5，控制器：STM32F765IIT6和STM32F103C8T6, 其中F7被称作FMU主处理器,F1被称作IO协处理器，F1与F7分别运行自己的Chibios代码，并且FMU与IO通过串口进行通信，共同完成飞行控制任务。

---

打开ardupilot的 ardupilot/Tools/IO_Firmware文件夹，可以看到有一个固件名称fmuv2_IO.bin文件，这个固件就是协处理器的固件，FMU通过串口直接给IO芯片烧写这个固件。跟之前Nuttx那套系统稍有不同，飞控在通过usb下载固件时，实际是先给FMU芯片下载固件的，从硬件也可以看出烧写固件的USB线接口属于FMU的。对FMU固件下载完成后，FMU把放到ardupilot/Tools/IO_Firmware目录下面的IO固件，通过串口8与IO的串口2进行通信，通过串口实现了烧写固件的原理。

---

想要把IO的固件被烧写，需要被第一步编译生成的iofirmware.bin文件放到ardupilot/Tools/IO_Firmware文件夹下面，然后修改文件名字为fmuv2_IO.bin，最后就会被FMU自动调用，完成固件烧写。

---

3. IO代码分析

---

协处理器入口函数，同样也是主处理器入口函数

AP_HAL_MAIN_CALLBACKS(&copter);

#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, CALLBACKS); \
        return 0; \
    } \
    }

定义AP_MAIN=main

#ifndef AP_MAIN
#define AP_MAIN main
#endif

那么我们可以得到

#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
    int main(int argc, char* const argv[]); \
    int main(int argc, char* const argv[]) { \
        hal.run(argc, argv, CALLBACKS); \  //注意这个hal.run函数
        return 0; \
    } \
    }

协处理器现在也跑这个函数

void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{

    assert(callbacks);       //用来让程序测试条件，如果条件正确继续执行，如果条件错误，报错。
    g_callbacks = callbacks; //函数定义传递

    //接管执行main------------Takeover main
    main_loop();
}

static void main_loop()
{
    daemon_task = chThdGetSelfX(); //返回当前线程

    /*
      把main loop的优先级切换到高优先级-----switch to high priority for main loop
     */
    chThdSetPriority(APM_MAIN_PRIORITY); //180

    hal.uartB->begin(38400); 
    hal.uartC->begin(57600);  
    hal.analogin->init();     //模拟输入初始化，主要测试ADC功能，检查电源电压

    hal.scheduler->init();   //初始化任务init线程

    /*
        run setup() at low priority to ensure CLI doesn't hang the system, and to allow initial sensor read loops to run
     */

    //以低优先级运行SETUP（）以确保CLI(命令行界面)不挂起系统，并允许初始传感器读取循环运行。
    hal_chibios_set_priority(APM_STARTUP_PRIORITY); //APM_STARTUP_PRIORITY=10

    schedulerInstance.hal_initialized(); //_hal_initialized = true

    g_callbacks->setup();                 //调用应用层的setup（）函数
    hal.scheduler->system_initialized(); //系统初始化

    thread_running = true;
    chRegSetThreadName(SKETCHNAME);
    
    /*
      main loop切换到低优先级-------switch to high priority for main loop
     */
    chThdSetPriority(APM_MAIN_PRIORITY);

    hal.uartG->printf("UARTG\r\n"); //自己添加打印函数


    while (true)
    {
        g_callbacks->loop();  //调用APP的loop线程

        /*
          give up 250 microseconds of time if the INS loop hasn't
          called delay_microseconds_boost(), to ensure low priority
          drivers get a chance to run. Calling
          delay_microseconds_boost() means we have already given up
          time from the main loop, so we don't need to do it again
          here
          如果INS回路回调Delay-MySudiSsBooSth（）函数没有响应，则放弃250微秒的时间。
         以确保低优先级。有机会运行。回调延迟函数delay_microseconds_boost意味着我们已经放弃了主回路循环，所以我们不需要再做一次。
         */
       // hal.uartG->printf("MMM\r\n"); //自己添加打印函数
        if (!schedulerInstance.check_called_boost())
        {
        	hal.uartG->printf("NNN\r\n"); //自己添加打印函数
            hal.scheduler->delay_microseconds(250);
        }
    }
    thread_running = false;
}

其中hal.scheduler->init();函数运行一部分

void Scheduler::init()
{

    chBSemObjectInit(&_timer_semaphore, false); //定时器二进制信号量，初始状态是0，没有被取用，所以后面的信号量就可以被使用
    chBSemObjectInit(&_io_semaphore, false);    //IO二进制信号量，初始状态是0，没有被取用，，所以后面的信号量就可以被使用
    //设置定时器线程-这将调用任务在1kHz---- setup the timer thread - this will call tasks at 1kHz
    _timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
                     sizeof(_timer_thread_wa),
                     APM_TIMER_PRIORITY,        /* Initial priority.181    */
                     _timer_thread,             /* Thread function.     */
                     this);                     /* Thread parameter.    */

    //设置RCIN线程-这将调用任务在1kHz---- setup the RCIN thread - this will call tasks at 1kHz
    _rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
                     sizeof(_rcin_thread_wa),
                     APM_RCIN_PRIORITY,        /* Initial priority. 177   */
                     _rcin_thread,             /* Thread function.     */
                     this);                     /* Thread parameter.    */
 }

1.重要函数：g_callbacks->setup();

void setup(void)
{
	
    hal.rcin->init();   //初始化rc输入
    hal.rcout->init(); //初始化rc输出，电机

    for (uint8_t i = 0; i< 14; i++) 
    {
        hal.rcout->enable_ch(i);
    }

    iomcu.init();
    
    iomcu.calculate_fw_crc();
    uartStart(&UARTD2, &uart_cfg); //配置串口2，开启串口2的DMA中断
    uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet);//在UART外围设备上启动接收操作
}

---

---

1.执行hal.rcin->init()

---

---

这里用两个宏定义，用来选择执行FMU或者IO的过程

void RCInput::init()
{
#if HAL_USE_ICU == TRUE
    //attach timer channel on which the signal will be received
    sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL);
    rcin_prot.init();
#endif

#if HAL_USE_EICU == TRUE
    sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL);
    rcin_prot.init();
#endif

    _init = true;
}

---

---

2.执行hal.rcout->init()

---

---

void RCOutput::init()
{
    uint8_t pwm_count = AP_BoardConfig::get_pwm_count();
    for (uint8_t i = 0; i < NUM_GROUPS; i++ ) 
    {
        //Start Pwm groups
        pwm_group &group = pwm_group_list[i];
        group.current_mode = MODE_PWM_NORMAL;
        for (uint8_t j = 0; j < 4; j++ ) 
        {
            uint8_t chan = group.chan[j];
            if (chan >= pwm_count) {
                group.chan[j] = CHAN_DISABLED;
            }
            if (group.chan[j] != CHAN_DISABLED) {
                num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1);
                group.ch_mask |= (1U<

3.执行 iomcu.init()函数

void AP_IOMCU_FW::init()
{
    thread_ctx = chThdGetSelfX();

    if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0)
    {
        has_heater = true;
    }
}

---

---

2.重要函数：g_callbacks->loop();

---

---

void loop(void)
{
    iomcu.update();
}

运行协处理器的更新函数

void AP_IOMCU_FW::update()
{
    eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(1));

    if (do_reboot && (AP_HAL::millis() > reboot_time))
    {
        hal.scheduler->reboot(true);
        while (true) {}
    }

    if ((mask & EVENT_MASK(IOEVENT_PWM)) ||
        (last_safety_off != reg_status.flag_safety_off))
    {
        last_safety_off = reg_status.flag_safety_off;
        pwm_out_update();
    }

    // run remaining functions at 1kHz
    uint32_t now = AP_HAL::millis();
    if (now != last_loop_ms)
    {
        last_loop_ms = now;
        heater_update(); //更新心跳包，指示led
        rcin_update();   //遥控器输入更新
        safety_update(); //安全开关更新
        rcout_mode_update(); //电机输出模式更新
        hal.rcout->timer_tick();
    }
}

---

1.pwm_out_update()输出更新

---

void AP_IOMCU_FW::pwm_out_update()
{
    //TODO: PWM mixing
    memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
    hal.rcout->cork();
    for (uint8_t i = 0; i < SERVO_COUNT; i++) 
    {
        if (reg_servo.pwm[i] != 0) 
        {
            hal.rcout->write(i, reg_status.flag_safety_off?reg_servo.pwm[i]:0);
        }
    }
    hal.rcout->push();
}

---

2.heater_update()更新

---

void AP_IOMCU_FW::heater_update()
{
    uint32_t now = AP_HAL::millis();
    if (!has_heater) 
    {
        // use blue LED as heartbeat
        if (now - last_blue_led_ms > 500) 
        {
            palToggleLine(HAL_GPIO_PIN_HEATER);
            last_blue_led_ms = now;
        }
    } else if (reg_setup.heater_duty_cycle == 0 || (now - last_heater_ms > 3000UL)) 
    {
        palWriteLine(HAL_GPIO_PIN_HEATER, 0);
    } else 
    {
        uint8_t cycle = ((now / 10UL) % 100U);
        palWriteLine(HAL_GPIO_PIN_HEATER, !(cycle >= reg_setup.heater_duty_cycle));
    }
}

---

3.rcin_update()更新

---

void AP_IOMCU_FW::rcin_update()
{
    if (hal.rcin->new_input()) 
    {
        rc_input.count = hal.rcin->num_channels();
        rc_input.flags_rc_ok = true;
        for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS; i++) 
        {
            rc_input.pwm[i] = hal.rcin->read(i);
        }
        rc_input.last_input_us = AP_HAL::micros();
    }
    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);
    }

}

---

4.safety_update()更新

---

void AP_IOMCU_FW::safety_update(void)
{
    uint32_t now = AP_HAL::millis();
    if (now - safety_update_ms < 100) 
    {
        // update safety at 10Hz
        return;
    }
    safety_update_ms = now;

    bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
    if (safety_pressed) 
    {
        if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) 
        {
            safety_pressed = false;
        } else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) 
        {
            safety_pressed = false;
        }
    }
    if (safety_pressed) 
    {
        safety_button_counter++;
    } else 
    {
        safety_button_counter = 0;
    }
    if (safety_button_counter == 10) 
    {
        // safety has been pressed for 1 second, change state
        reg_status.flag_safety_off = !reg_status.flag_safety_off;
    }

    led_counter = (led_counter+1) % 16;
    const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
    palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
}

---

5.rcout_mode_update()更新

---

void AP_IOMCU_FW::rcout_mode_update(void)
{
    bool use_oneshot = (reg_setup.features & P_SETUP_FEATURES_ONESHOT) != 0;
    if (use_oneshot && !oneshot_enabled) 
    {
        oneshot_enabled = true;
        hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
    }
    bool use_brushed = (reg_setup.features & P_SETUP_FEATURES_BRUSHED) != 0;
    if (use_brushed && !brushed_enabled) 
    {
        brushed_enabled = true;
        if (reg_setup.pwm_rates == 0) 
        {
            // default to 2kHz for all channels for brushed output
            reg_setup.pwm_rates = 0xFF;
            reg_setup.pwm_altrate = 2000;
            hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
        }
        hal.rcout->set_esc_scaling(1000, 2000);
        hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
        hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
    }
}

---

6.hal.rcout->timer_tick()更新

---

void RCOutput::timer_tick(void)
{
    safety_update(); //更新安全状态
    
    uint64_t now = AP_HAL::micros64();
    for (uint8_t i = 0; i < NUM_GROUPS; i++ )
    {
        pwm_group &group = pwm_group_list[i];
        if (!serial_group &&
            group.current_mode >= MODE_PWM_DSHOT150 &&
            group.current_mode <= MODE_PWM_DSHOT1200 &&
            now - group.last_dshot_send_us > 400) {
            // do a blocking send now, to guarantee DShot sends at
            // above 1000 Hz. This makes the protocol more reliable on
            // long cables, and also keeps some ESCs happy that don't
            // like low rates
        	//现在做一个阻塞发送，保证DShot发送在1000赫兹以上。这使得协议在长电缆上更加可靠，同时也保持一些不喜欢低利率的ESC高兴。
            dshot_send(group, true);
        }
    }
    if (min_pulse_trigger_us == 0 ||
        serial_group != nullptr)
    {
        return;
    }
    if (now > min_pulse_trigger_us &&
        now - min_pulse_trigger_us > 4000)
    {
        //最小250Hz触发器--- trigger at a minimum of 250Hz
        trigger_groups();
    }
}

---

---

4. IO与FMU如何进行通信

---

---

---

5. IO如何下载固件

---

很多人可能疑惑这里，一条USB线如何做到既可以给Fmu又可以给IO下载固件，这里大致说下。

---

从初始化：void Copter::init_ardupilot()

void Copter::init_ardupilot()
{
      //板层初始化，包含gpio,rc,pwm,sbus等
    BoardConfig.init();
}

void AP_BoardConfig::init()

void AP_BoardConfig::init()
{
    board_setup();

#if HAL_HAVE_IMU_HEATER
    // let the HAL know the target temperature. We pass a pointer as
    // we want the user to be able to change the parameter without
    // rebooting
    hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
#endif

    AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW);
}

board_setup();

void AP_BoardConfig::board_setup()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
    px4_setup_peripherals(); //这里配置px4类型的板子
    px4_setup_pwm();
    px4_setup_safety_mask();
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
    // init needs to be done after boardconfig is read so parameters are set
    hal.gpio->init();        //初始化GPIO
    hal.rcin->init();        //初始化rc_In
    hal.rcout->init();       //初始化rc_out
#endif
    board_setup_uart();      //初始化串口
    board_setup_sbus();      //初始化Sbus
#if AP_FEATURE_BOARD_DETECT
    board_setup_drivers();   //配置板层识别
#endif
}

需要注意这个函数hal.rcout->init()，同一个函数可以实现FMU与IO共用; //初始化rc_out

void AP_IOMCU::init(void)
{
    // uart runs at 1.5MBit
    uart.begin(1500*1000, 256, 256);
    uart.set_blocking_writes(false);
    uart.set_unbuffered_writes(true);

    //检查IO固件的CRC-----check IO firmware CRC
    hal.scheduler->delay(2000);
    
    AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
    if (!boardconfig || boardconfig->io_enabled() == 1)
    {
        check_crc(); //检测是否需要跟新固件，也就是FMU要检查IO是否需要更新固件，不需要的话，就不更新
    }

    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");
    }
}

check_crc(); //检测是否需要跟新固件，也就是FMU要检查IO是否需要更新固件，不需要的话，就不更新。并且至少4K空间的大小给Bootloader

bool AP_IOMCU::check_crc(void)
{
    // flash size minus 4k bootloader
	const uint32_t flash_size = 0x10000 - 0x1000;
    
    fw = AP_ROMFS::find_decompress(fw_name, fw_size);
    if (!fw) {
        hal.console->printf("failed to find %s\n", fw_name);
        return false;
    }
    uint32_t crc = crc_crc32(0, fw, fw_size);

    // pad CRC to max size
	for (uint32_t i=0; iprintf("IOMCU: CRC ok\n");
        crc_is_ok = true;
        free(fw);
        fw = nullptr;
        return true;
    } else {
        hal.console->printf("IOMCU: CRC mismatch expected: 0x%X got: 0x%X\n", (unsigned)crc, (unsigned)io_crc);
    }

    const uint16_t magic = REBOOT_BL_MAGIC;
    write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);

    if (!upload_fw()) //更新固件
    {
        free(fw);
        fw = nullptr;
        AP_BoardConfig::sensor_config_error("Failed to update IO firmware");
    }
    
    free(fw);
    fw = nullptr;
    return false;
}

bool AP_IOMCU::upload_fw(void)
{
    // set baudrate for bootloader
    uart.begin(115200, 256, 256);

    bool ret = false;

    /* look for the bootloader for 150 ms */
    for (uint8_t i = 0; i < 15; i++) 
    {
        ret = sync();
        if (ret) 
        {
            break;
        }
        hal.scheduler->delay(10);
    }

    if (!ret) 
    {
        debug("IO update failed sync");
        return false;
    }

    uint32_t bl_rev;
    ret = get_info(INFO_BL_REV, bl_rev);

    if (!ret) 
    {
        debug("Err: failed to contact bootloader");
        return false;
    }
    if (bl_rev > BL_REV) {
        debug("Err: unsupported bootloader revision %u", unsigned(bl_rev));
        return false;
    }
    debug("found bootloader revision: %u", unsigned(bl_rev));

    ret = erase();
    if (!ret) {
        debug("erase failed");
        return false;
    }

    ret = program(fw_size);
    if (!ret) {
        debug("program failed");
        return false;
    }

    if (bl_rev <= 2) {
        ret = verify_rev2(fw_size);
    } else {
        ret = verify_rev3(fw_size);
    }

    if (!ret) {
        debug("verify failed");
        return false;
    }

    ret = reboot();

    if (!ret) {
        debug("reboot failed");
        return false;
    }

    debug("update complete");

    // sleep for enough time for the IO chip to boot
    hal.scheduler->delay(100);

    return true;
}

---

---

创建一个线程电机处理，遥控器处理线程：thread_main

---

---

void AP_IOMCU::thread_main(void)
{
    thread_ctx = chThdGetSelfX();

    uart.begin(1500*1000, 256, 256);
    uart.set_blocking_writes(false);
    uart.set_unbuffered_writes(true);
    
    trigger_event(IOEVENT_INIT);
    
    while (true)
    {
        eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10));

        // check for pending IO events
        if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT))
        {
            send_servo_out(); //发送电机输出
        }

        if (mask & EVENT_MASK(IOEVENT_INIT))
        {
            // set IO_ARM_OK and FMU_ARMED
            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
                                 P_SETUP_ARMING_IO_ARM_OK |
                                 P_SETUP_ARMING_FMU_ARMED |
                                 P_SETUP_ARMING_RC_HANDLING_DISABLED))
            {
                event_failed(IOEVENT_INIT);
                continue;
            }
        }

        
        if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC))
            {
                event_failed(IOEVENT_FORCE_SAFETY_OFF);
                continue;
            }
        }

        if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON))
        {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC))
            {
                event_failed(IOEVENT_FORCE_SAFETY_ON);
                continue;
            }
        }

        
        if (mask & EVENT_MASK(IOEVENT_SET_RATES))
        {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq) ||
                !write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask))
            {
                event_failed(IOEVENT_SET_RATES);
                continue;
            }
        }

        if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS))
        {
            if (!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)) {
                event_failed(IOEVENT_ENABLE_SBUS);
                continue;                
            }
        }

        if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET))
        {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)) {
                event_failed(IOEVENT_SET_HEATER_TARGET);
                continue;
            }
        }

        if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)) {
                event_failed(IOEVENT_SET_DEFAULT_RATE);
                continue;
            }
        }

        if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
                event_failed(IOEVENT_SET_ONESHOT_ON);
                continue;
            }
        }

        if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
                event_failed(IOEVENT_SET_SAFETY_MASK);
                continue;
            }
        }
        
        // check for regular timed events
        uint32_t now = AP_HAL::millis();
        if (now - last_rc_read_ms > 20) {
            // read RC input at 50Hz
            read_rc_input();
            last_rc_read_ms = AP_HAL::millis();
        }
        
        if (now - last_status_read_ms > 50) {
            // read status at 20Hz
            read_status();
            last_status_read_ms = AP_HAL::millis();
        }

        if (now - last_servo_read_ms > 50) {
            // read servo out at 20Hz
            read_servo();
            last_servo_read_ms = AP_HAL::millis();
        }

#ifdef IOMCU_DEBUG
        if (now - last_debug_ms > 1000) {
            print_debug();
            last_debug_ms = AP_HAL::millis();
        }
#endif // IOMCU_DEBUG

        if (now - last_safety_option_check_ms > 1000) {
            update_safety_options();
            last_safety_option_check_ms = now;
        }

        // update safety pwm
        if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
            uint8_t set = pwm_out.safety_pwm_set;
            if (write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
                pwm_out.safety_pwm_sent = set;
            }
        }

        // update failsafe pwm
        if (pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent)
        {
            uint8_t set = pwm_out.failsafe_pwm_set;
            if (write_registers(PAGE_FAILSAFE_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.failsafe_pwm)) {
                pwm_out.failsafe_pwm_sent = set;
            }
        }
    }
}