Ardupilot chibios IO固件,IO与FMU通信,固件下载(3)
目录
文章目录
- 目录
- 摘要
- 1. IO如何编译
- 2. IO如何被FMU烧写固件
- 3. IO代码分析
- 1.重要函数：g_callbacks->setup();
- 1.执行hal.rcin->init()
- 2.执行hal.rcout->init()
- 3.执行 iomcu.init()函数
- 2.重要函数：g_callbacks->loop();
- 1.pwm_out_update()输出更新
- 2.heater_update()更新
- 3.rcin_update()更新
- 4.safety_update()更新
- 5.rcout_mode_update()更新
- 6.hal.rcout->timer_tick()更新
- 4. IO与FMU如何进行通信
- 5. IO如何下载固件
摘要
本节主要记录自己学习：
(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;
}
}
}
}