mirror of https://github.com/ArduPilot/ardupilot
HAL_ChibiOS: added expected delay in bootloader flash
This commit is contained in:
parent
dc6948f336
commit
378d5c7a5a
|
@ -238,6 +238,8 @@ Util::FlashBootloader Util::flash_bootloader()
|
||||||
hal.console->printf("failed to find %s\n", fw_name);
|
hal.console->printf("failed to find %s\n", fw_name);
|
||||||
return FlashBootloader::NOT_AVAILABLE;
|
return FlashBootloader::NOT_AVAILABLE;
|
||||||
}
|
}
|
||||||
|
// make sure size is multiple of 32
|
||||||
|
fw_size = (fw_size + 31U) & ~31U;
|
||||||
|
|
||||||
const uint32_t addr = hal.flash->getpageaddr(0);
|
const uint32_t addr = hal.flash->getpageaddr(0);
|
||||||
if (!memcmp(fw, (const void*)addr, fw_size)) {
|
if (!memcmp(fw, (const void*)addr, fw_size)) {
|
||||||
|
@ -250,13 +252,14 @@ Util::FlashBootloader Util::flash_bootloader()
|
||||||
uint32_t erased_size = 0;
|
uint32_t erased_size = 0;
|
||||||
uint8_t erase_page = 0;
|
uint8_t erase_page = 0;
|
||||||
while (erased_size < fw_size) {
|
while (erased_size < fw_size) {
|
||||||
if (!hal.flash->erasepage(erase_page)) {
|
uint32_t page_size = hal.flash->getpagesize(erase_page);
|
||||||
hal.console->printf("Erase %u failed\n", erase_page);
|
if (page_size == 0) {
|
||||||
AP_ROMFS::free(fw);
|
AP_ROMFS::free(fw);
|
||||||
return FlashBootloader::FAIL;
|
return FlashBootloader::FAIL;
|
||||||
}
|
}
|
||||||
uint32_t page_size = hal.flash->getpagesize(erase_page);
|
hal.scheduler->expect_delay_ms(1000);
|
||||||
if (page_size == 0) {
|
if (!hal.flash->erasepage(erase_page)) {
|
||||||
|
hal.console->printf("Erase %u failed\n", erase_page);
|
||||||
AP_ROMFS::free(fw);
|
AP_ROMFS::free(fw);
|
||||||
return FlashBootloader::FAIL;
|
return FlashBootloader::FAIL;
|
||||||
}
|
}
|
||||||
|
@ -266,20 +269,24 @@ Util::FlashBootloader Util::flash_bootloader()
|
||||||
|
|
||||||
hal.console->printf("Flashing %s @%08x\n", fw_name, (unsigned int)addr);
|
hal.console->printf("Flashing %s @%08x\n", fw_name, (unsigned int)addr);
|
||||||
const uint8_t max_attempts = 10;
|
const uint8_t max_attempts = 10;
|
||||||
|
hal.flash->keep_unlocked(true);
|
||||||
for (uint8_t i=0; i<max_attempts; i++) {
|
for (uint8_t i=0; i<max_attempts; i++) {
|
||||||
|
hal.scheduler->expect_delay_ms(1000);
|
||||||
bool ok = hal.flash->write(addr, fw, fw_size);
|
bool ok = hal.flash->write(addr, fw, fw_size);
|
||||||
if (!ok) {
|
if (!ok) {
|
||||||
hal.console->printf("Flash failed! (attempt=%u/%u)\n",
|
hal.console->printf("Flash failed! (attempt=%u/%u)\n",
|
||||||
i+1,
|
i+1,
|
||||||
max_attempts);
|
max_attempts);
|
||||||
hal.scheduler->delay(1000);
|
hal.scheduler->delay(100);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
hal.console->printf("Flash OK\n");
|
hal.console->printf("Flash OK\n");
|
||||||
|
hal.flash->keep_unlocked(false);
|
||||||
AP_ROMFS::free(fw);
|
AP_ROMFS::free(fw);
|
||||||
return FlashBootloader::OK;
|
return FlashBootloader::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
hal.flash->keep_unlocked(false);
|
||||||
hal.console->printf("Flash failed after %u attempts\n", max_attempts);
|
hal.console->printf("Flash failed after %u attempts\n", max_attempts);
|
||||||
AP_ROMFS::free(fw);
|
AP_ROMFS::free(fw);
|
||||||
return FlashBootloader::FAIL;
|
return FlashBootloader::FAIL;
|
||||||
|
|
Loading…
Reference in New Issue