diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index a7fefc12e01f6..14aee80e732ce 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include extern const AP_HAL::HAL &hal; @@ -109,6 +110,13 @@ void AP_IOMCU::thread_main(void) trigger_event(IOEVENT_INIT); while (!do_shutdown) { + // check if we have lost contact with the IOMCU + const uint32_t now_ms = AP_HAL::millis(); + if (last_reg_read_ms != 0 && now_ms - last_reg_read_ms > 1000U) { + INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset); + last_reg_read_ms = 0; + } + eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10)); // check for pending IO events @@ -322,6 +330,10 @@ void AP_IOMCU::read_status() uint16_t *r = (uint16_t *)®_status; if (!read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r)) { read_status_errors++; + if (read_status_errors == 20 && last_iocmu_timestamp_ms != 0) { + // the IOMCU has stopped responding to status requests + INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset); + } return; } if (read_status_ok == 0) { @@ -538,6 +550,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 total_errors += protocol_fail_count; protocol_fail_count = 0; protocol_count++; + last_reg_read_ms = AP_HAL::millis(); return true; } @@ -785,7 +798,7 @@ void AP_IOMCU::update_safety_options(void) if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) { desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_ON; } - if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && hal.util->get_soft_armed()) { + if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && AP::arming().is_armed()) { desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF); } if (last_safety_options != desired_options) { @@ -851,6 +864,9 @@ bool AP_IOMCU::check_crc(void) const uint16_t magic = REBOOT_BL_MAGIC; write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic); + // avoid internal error on fw upload delay + last_reg_read_ms = 0; + if (!upload_fw()) { AP_ROMFS::free(fw); fw = nullptr; @@ -915,7 +931,7 @@ void AP_IOMCU::shutdown(void) */ void AP_IOMCU::bind_dsm(uint8_t mode) { - if (!is_chibios_backend || hal.util->get_soft_armed()) { + if (!is_chibios_backend || AP::arming().is_armed()) { // only with ChibiOS IO firmware, and disarmed return; } @@ -1021,7 +1037,9 @@ void AP_IOMCU::check_iomcu_reset(void) return; } uint32_t dt_ms = reg_status.timestamp_ms - last_iocmu_timestamp_ms; - uint32_t ts1 = last_iocmu_timestamp_ms; +#if IOMCU_DEBUG_ENABLE + const uint32_t ts1 = last_iocmu_timestamp_ms; +#endif // when we are in an expected delay allow for a larger time // delta. This copes with flash erase, such as bootloader update const uint32_t max_delay = hal.scheduler->in_expected_delay()?5000:500; @@ -1034,19 +1052,23 @@ void AP_IOMCU::check_iomcu_reset(void) } detected_io_reset = true; INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset); - hal.console->printf("IOMCU reset t=%u %u %u dt=%u\n", - unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms)); + debug("IOMCU reset t=%u %u %u dt=%u\n", + unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms)); - if (last_safety_off && !reg_status.flag_safety_off && hal.util->get_soft_armed()) { + bool have_forced_off = false; + if (last_safety_off && !reg_status.flag_safety_off && AP::arming().is_armed()) { AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); uint16_t options = boardconfig?boardconfig->get_safety_button_options():0; if (safety_forced_off || (options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) == 0) { // IOMCU has reset while armed with safety off - force it off // again so we can keep flying + have_forced_off = true; force_safety_off(); } } - last_safety_off = reg_status.flag_safety_off; + if (!have_forced_off) { + last_safety_off = reg_status.flag_safety_off; + } // we need to ensure the mixer data and the rates are sent over to // the IOMCU diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index d792731f9a6a7..8e508ebebb442 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -161,6 +161,7 @@ class AP_IOMCU { uint32_t last_rc_read_ms; uint32_t last_servo_read_ms; uint32_t last_safety_option_check_ms; + uint32_t last_reg_read_ms; // last value of safety options uint16_t last_safety_options = 0xFFFF; diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index ea30c0077af91..e981e01ad4b76 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -41,8 +41,13 @@ void loop(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -// enable testing of IOMCU watchdog using safety switch -#define IOMCU_ENABLE_WATCHDOG_TEST 0 +/* + enable testing of IOMCU reset using safety switch + a value of 0 means normal operation + a value of 1 means test with watchdog + a value of 2 means test with reboot +*/ +#define IOMCU_ENABLE_RESET_TEST 0 // pending events on the main thread enum ioevents { @@ -707,21 +712,40 @@ void AP_IOMCU_FW::safety_update(void) } } -#if IOMCU_ENABLE_WATCHDOG_TEST - if (safety_button_counter == 50) { +#if IOMCU_ENABLE_RESET_TEST + { // deliberate lockup of IOMCU on 5s button press, for testing // watchdog - while (true) { - hal.scheduler->delay(50); - palToggleLine(HAL_GPIO_PIN_SAFETY_LED); - if (palReadLine(HAL_GPIO_PIN_SAFETY_INPUT)) { - // only trigger watchdog on button release, so we - // don't end up stuck in the bootloader - stm32_watchdog_pat(); + static uint32_t safety_test_counter; + static bool should_lockup; + if (palReadLine(HAL_GPIO_PIN_SAFETY_INPUT)) { + safety_test_counter++; + } else { + safety_test_counter = 0; + } + if (safety_test_counter == 50) { + should_lockup = true; + } + // wait for lockup for safety to be released so we don't end + // up in the bootloader + if (should_lockup && palReadLine(HAL_GPIO_PIN_SAFETY_INPUT) == 0) { +#if IOMCU_ENABLE_RESET_TEST == 1 + // lockup with watchdog + while (true) { + hal.scheduler->delay(50); + palToggleLine(HAL_GPIO_PIN_SAFETY_LED); } +#else + // hard fault to simulate power reset or software fault + void *foo = (void*)0xE000ED38; + typedef void (*fptr)(); + fptr gptr = (fptr) (void *) foo; + gptr(); + while (true) {} +#endif } } -#endif +#endif // IOMCU_ENABLE_RESET_TEST led_counter = (led_counter+1) % 16; const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;