AP_HAL_SITL: allow for selection of Storage backend type at runtime

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

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -7,6 +7,7 @@
 #include "HAL_SITL_Class.h"
 #include "UARTDriver.h"
 #include <AP_HAL/utility/getopt_cpp.h>
+#include <AP_HAL_SITL/Storage.h>
 #include <AP_Logger/AP_Logger_SITL.h>
 #include <AP_Param/AP_Param.h>
 
@@ -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) {

libraries/AP_HAL_SITL/Storage.cpp

@@ -2,6 +2,7 @@
 
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_SITL.h"
 
 #include <assert.h>
 #include <sys/types.h>
@@ -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) {
-            return;
-        }
-        if (write(log_fd, &_buffer[offset], STORAGE_LINE_SIZE) != STORAGE_LINE_SIZE) {
+    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;
         }
-        _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;
 }
 
 

libraries/AP_HAL_SITL/Storage.h

@@ -5,13 +5,13 @@
 #include "AP_HAL_SITL_Namespace.h"
 #include <AP_FlashStorage/AP_FlashStorage.h>
 
-// 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<STORAGE_NUM_LINES> _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
 };