diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 782834ac65..44f107ebba 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -658,6 +658,7 @@ bool AP_IOMCU::check_crc(void) if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc) && io_crc == crc) { hal.console->printf("IOMCU: CRC ok\n"); + crc_is_ok = true; return true; } @@ -669,4 +670,13 @@ bool AP_IOMCU::check_crc(void) return false; } +/* + check that IO is healthy. This should be used in arming checks + */ +bool AP_IOMCU::healthy(void) +{ + // for now just check CRC + return crc_is_ok; +} + #endif // HAL_WITH_IO_MCU diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index ff3a782964..0ec6658479 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -75,6 +75,9 @@ public: // set to oneshot mode void set_oneshot_mode(void); + // check if IO is healthy + bool healthy(void); + private: AP_HAL::UARTDriver &uart; @@ -195,6 +198,8 @@ private: bool corked; + bool crc_is_ok; + // firmware upload const char *fw_name = "io_firmware.bin"; const uint8_t *fw; diff --git a/libraries/AP_IOMCU/fw_uploader.cpp b/libraries/AP_IOMCU/fw_uploader.cpp index b1dcc8a951..5474cade9d 100644 --- a/libraries/AP_IOMCU/fw_uploader.cpp +++ b/libraries/AP_IOMCU/fw_uploader.cpp @@ -436,6 +436,9 @@ bool AP_IOMCU::verify_rev3(uint32_t fw_size_local) debug("CRC wrong: received: 0x%x, expected: 0x%x", (unsigned)crc, (unsigned)sum); return false; } + + crc_is_ok = true; + return true; }