diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 62089bb785..a67f84c8eb 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -14,11 +14,24 @@ public: void run(int argc, char * const argv[], Callbacks* callbacks) const override; static void actually_reboot(); + void set_storage_posix_enabled(bool _enabled) { + storage_posix_enabled = _enabled; + } + bool get_storage_posix_enabled() const { return storage_posix_enabled; } + void set_storage_flash_enabled(bool _enabled) { + storage_flash_enabled = _enabled; + } + bool get_storage_flash_enabled() const { return storage_flash_enabled; } + private: HALSITL::SITL_State *_sitl_state; void setup_signal_handlers() const; static void exit_signal_handler(int); + + bool storage_posix_enabled; + bool storage_flash_enabled; + }; #if HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index a9755b9724..7ae0bb3846 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -7,6 +7,7 @@ #include "HAL_SITL_Class.h" #include "UARTDriver.h" #include +#include #include #include @@ -263,6 +264,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_START_TIME, CMDLINE_SYSID, CMDLINE_SLAVE, +#if STORAGE_USE_FLASH + CMDLINE_SET_STORAGE_FLASH_ENABLED, +#endif +#if STORAGE_USE_POSIX + CMDLINE_SET_STORAGE_POSIX_ENABLED, +#endif }; const struct GetOptLong::option options[] = { @@ -311,6 +318,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) {"start-time", true, 0, CMDLINE_START_TIME}, {"sysid", true, 0, CMDLINE_SYSID}, {"slave", true, 0, CMDLINE_SLAVE}, +#if STORAGE_USE_FLASH + {"set-storage-flash-enabled", true, 0, CMDLINE_SET_STORAGE_FLASH_ENABLED}, +#endif +#if STORAGE_USE_POSIX + {"set-storage-posix-enabled", true, 0, CMDLINE_SET_STORAGE_POSIX_ENABLED}, +#endif {0, false, 0, 0} }; @@ -319,6 +332,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) HALSITL::UARTDriver::_console = true; } + // storage defaults are set here: + bool storage_posix_enabled = true; + bool storage_flash_enabled = false; + bool erase_all_storage = false; + if (asprintf(&autotest_dir, SKETCHBOOK "/Tools/autotest") <= 0) { AP_HAL::panic("out of memory"); } @@ -332,12 +350,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) while (!is_replay && (opt = gopt.getoption()) != -1) { switch (opt) { case 'w': -#if HAL_LOGGING_FILESYSTEM_ENABLED - AP_Param::erase_all(); -#endif -#if HAL_LOGGING_SITL_ENABLED - unlink(AP_Logger_SITL::filename); -#endif + erase_all_storage = true; break; case 'u': AP_Param::set_hide_disabled_groups(false); @@ -463,6 +476,16 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) printf("Setting SYSID_THISMAV=%d\n", sysid); break; } +#if STORAGE_USE_POSIX + case CMDLINE_SET_STORAGE_POSIX_ENABLED: + storage_posix_enabled = atoi(gopt.optarg); + break; +#endif +#if STORAGE_USE_FLASH + case CMDLINE_SET_STORAGE_FLASH_ENABLED: + storage_flash_enabled = atoi(gopt.optarg); + break; +#endif case 'h': _usage(); exit(0); @@ -522,11 +545,28 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) exit(1); } + if (storage_posix_enabled && storage_flash_enabled) { + // this will change in the future! + printf("Only one of flash or posix storage may be selected"); + exit(1); + } + if (AP::sitl()) { // Set SITL start time. AP::sitl()->start_time_UTC = start_time_UTC; } + ((HAL_SITL&)hal).set_storage_posix_enabled(storage_posix_enabled); + ((HAL_SITL&)hal).set_storage_flash_enabled(storage_flash_enabled); + + if (erase_all_storage) { + AP_Param::erase_all(); +#if HAL_LOGGING_SITL_ENABLED + unlink(AP_Logger_SITL::filename); +#endif + unlink("flash.dat"); + } + fprintf(stdout, "Starting sketch '%s'\n", SKETCH); if (strcmp(SKETCH, "ArduCopter") == 0) { diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index 9a8eb951bc..9e468e2ecd 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -2,6 +2,7 @@ #include #include +#include "AP_HAL_SITL.h" #include #include @@ -47,52 +48,59 @@ extern const AP_HAL::HAL& hal; void Storage::_storage_open(void) { - if (_initialised) { + if (_initialisedType != StorageBackend::None) { + // don't reinit return; } -#if STORAGE_USE_POSIX - // if we have failed filesystem init don't try again - if (log_fd == -1) { - return; - } -#endif - _dirty_mask.clearall(); #if STORAGE_USE_FLASH - // load from storage backend - _flash_load(); -#elif STORAGE_USE_POSIX - log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT, 0644); - if (log_fd == -1) { - hal.console->printf("open failed of " HAL_STORAGE_FILE "\n"); + if (((HAL_SITL&)hal).get_storage_flash_enabled()) { + // load from storage backend + _flash_load(); + _initialisedType = StorageBackend::Flash; return; } +#endif // STORAGE_USE_FLASH - fcntl(log_fd, F_SETFD, FD_CLOEXEC); +#if STORAGE_USE_POSIX + if (((HAL_SITL&)hal).get_storage_posix_enabled()) { + // if we have failed filesystem init don't try again (this is + // initialised to zero in the constructor) + if (log_fd == -1) { + return; + } - int ret = read(log_fd, _buffer, HAL_STORAGE_SIZE); - if (ret < 0) { - hal.console->printf("read failed for " HAL_STORAGE_FILE "\n"); - close(log_fd); - log_fd = -1; + log_fd = open(HAL_STORAGE_FILE, O_RDWR|O_CREAT, 0644); + if (log_fd == -1) { + hal.console->printf("open failed of " HAL_STORAGE_FILE "\n"); + return; + } + + fcntl(log_fd, F_SETFD, FD_CLOEXEC); + + int ret = read(log_fd, _buffer, HAL_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], HAL_STORAGE_SIZE-ret) != HAL_STORAGE_SIZE-ret) { + hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n"); + close(log_fd); + log_fd = -1; + return; + } + _initialisedType = StorageBackend::SDCard; // AKA POSIX return; } - // pre-fill to full size - if (lseek(log_fd, ret, SEEK_SET) != ret || - write(log_fd, &_buffer[ret], HAL_STORAGE_SIZE-ret) != HAL_STORAGE_SIZE-ret) { - hal.console->printf("setup failed for " HAL_STORAGE_FILE "\n"); - close(log_fd); - log_fd = -1; - return; - } - using_filesystem = true; -#else -#error "No storage system enabled" #endif - _initialised = true; +// ::printf("No storage backend enabled"); } /* @@ -138,7 +146,7 @@ void Storage::write_block(uint16_t loc, const void *src, size_t n) void Storage::_timer_tick(void) { - if (!_initialised) { + if (_initialisedType == StorageBackend::None) { return; } if (_dirty_mask.empty()) { @@ -160,37 +168,40 @@ void Storage::_timer_tick(void) } #if STORAGE_USE_POSIX - if (using_filesystem && log_fd != -1) { - const off_t offset = STORAGE_LINE_SIZE*i; - if (lseek(log_fd, offset, SEEK_SET) != offset) { + if (((HAL_SITL&)hal).get_storage_posix_enabled()) { + if (log_fd != -1) { + const off_t offset = STORAGE_LINE_SIZE*i; + if (lseek(log_fd, offset, SEEK_SET) != offset) { + return; + } + if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) { + return; + } + _dirty_mask.clear(i); return; } - if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) { - return; - } - _dirty_mask.clear(i); - return; - } + } #endif - + #if STORAGE_USE_FLASH - // save to storage backend - _flash_write(i); + if (((HAL_SITL&)hal).get_storage_flash_enabled()) { + // save to storage backend + _flash_write(i); + return; + } #endif } +#if STORAGE_USE_FLASH + /* load all data from flash */ void Storage::_flash_load(void) { -#if STORAGE_USE_FLASH if (!_flash.init()) { AP_HAL::panic("unable to init flash storage"); } -#else - AP_HAL::panic("unable to init storage"); -#endif } /* @@ -198,16 +209,13 @@ void Storage::_flash_load(void) */ void Storage::_flash_write(uint16_t line) { -#if STORAGE_USE_FLASH if (_flash.write(line*STORAGE_LINE_SIZE, STORAGE_LINE_SIZE)) { // mark the line clean _dirty_mask.clear(line); } -#endif } -#if STORAGE_USE_FLASH /* emulate writing to flash */ @@ -342,6 +350,7 @@ bool Storage::_flash_erase_ok(void) // only allow erase while disarmed return !hal.util->get_soft_armed(); } + #endif // STORAGE_USE_FLASH /* @@ -350,7 +359,10 @@ bool Storage::_flash_erase_ok(void) */ bool Storage::healthy(void) { - return _initialised && AP_HAL::millis() - _last_empty_ms < 2000; + if (_initialisedType == StorageBackend::None) { + return false; + } + return AP_HAL::millis() - _last_empty_ms < 2000; } diff --git a/libraries/AP_HAL_SITL/Storage.h b/libraries/AP_HAL_SITL/Storage.h index 210f83b0af..d156f01d78 100644 --- a/libraries/AP_HAL_SITL/Storage.h +++ b/libraries/AP_HAL_SITL/Storage.h @@ -5,13 +5,13 @@ #include "AP_HAL_SITL_Namespace.h" #include -// define which storage system to use. This allows us to test flash storage with --sitl-flash-storage -// configure option #ifndef STORAGE_USE_FLASH -#define STORAGE_USE_FLASH 0 +#define STORAGE_USE_FLASH 1 #endif +#ifndef STORAGE_USE_POSIX #define STORAGE_USE_POSIX 1 +#endif #define STORAGE_LINE_SHIFT 3 @@ -28,7 +28,14 @@ public: bool healthy(void) override; private: - volatile bool _initialised; + enum class StorageBackend: uint8_t { + None, +// FRAM, + Flash, + SDCard, // AKA POSIX + }; + StorageBackend _initialisedType = StorageBackend::None; + void _storage_create(void); void _storage_open(void); void _save_backup(void); @@ -36,31 +43,29 @@ private: uint8_t _buffer[HAL_STORAGE_SIZE] __attribute__((aligned(4))); Bitmask _dirty_mask; + uint32_t _last_empty_ms; + #if STORAGE_USE_FLASH bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length); bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length); bool _flash_erase_sector(uint8_t sector); bool _flash_erase_ok(void); -#endif bool _flash_failed; uint32_t _last_re_init_ms; - uint32_t _last_empty_ms; -#if STORAGE_USE_FLASH AP_FlashStorage _flash{_buffer, HAL_FLASH_SECTOR_SIZE, 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), FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)}; -#endif - + void _flash_load(void); void _flash_write(uint16_t line); +#endif #if STORAGE_USE_POSIX - bool using_filesystem; int log_fd; #endif };