diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 56bc9d822d..e4ee42cc4d 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -330,7 +330,11 @@ void Storage::_flash_load(void) #ifdef STORAGE_FLASH_PAGE _flash_page = STORAGE_FLASH_PAGE; +#if AP_FLASH_STORAGE_DOUBLE_PAGE + ::printf("Storage: Using flash pages %u to %u\n", _flash_page, _flash_page+3); +#else ::printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1); +#endif if (!_flash.init()) { AP_HAL::panic("Unable to init flash storage"); @@ -359,6 +363,9 @@ bool Storage::_flash_write(uint16_t line) bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) { #ifdef STORAGE_FLASH_PAGE +#if AP_FLASH_STORAGE_DOUBLE_PAGE + sector *= 2; +#endif size_t base_address = hal.flash->getpageaddr(_flash_page+sector); for (uint8_t i=0; igetpageaddr(_flash_page+sector); const uint8_t *b = ((const uint8_t *)base_address)+offset; memcpy(data, b, length); @@ -405,6 +415,9 @@ bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, u bool Storage::_flash_erase_sector(uint8_t sector) { #ifdef STORAGE_FLASH_PAGE +#if AP_FLASH_STORAGE_DOUBLE_PAGE + sector *= 2; +#endif // erasing a page can take long enough that USB may not initialise properly if it happens // while the host is connecting. Only do a flash erase if we have been up for more than 4s for (uint8_t i=0; ierasepage(_flash_page+sector) && hal.flash->erasepage(_flash_page+sector+1)) { + return true; + } +#else if (hal.flash->erasepage(_flash_page+sector)) { return true; } +#endif hal.scheduler->delay(1); } return false; diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index 59266d19bb..80271fe3d8 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -47,6 +47,13 @@ static_assert(CH_STORAGE_SIZE % CH_STORAGE_LINE_SIZE == 0, "Storage is not multiple of line size"); +/* + on boards with 8k sector sizes we double up to treat pairs of sectors as one + */ +#ifndef AP_FLASH_STORAGE_DOUBLE_PAGE +#define AP_FLASH_STORAGE_DOUBLE_PAGE 0 +#endif + class ChibiOS::Storage : public AP_HAL::Storage { public: void init() override {} @@ -86,7 +93,11 @@ private: #ifdef STORAGE_FLASH_PAGE AP_FlashStorage _flash{_buffer, +#if AP_FLASH_STORAGE_DOUBLE_PAGE + stm32_flash_getpagesize(STORAGE_FLASH_PAGE)*2, +#else stm32_flash_getpagesize(STORAGE_FLASH_PAGE), +#endif FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t),