diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 69f545fc5a..15f40203ec 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL &hal; @@ -32,12 +34,19 @@ enum ioevents { IOEVENT_SET_HEATER_TARGET, IOEVENT_SET_DEFAULT_RATE, IOEVENT_SET_SAFETY_MASK, + IOEVENT_MIXING }; AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) : uart(_uart) {} +#if 0 +#define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define debug(fmt, args ...) +#endif + /* initialise library, starting thread */ @@ -54,12 +63,15 @@ void AP_IOMCU::init(void) AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance(); if (!boardconfig || boardconfig->io_enabled() == 1) { check_crc(); + } else { + crc_is_ok = true; } 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"); } + initialised = true; } /* @@ -95,6 +107,13 @@ void AP_IOMCU::thread_main(void) } if (mask & EVENT_MASK(IOEVENT_INIT)) { + // get protocol version + if (!read_registers(PAGE_CONFIG, PAGE_CONFIG_PROTOCOL_VERSION, sizeof(config)/2, (uint16_t *)&config)) { + event_failed(IOEVENT_INIT); + continue; + } + debug("io protocol: %u\n", config.protocol_version); + // set IO_ARM_OK and FMU_ARMED if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0, P_SETUP_ARMING_IO_ARM_OK | @@ -105,7 +124,13 @@ void AP_IOMCU::thread_main(void) } } - + if (mask & EVENT_MASK(IOEVENT_MIXING)) { + if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) { + event_failed(IOEVENT_MIXING); + 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); @@ -165,7 +190,7 @@ void AP_IOMCU::thread_main(void) continue; } } - + // check for regular timed events uint32_t now = AP_HAL::millis(); if (now - last_rc_read_ms > 20) { @@ -232,6 +257,8 @@ void AP_IOMCU::send_servo_out() uint8_t n = pwm_out.num_channels; if (rate.sbus_rate_hz == 0) { n = MIN(n, 8); + } else { + n = MIN(n, IOMCU_MAX_CHANNELS); } uint32_t now = AP_HAL::micros(); if (now - last_servo_out_us >= 2000) { @@ -313,6 +340,15 @@ void AP_IOMCU::discard_input(void) */ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs) { + while (count > PKT_MAX_REGS) { + if (!read_registers(page, offset, PKT_MAX_REGS, regs)) { + return false; + } + offset += PKT_MAX_REGS; + count -= PKT_MAX_REGS; + regs += PKT_MAX_REGS; + } + IOPacket pkt; discard_input(); @@ -332,6 +368,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 */ pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) { + protocol_fail_count++; return false; } @@ -352,21 +389,25 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 pkt.crc = 0; uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); if (got_crc != expected_crc) { - hal.console->printf("bad crc %02x should be %02x n=%u %u/%u/%u\n", - got_crc, expected_crc, - n, page, offset, count); + debug("bad crc %02x should be %02x n=%u %u/%u/%u\n", + got_crc, expected_crc, + n, page, offset, count); + protocol_fail_count++; return false; } if (pkt.code != CODE_SUCCESS) { - hal.console->printf("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count); + debug("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count); + protocol_fail_count++; return false; } if (pkt.count < count) { - hal.console->printf("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n); + debug("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n); + protocol_fail_count++; return false; } memcpy(regs, pkt.regs, count*2); + protocol_fail_count = 0; return true; } @@ -375,6 +416,14 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 */ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs) { + while (count > PKT_MAX_REGS) { + if (!write_registers(page, offset, PKT_MAX_REGS, regs)) { + return false; + } + offset += PKT_MAX_REGS; + count -= PKT_MAX_REGS; + regs += PKT_MAX_REGS; + } IOPacket pkt; discard_input(); @@ -389,12 +438,14 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons memcpy(pkt.regs, regs, 2*count); pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); if (uart.write((uint8_t *)&pkt, pkt.get_size()) != pkt.get_size()) { + protocol_fail_count++; return false; } // wait for the expected number of reply bytes or timeout if (!uart.wait_timeout(4, 10)) { - //hal.console->printf("no reply for %u/%u/%u\n", page, offset, count); + //debug("no reply for %u/%u/%u\n", page, offset, count); + protocol_fail_count++; return false; } @@ -407,18 +458,21 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons } if (pkt.code != CODE_SUCCESS) { - hal.console->printf("bad code %02x write %u/%u/%u %02x/%02x n=%u\n", - pkt.code, page, offset, count, - pkt.page, pkt.offset, n); + debug("bad code %02x write %u/%u/%u %02x/%02x n=%u\n", + pkt.code, page, offset, count, + pkt.page, pkt.offset, n); + protocol_fail_count++; return false; } uint8_t got_crc = pkt.crc; pkt.crc = 0; uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size()); if (got_crc != expected_crc) { - hal.console->printf("bad crc %02x should be %02x\n", got_crc, expected_crc); + debug("bad crc %02x should be %02x\n", got_crc, expected_crc); + protocol_fail_count++; return false; } + protocol_fail_count = 0; return true; } @@ -713,8 +767,7 @@ void AP_IOMCU::set_safety_mask(uint16_t chmask) */ bool AP_IOMCU::healthy(void) { - // for now just check CRC - return crc_is_ok; + return crc_is_ok && protocol_fail_count == 0; } /* @@ -728,4 +781,53 @@ void AP_IOMCU::shutdown(void) } } +/* + setup for mixing. This allows fixed wing aircraft to fly in manual + mode if the FMU dies + */ +bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan) +{ + if (config.protocol_version != IOMCU_PROTOCOL_VERSION) { + return false; + } + bool changed = false; +#define MIX_UPDATE(a,b) do { if ((a) != (b)) { a = b; changed = true; }} while (0) + + // update mixing structure, checking for changes + for (uint8_t i=0; iget_trim()); + MIX_UPDATE(mixing.servo_min[i], ch->get_output_min()); + MIX_UPDATE(mixing.servo_max[i], ch->get_output_max()); + MIX_UPDATE(mixing.servo_function[i], ch->get_function()); + } + // update RCMap + MIX_UPDATE(mixing.rc_channel[0], rcmap->roll()); + MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch()); + MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle()); + MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw()); + for (uint8_t i=0; i<4; i++) { + if (mixing.rc_channel[i] <= 0 || mixing.rc_channel[i] >= NUM_RC_CHANNELS) { + continue; + } + const RC_Channel *ch = RC_Channels::rc_channel(mixing.rc_channel[i]-1); + if (!ch) { + continue; + } + MIX_UPDATE(mixing.rc_min[i], ch->get_radio_min()); + MIX_UPDATE(mixing.rc_max[i], ch->get_radio_max()); + MIX_UPDATE(mixing.rc_trim[i], ch->get_radio_trim()); + } + MIX_UPDATE(mixing.rc_chan_override, override_chan); + // and enable + MIX_UPDATE(mixing.enabled, 1); + if (changed) { + trigger_event(IOEVENT_MIXING); + } + return true; +} + #endif // HAL_WITH_IO_MCU diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index acbcc22406..d3f64d0c05 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -11,6 +11,7 @@ #include "ch.h" #include "iofirmware/ioprotocol.h" +#include class AP_IOMCU { public: @@ -89,6 +90,9 @@ public: // shutdown IO protocol (for reboot) void shutdown(); + // setup for FMU failsafe mixing + bool setup_mixing(RCMapper *rcmap, int8_t override_chan); + private: AP_HAL::UARTDriver &uart; @@ -137,12 +141,18 @@ private: void discard_input(void); void event_failed(uint8_t event); void update_safety_options(void); - + + // CONFIG page + struct page_config config; + // PAGE_STATUS values struct page_reg_status reg_status; // PAGE_RAW_RCIN values struct page_rc_input rc_input; + + // MIXER values + struct page_mixing mixing; // output pwm values struct { @@ -180,6 +190,9 @@ private: bool done_shutdown; bool crc_is_ok; + bool initialised; + + uint32_t protocol_fail_count; // 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 1f354808c4..790b602d8d 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -326,6 +326,9 @@ bool AP_IOMCU_FW::handle_code_read() } while(0); switch (rx_io_packet.page) { + case PAGE_CONFIG: + COPY_PAGE(config); + break; case PAGE_SETUP: COPY_PAGE(reg_setup); break; @@ -461,6 +464,11 @@ bool AP_IOMCU_FW::handle_code_write() chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM)); break; } + case PAGE_MIXING: { + uint8_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; + memcpy(((uint16_t *)&mixing)+offset, &rx_io_packet.regs[0], num_values*2); + break; + } default: break; diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 57e62080ac..2de22cbd08 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -4,7 +4,6 @@ #include "ch.h" #include "ioprotocol.h" -#define IOMCU_MAX_CHANNELS 16 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX #define SERVO_COUNT 8 @@ -56,6 +55,9 @@ private: uint16_t pwm_altclock = 1; } reg_setup; + // CONFIG values + struct page_config config; + // PAGE_STATUS values struct page_reg_status reg_status; @@ -80,6 +82,9 @@ private: uint16_t sbus_rate_hz; } rate; + // MIXER values + struct page_mixing mixing; + // sbus rate handling uint32_t sbus_last_ms; uint32_t sbus_interval_ms; diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 0770d890f4..be12d22457 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -50,6 +50,7 @@ enum iopage { PAGE_DIRECT_PWM = 54, PAGE_FAILSAFE_PWM = 55, PAGE_DISARMED_PWM = 108, + PAGE_MIXING = 200, }; // setup page registers @@ -77,6 +78,10 @@ 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 +// config page registers +#define PAGE_CONFIG_PROTOCOL_VERSION 0 +#define IOMCU_PROTOCOL_VERSION 10 + // magic value for rebooting to bootloader #define REBOOT_BL_MAGIC 14662 @@ -84,6 +89,10 @@ enum iopage { #define PAGE_REG_SETUP_FORCE_SAFETY_ON 14 #define FORCE_SAFETY_MAGIC 22027 +struct PACKED page_config { + uint16_t protocol_version = IOMCU_PROTOCOL_VERSION; +}; + struct PACKED page_reg_status { uint16_t freemem; uint16_t cpuload; @@ -130,3 +139,25 @@ struct PACKED page_rc_input { uint16_t last_frame_count; uint32_t last_input_us; }; + +/* + data for mixing on FMU failsafe + */ +struct PACKED page_mixing { + uint16_t servo_min[IOMCU_MAX_CHANNELS]; + uint16_t servo_max[IOMCU_MAX_CHANNELS]; + uint16_t servo_trim[IOMCU_MAX_CHANNELS]; + uint8_t servo_function[IOMCU_MAX_CHANNELS]; + + // RC input arrays are in AETR order + uint16_t rc_min[4]; + uint16_t rc_max[4]; + uint16_t rc_trim[4]; + uint8_t rc_channel[4]; + + // channel which when high forces mixer + int8_t rc_chan_override; + + // enabled needs to be 1 to enable mixing + uint8_t enabled; +};