From 9d87645c6e61d925eecfec5c3f6028aca0ca1707 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Feb 2021 08:47:24 +1100 Subject: [PATCH] AP_IOMCU: fixed event handling bug in IOMCU code if a a message send fails to IOMCU then we were discarding all currently pending non-recurring events. This means that states like oneshot enable would be lost if the IOMCU reset. We now re-trigger all events that have not yet been handled --- libraries/AP_IOMCU/AP_IOMCU.cpp | 55 ++++++++++++++++++++++++--------- libraries/AP_IOMCU/AP_IOMCU.h | 4 ++- 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index ffbf2d147e..de27451f97 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -85,11 +85,11 @@ void AP_IOMCU::init(void) /* handle event failure */ -void AP_IOMCU::event_failed(uint8_t event) +void AP_IOMCU::event_failed(uint32_t event_mask) { // wait 0.5ms then retry hal.scheduler->delay_microseconds(500); - trigger_event(event); + chEvtSignal(thread_ctx, event_mask); } /* @@ -113,11 +113,12 @@ void AP_IOMCU::thread_main(void) if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) { send_servo_out(); } + mask &= ~EVENT_MASK(IOEVENT_SEND_PWM_OUT); if (mask & EVENT_MASK(IOEVENT_INIT)) { // get protocol version if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) { - event_failed(IOEVENT_INIT); + event_failed(mask); continue; } is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION && @@ -128,84 +129,94 @@ void AP_IOMCU::thread_main(void) P_SETUP_ARMING_IO_ARM_OK | P_SETUP_ARMING_FMU_ARMED | P_SETUP_ARMING_RC_HANDLING_DISABLED)) { - event_failed(IOEVENT_INIT); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_INIT); if (mask & EVENT_MASK(IOEVENT_MIXING)) { if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) { - event_failed(IOEVENT_MIXING); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_MIXING); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF); 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); + event_failed(mask); continue; } } - + mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_ON); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_RATES); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_ENABLE_SBUS); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_HEATER_TARGET); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_DEFAULT_RATE); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_ONESHOT_ON); if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) { if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) { - event_failed(IOEVENT_SET_BRUSHED_ON); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON); 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); + event_failed(mask); continue; } } + mask &= ~EVENT_MASK(IOEVENT_SET_SAFETY_MASK); // check for regular timed events uint32_t now = AP_HAL::millis(); @@ -739,12 +750,14 @@ void AP_IOMCU::set_default_rate(uint16_t rate_hz) void AP_IOMCU::set_oneshot_mode(void) { trigger_event(IOEVENT_SET_ONESHOT_ON); + rate.oneshot_enabled = true; } // setup for brushed mode void AP_IOMCU::set_brushed_mode(void) { trigger_event(IOEVENT_SET_BRUSHED_ON); + rate.brushed_enabled = true; } // handling of BRD_SAFETYOPTION parameter @@ -1045,6 +1058,18 @@ void AP_IOMCU::check_iomcu_reset(void) } trigger_event(IOEVENT_SET_RATES); trigger_event(IOEVENT_SET_DEFAULT_RATE); + if (rate.oneshot_enabled) { + trigger_event(IOEVENT_SET_ONESHOT_ON); + } + if (rate.brushed_enabled) { + trigger_event(IOEVENT_SET_BRUSHED_ON); + } + if (rate.sbus_rate_hz) { + trigger_event(IOEVENT_ENABLE_SBUS); + } + if (pwm_out.safety_mask) { + trigger_event(IOEVENT_SET_SAFETY_MASK); + } last_rc_protocols = 0; } diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 69f02cce39..df604a00eb 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -159,7 +159,7 @@ private: void read_servo(void); void read_status(void); void discard_input(void); - void event_failed(uint8_t event); + void event_failed(uint32_t event_mask); void update_safety_options(void); void send_rc_protocols(void); @@ -201,6 +201,8 @@ private: uint16_t chmask; uint16_t default_freq = 50; uint16_t sbus_rate_hz; + bool oneshot_enabled; + bool brushed_enabled; } rate; // IMU heater duty cycle