Browse Source

HAL_ChibiOS: support having no flash storage option

mission-4.1.18
Andrew Tridgell 7 years ago
parent
commit
b961e12456
  1. 10
      libraries/AP_HAL_ChibiOS/Storage.cpp
  2. 2
      libraries/AP_HAL_ChibiOS/Storage.h

10
libraries/AP_HAL_ChibiOS/Storage.cpp

@ -128,6 +128,7 @@ void Storage::_timer_tick(void)
*/ */
void Storage::_flash_load(void) void Storage::_flash_load(void)
{ {
#ifdef STORAGE_FLASH_PAGE
_flash_page = STORAGE_FLASH_PAGE; _flash_page = STORAGE_FLASH_PAGE;
hal.console->printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1); hal.console->printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
@ -135,6 +136,9 @@ void Storage::_flash_load(void)
if (!_flash.init()) { if (!_flash.init()) {
AP_HAL::panic("unable to init flash storage"); AP_HAL::panic("unable to init flash storage");
} }
#else
AP_HAL::panic("unable to init storage");
#endif
} }
/* /*
@ -142,10 +146,12 @@ void Storage::_flash_load(void)
*/ */
void Storage::_flash_write(uint16_t line) void Storage::_flash_write(uint16_t line)
{ {
#ifdef STORAGE_FLASH_PAGE
if (_flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE)) { if (_flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE)) {
// mark the line clean // mark the line clean
_dirty_mask.clear(line); _dirty_mask.clear(line);
} }
#endif
} }
/* /*
@ -153,6 +159,7 @@ void Storage::_flash_write(uint16_t line)
*/ */
bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
{ {
#ifdef STORAGE_FLASH_PAGE
size_t base_address = stm32_flash_getpageaddr(_flash_page+sector); size_t base_address = stm32_flash_getpageaddr(_flash_page+sector);
bool ret = stm32_flash_write(base_address+offset, data, length) == length; bool ret = stm32_flash_write(base_address+offset, data, length) == length;
if (!ret && _flash_erase_ok()) { if (!ret && _flash_erase_ok()) {
@ -167,6 +174,9 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *
} }
} }
return ret; return ret;
#else
return false;
#endif
} }
/* /*

2
libraries/AP_HAL_ChibiOS/Storage.h

@ -58,12 +58,14 @@ private:
bool _flash_failed; bool _flash_failed;
uint32_t _last_re_init_ms; uint32_t _last_re_init_ms;
#ifdef STORAGE_FLASH_PAGE
AP_FlashStorage _flash{_buffer, AP_FlashStorage _flash{_buffer,
stm32_flash_getpagesize(STORAGE_FLASH_PAGE), stm32_flash_getpagesize(STORAGE_FLASH_PAGE),
FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t), 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_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t),
FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)}; FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)};
#endif
void _flash_load(void); void _flash_load(void);
void _flash_write(uint16_t line); void _flash_write(uint16_t line);

Loading…
Cancel
Save