diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 12959f4e4b..309da5df58 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -19,6 +19,7 @@ #include "Storage.h" #include "hwdef/common/flash.h" +#include "sdcard.h" using namespace ChibiOS; @@ -26,12 +27,25 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; +#ifndef HAL_STORAGE_FILE +// using SKETCHNAME allows the one microSD to be used +// for multiple vehicle types +#define HAL_STORAGE_FILE "/APM/" SKETCHNAME ".stg" +#endif + void Storage::_storage_open(void) { if (_initialised) { return; } +#ifdef USE_POSIX + // if we have failed filesystem init don't try again + if (log_fd == -1) { + return; + } +#endif + _dirty_mask.clearall(); #if HAL_WITH_RAMTRON @@ -46,8 +60,37 @@ void Storage::_storage_open(void) // allow for FMUv3 with no FRAM chip, fall through to flash storage #endif +#ifdef STORAGE_FLASH_PAGE // load from storage backend _flash_load(); +#endif + +#ifdef USE_POSIX + // allow for fallback to microSD based storage + sdcard_init(); + + log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT); + if (log_fd == -1) { + hal.console->printf("open failed of " HAL_STORAGE_FILE "\n"); + return; + } + int ret = read(log_fd, _buffer, CH_STORAGE_SIZE); + if (ret < 0) { + hal.console->printf("read failed for " HAL_STORAGE_FILE "\n"); + close(log_fd); + log_fd = -1; + return; + } + // pre-fill to full size + if (lseek(log_fd, ret, SEEK_SET) != ret || + write(log_fd, &_buffer[ret], CH_STORAGE_SIZE-ret) != CH_STORAGE_SIZE-ret) { + hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n"); + close(log_fd); + log_fd = -1; + return; + } + using_filesystem = true; +#endif _initialised = true; } @@ -92,10 +135,13 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n) void Storage::_timer_tick(void) { - if (!_initialised || _dirty_mask.empty()) { + if (!_initialised) { + return; + } + if (_dirty_mask.empty()) { + _last_empty_ms = AP_HAL::millis(); return; } - // write out the first dirty line. We don't write more // than one to keep the latency of this call to a minimum @@ -119,8 +165,27 @@ void Storage::_timer_tick(void) } #endif +#ifdef USE_POSIX + if (using_filesystem && log_fd != -1) { + uint32_t offset = CH_STORAGE_LINE_SIZE*i; + if (lseek(log_fd, offset, SEEK_SET) != offset) { + return; + } + if (write(log_fd, &_buffer[offset], CH_STORAGE_LINE_SIZE) != CH_STORAGE_LINE_SIZE) { + return; + } + if (fsync(log_fd) != 0) { + return; + } + _dirty_mask.clear(i); + return; + } +#endif + +#ifdef STORAGE_FLASH_PAGE // save to storage backend _flash_write(i); +#endif } /* @@ -207,4 +272,13 @@ bool Storage::_flash_erase_ok(void) return !hal.util->get_soft_armed(); } +/* + consider storage healthy if we have nothing to write sometime in the + last 2 seconds + */ +bool Storage::healthy(void) +{ + return _initialised && AP_HAL::millis() - _last_empty_ms < 2000; +} + #endif // HAL_USE_EMPTY_STORAGE diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index 6d2cae2b39..4c958a287a 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -41,6 +41,7 @@ public: void write_block(uint16_t dst, const void* src, size_t n); void _timer_tick(void) override; + bool healthy(void) override; private: volatile bool _initialised; @@ -57,6 +58,7 @@ private: uint8_t _flash_page; bool _flash_failed; uint32_t _last_re_init_ms; + uint32_t _last_empty_ms; #ifdef STORAGE_FLASH_PAGE AP_FlashStorage _flash{_buffer, @@ -74,6 +76,10 @@ private: AP_RAMTRON fram; bool using_fram; #endif +#ifdef USE_POSIX + bool using_filesystem; + int log_fd; +#endif }; #endif // HAL_USE_EMPTY_STORAGE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat index 03f732c4be..3d53041468 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat @@ -240,8 +240,6 @@ PD2 SDMMC_CMD SDMMC1 define HAL_STORAGE_SIZE 16384 define HAL_WITH_RAMTRON 1 -# define STORAGE_FLASH_PAGE 11 - define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index a6a1c90597..93b39e1666 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -22,6 +22,7 @@ extern const AP_HAL::HAL& hal; #ifdef USE_POSIX static FATFS SDC_FS; // FATFS object +static bool sdcard_init_done; #endif #if HAL_USE_MMC_SPI @@ -39,6 +40,10 @@ static bool sdcard_running; void sdcard_init() { #ifdef USE_POSIX + if (sdcard_init_done) { + return; + } + sdcard_init_done = true; #if HAL_USE_SDC #if defined(STM32_SDC_USE_SDMMC1) && STM32_SDC_USE_SDMMC1 == TRUE