ardupilot 的IOMCU是怎么回事

目录

文章目录

• 目录
• 摘要
• 1.AP_IOMCU文件有啥用？
• 2.AP_IOMCU文件具体代码
• 3.总结

摘要

本节主要学习AP_IOMCU这个文件夹下的东西是做什么用的，有啥功能？

注：主要实现FMU芯片与IO芯片进行通信，完成通过数据量传输：通过FMU给IO芯片烧写bootloader，完成通过FMU计算需要输出的目标电机PWM值，完成安全开关指示灯，完成SBUS数据解析等。

为啥是这样？可以通过apm的飞控原理图可以看出，主芯片STM32F765IIK6是没有电机输出引脚的（伺服6个通道这里不考虑），废话不多说，看原理图

1.AP_IOMCU文件有啥用？

从文件名可以直接的猜测是烧写固件和建立FMU和IO之间的联系。

2.AP_IOMCU文件具体代码

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();
    //判断是否需要进行IOMCU的固件检测更新
    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");
    }
}
           

备注1:check_crc()函数

bool AP_IOMCU::check_crc(void)
{
    //闪存最小尺寸是4k存储bootloader代码---- flash size minus 4k bootloader
	const uint32_t flash_size = 0x10000 - 0x1000;
	hal.console->printf("fw_name=%s\r\n",fw_name);
	hal.console->printf("fw_size=%d\r\n",fw_size);
    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; i<flash_size-fw_size; i++)
	{
		uint8_t b = 0xff;
		crc = crc_crc32(crc, &b, 1);
	}

    uint32_t io_crc = 0;
    if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc) &&
        io_crc == crc)
    {
        hal.console->printf("IOMCU: CRC ok\n");
        crc_is_ok = true;
        free(fw);
        fw = nullptr;
//        return true; //把这个注销可以看到FMU会给IO进行烧写代码
    }

    const uint16_t magic = REBOOT_BL_MAGIC;
    write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
    hal.console->printf("HHH\r\n");
    if (!upload_fw())
    {
        free(fw);
        fw = nullptr;
        AP_BoardConfig::sensor_config_error("Failed to update IO firmware");
    }
    
    free(fw);
    fw = nullptr;
    return false;
}
           

备注2:upload_fw()函数实现如何烧写代码

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

备注3:thread_main

void AP_IOMCU::thread_main(void)
{
    thread_ctx = chThdGetSelfX();
    chEvtSignal(thread_ctx, initial_event_mask);

    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, MS2ST(10));
//        printf("mask=%d\r\n",mask);
        //检查挂起的IO事件----- check for pending IO events
        if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT))
        {
            send_servo_out();
        }

        if (mask & EVENT_MASK(IOEVENT_INIT))
        {
            //设置IO_ARM_OK 和 FMU_ARMED两个参数------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;
            }
        }
        //设置SBUS
        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)
        {
            //读取20Hz时的伺服输出--- 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;
        }

        //更新安全开关pwm------ update safety pwm
        if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent)
        {
            uint8_t set = pwm_out.safety_pwm_set;
            write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm);            
            pwm_out.safety_pwm_sent = set;
        }
    }
}
           

3.总结

本篇核心不在代码分析，而是重点说明FMU和IO之间的关系，后续会对代码进行细致分析。只有先理清楚他们之间的关系，才能正确分析代码实现逻辑，不然看代码很难看清楚，具体如何实现APP显示通过串口或者USB给飞控升级固件，自己已经研究过，并且实现了。有需要交流的可以联系我。不知道大家还记得我们编译出来的固件名字是ardupilot.bin或者ardupilot.apj这个吗，这里面是包含了io_firmware的代码的，从上面那个打印就可以看出，FMU通过串口完成了IO代码的同步、擦除、烧写、校验。