Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: allow for selection of Storage backend type at runtime

Browse Source
This commit is contained in:
Peter Barker 2021-06-11 12:30:38 +10:00 committed by Andrew Tridgell
parent 102b1c6879
commit 3292128531
4 changed files with 139 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_HAL_SITL/HAL_SITL_Class.h
View File

@ -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

52
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -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) {

118
libraries/AP_HAL_SITL/Storage.cpp
View File

@ -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) {
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;
}

25
libraries/AP_HAL_SITL/Storage.h
View File

@ -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
};