mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_IOMCU: fail to boot if IO firmware CRC and update fails
we don't want to fly with a bad IO firmware
This commit is contained in:
parent
677b5f94b2
commit
3242b5eeb9
@ -121,8 +121,6 @@ AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) :
|
|||||||
*/
|
*/
|
||||||
void AP_IOMCU::init(void)
|
void AP_IOMCU::init(void)
|
||||||
{
|
{
|
||||||
//upload_fw(fw_name);
|
|
||||||
|
|
||||||
// uart runs at 1.5MBit
|
// uart runs at 1.5MBit
|
||||||
uart.begin(1500*1000, 256, 256);
|
uart.begin(1500*1000, 256, 256);
|
||||||
uart.set_blocking_writes(false);
|
uart.set_blocking_writes(false);
|
||||||
@ -131,7 +129,10 @@ void AP_IOMCU::init(void)
|
|||||||
// check IO firmware CRC
|
// check IO firmware CRC
|
||||||
hal.scheduler->delay(2000);
|
hal.scheduler->delay(2000);
|
||||||
|
|
||||||
check_crc();
|
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
|
||||||
|
if (!boardconfig || boardconfig->io_enabled() == 1) {
|
||||||
|
check_crc();
|
||||||
|
}
|
||||||
|
|
||||||
thread_ctx = chThdCreateFromHeap(NULL,
|
thread_ctx = chThdCreateFromHeap(NULL,
|
||||||
THD_WORKING_AREA_SIZE(1024),
|
THD_WORKING_AREA_SIZE(1024),
|
||||||
@ -681,9 +682,11 @@ bool AP_IOMCU::check_crc(void)
|
|||||||
|
|
||||||
const uint16_t magic = REBOOT_BL_MAGIC;
|
const uint16_t magic = REBOOT_BL_MAGIC;
|
||||||
write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
|
write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
|
||||||
hal.scheduler->delay(100);
|
|
||||||
upload_fw(fw_name);
|
|
||||||
|
|
||||||
|
if (!upload_fw(fw_name)) {
|
||||||
|
AP_BoardConfig::sensor_config_error("Failed to update IO firmware");
|
||||||
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user