diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index d89d7bc2b2..9ee672e1ed 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -19,6 +19,7 @@ #include #include #include +#include extern const AP_HAL::HAL &hal; @@ -247,6 +248,8 @@ void AP_IOMCU::thread_main(void) pwm_out.failsafe_pwm_sent = set; } } + + send_rc_protocols(); } done_shutdown = true; } @@ -769,6 +772,18 @@ void AP_IOMCU::update_safety_options(void) } } +// update enabled RC protocols mask +void AP_IOMCU::send_rc_protocols() +{ + const uint32_t v = rc().enabled_protocols(); + if (last_rc_protocols == v) { + return; + } + if (write_registers(PAGE_SETUP, PAGE_REG_SETUP_RC_PROTOCOLS, 2, (uint16_t *)&v)) { + last_rc_protocols = v; + } +} + /* check ROMFS firmware against CRC on IOMCU, and if incorrect then upload new firmware */ @@ -1023,6 +1038,7 @@ void AP_IOMCU::check_iomcu_reset(void) } trigger_event(IOEVENT_SET_RATES); trigger_event(IOEVENT_SET_DEFAULT_RATE); + last_rc_protocols = 0; } diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 57a922c11c..69f02cce39 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -161,6 +161,7 @@ private: void discard_input(void); void event_failed(uint8_t event); void update_safety_options(void); + void send_rc_protocols(void); // CONFIG page struct page_config config; @@ -223,6 +224,7 @@ private: uint32_t last_iocmu_timestamp_ms; uint32_t read_status_errors; uint32_t read_status_ok; + uint32_t last_rc_protocols; // firmware upload const char *fw_name = "io_firmware.bin"; diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 9b407ace30..228825e7d3 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -558,6 +558,15 @@ bool AP_IOMCU_FW::handle_code_write() } break; + case PAGE_REG_SETUP_RC_PROTOCOLS: { + if (rx_io_packet.count == 2) { + uint32_t v; + memcpy(&v, &rx_io_packet.regs[0], 4); + AP::RC().set_rc_protocols(v); + } + break; + } + default: break; } diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 04d9fcce9e..4532f4369b 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -83,6 +83,7 @@ enum iopage { #define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */ #define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21 #define PAGE_REG_SETUP_DSM_BIND 22 +#define PAGE_REG_SETUP_RC_PROTOCOLS 23 // uses 2 slots, 23 and 24 // config page registers #define PAGE_CONFIG_PROTOCOL_VERSION 0