mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 04:28:30 -04:00
HAL_PX4: new buffered storeage driver for microsd cards
this does all IO in the timer thread, avoids writes that don't change data, and does all writes in multiples of 128 byte chunks. This should be about as friendly to SD cards as we can get.
This commit is contained in:
Andrew Tridgell
parent
911e090e07
commit
952df2fced
@ -6,7 +6,7 @@ namespace PX4 {
|
||||
class PX4ConsoleDriver;
|
||||
class PX4Scheduler;
|
||||
class PX4UARTDriver;
|
||||
class PX4EEPROMStorage;
|
||||
class PX4Storage;
|
||||
class PX4RCInput;
|
||||
class PX4RCOutput;
|
||||
class PX4AnalogIn;
|
||||
|
||||
@ -35,7 +35,7 @@ static Empty::EmptyGPIO gpioDriver;
|
||||
|
||||
static PX4ConsoleDriver consoleDriver;
|
||||
static PX4Scheduler schedulerInstance;
|
||||
static PX4EEPROMStorage storageDriver;
|
||||
static PX4Storage storageDriver;
|
||||
static PX4RCInput rcinDriver;
|
||||
static PX4RCOutput rcoutDriver;
|
||||
static PX4AnalogIn analogIn;
|
||||
|
||||
@ -16,6 +16,7 @@
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <poll.h>
|
||||
#include "UARTDriver.h"
|
||||
#include "Storage.h"
|
||||
|
||||
using namespace PX4;
|
||||
|
||||
@ -179,6 +180,9 @@ void *PX4Scheduler::_timer_thread(void)
|
||||
// process any pending serial bytes
|
||||
((PX4UARTDriver *)hal.uartA)->_timer_tick();
|
||||
((PX4UARTDriver *)hal.uartB)->_timer_tick();
|
||||
|
||||
// process any pending storage writes
|
||||
((PX4Storage *)hal.storage)->_timer_tick();
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@ -7,106 +7,190 @@
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "Storage.h"
|
||||
using namespace PX4;
|
||||
|
||||
#define EEPROM_SIZE 4096
|
||||
/*
|
||||
This stores 'eeprom' data on the SD card, with a 4k size, and a
|
||||
in-memory buffer. This keeps the latency down.
|
||||
*/
|
||||
|
||||
// name the eeprom file after the sketch so you can use the same sd
|
||||
// name the storage file after the sketch so you can use the same sd
|
||||
// card for ArduCopter and ArduPlane
|
||||
#define EEPROM_DIR "/fs/microsd/APM"
|
||||
#define EEPROM_FILE EEPROM_DIR "/" SKETCHNAME ".eeprom"
|
||||
#define STORAGE_DIR "/fs/microsd/APM"
|
||||
#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/* NuttX doesn't have pread/pwrite */
|
||||
static ssize_t pread(int fd, void *buf, size_t count, off_t ofs)
|
||||
void PX4Storage::_storage_create(void)
|
||||
{
|
||||
lseek(fd, ofs, SEEK_SET);
|
||||
return read(fd, buf, count);
|
||||
}
|
||||
|
||||
static ssize_t pwrite(int fd, const void *buf, size_t count, off_t ofs)
|
||||
{
|
||||
lseek(fd, ofs, SEEK_SET);
|
||||
return write(fd, buf, count);
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::_eeprom_open(void)
|
||||
{
|
||||
if (_eeprom_fd == -1) {
|
||||
mkdir(EEPROM_DIR, 0777);
|
||||
_eeprom_fd = open(EEPROM_FILE, O_RDWR|O_CREAT, 0777);
|
||||
if (_eeprom_fd == -1) {
|
||||
hal.scheduler->panic("Failed to open " EEPROM_FILE);
|
||||
mkdir(STORAGE_DIR, 0777);
|
||||
unlink(STORAGE_FILE);
|
||||
_fd = open(STORAGE_FILE, O_RDWR|O_CREAT|O_TRUNC, 0666);
|
||||
if (_fd == -1) {
|
||||
hal.scheduler->panic("Failed to create " STORAGE_FILE);
|
||||
}
|
||||
for (uint16_t loc=0; loc<sizeof(_buffer); loc += PX4_STORAGE_MAX_WRITE) {
|
||||
if (write(_fd, &_buffer[loc], PX4_STORAGE_MAX_WRITE) != PX4_STORAGE_MAX_WRITE) {
|
||||
hal.scheduler->panic("Error filling " STORAGE_FILE);
|
||||
}
|
||||
if (lseek(_eeprom_fd, 0, SEEK_END) < EEPROM_SIZE) {
|
||||
char c = 0;
|
||||
assert(pwrite(_eeprom_fd, &c, 1, EEPROM_SIZE-1) == 1);
|
||||
}
|
||||
// ensure the directory is updated with the new size
|
||||
fsync(_fd);
|
||||
}
|
||||
|
||||
void PX4Storage::_storage_open(void)
|
||||
{
|
||||
if (_fd != -1) {
|
||||
return;
|
||||
}
|
||||
|
||||
_dirty_mask = 0;
|
||||
_fd = open(STORAGE_FILE, O_RDWR);
|
||||
if (_fd == -1) {
|
||||
_storage_create();
|
||||
} else if (read(_fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
|
||||
close(_fd);
|
||||
_fd = -1;
|
||||
_storage_create();
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length)
|
||||
{
|
||||
uint16_t end = loc + length;
|
||||
while (loc < end) {
|
||||
uint8_t line = (loc >> PX4_STORAGE_LINE_SHIFT);
|
||||
_dirty_mask |= 1 << line;
|
||||
loc += PX4_STORAGE_LINE_SIZE;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t PX4Storage::read_byte(uint16_t loc)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)) {
|
||||
return 0;
|
||||
}
|
||||
_storage_open();
|
||||
return _buffer[loc];
|
||||
}
|
||||
|
||||
uint16_t PX4Storage::read_word(uint16_t loc)
|
||||
{
|
||||
uint16_t value;
|
||||
if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
|
||||
return 0;
|
||||
}
|
||||
_storage_open();
|
||||
memcpy(&value, &_buffer[loc], sizeof(value));
|
||||
return value;
|
||||
}
|
||||
|
||||
uint32_t PX4Storage::read_dword(uint16_t loc)
|
||||
{
|
||||
uint32_t value;
|
||||
if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
|
||||
return 0;
|
||||
}
|
||||
_storage_open();
|
||||
memcpy(&value, &_buffer[loc], sizeof(value));
|
||||
return value;
|
||||
}
|
||||
|
||||
void PX4Storage::read_block(void *dst, uint16_t loc, size_t n)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(n-1)) {
|
||||
return;
|
||||
}
|
||||
_storage_open();
|
||||
memcpy(dst, &_buffer[loc], n);
|
||||
}
|
||||
|
||||
void PX4Storage::write_byte(uint16_t loc, uint8_t value)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)) {
|
||||
return;
|
||||
}
|
||||
if (_buffer[loc] != value) {
|
||||
_storage_open();
|
||||
_buffer[loc] = value;
|
||||
_mark_dirty(loc, sizeof(value));
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Storage::write_word(uint16_t loc, uint16_t value)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
|
||||
return;
|
||||
}
|
||||
if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
|
||||
_storage_open();
|
||||
memcpy(&_buffer[loc], &value, sizeof(value));
|
||||
_mark_dirty(loc, sizeof(value));
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Storage::write_dword(uint16_t loc, uint32_t value)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
|
||||
return;
|
||||
}
|
||||
if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
|
||||
_storage_open();
|
||||
memcpy(&_buffer[loc], &value, sizeof(value));
|
||||
_mark_dirty(loc, sizeof(value));
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Storage::write_block(uint16_t loc, void *src, size_t n)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(n-1)) {
|
||||
return;
|
||||
}
|
||||
if (memcmp(src, &_buffer[loc], n) != 0) {
|
||||
_storage_open();
|
||||
memcpy(&_buffer[loc], src, n);
|
||||
_mark_dirty(loc, n);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Storage::_timer_tick(void)
|
||||
{
|
||||
if (_fd == -1 || _dirty_mask == 0) {
|
||||
return;
|
||||
}
|
||||
// write out the first dirty set of lines. We don't write more
|
||||
// than one to keep the latency of this call to a minimum
|
||||
uint8_t i, n;
|
||||
for (i=0; i<PX4_STORAGE_NUM_LINES; i++) {
|
||||
if (_dirty_mask & (1<<i)) {
|
||||
// mark that line clean
|
||||
_dirty_mask &= ~(1<<i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == PX4_STORAGE_NUM_LINES) {
|
||||
return;
|
||||
}
|
||||
// see how many lines to write
|
||||
for (n=1; (i+n) < PX4_STORAGE_NUM_LINES &&
|
||||
n < (1024>>PX4_STORAGE_LINE_SHIFT); n++) {
|
||||
if (!(_dirty_mask & (1<<(n+i)))) {
|
||||
break;
|
||||
}
|
||||
// mark that line clean
|
||||
_dirty_mask &= ~(1<<(i+n));
|
||||
}
|
||||
|
||||
// write them
|
||||
if (lseek(_fd, i<<PX4_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<PX4_STORAGE_LINE_SHIFT)) {
|
||||
write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
|
||||
if (_dirty_mask == 0) {
|
||||
fsync(_fd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t PX4EEPROMStorage::read_byte(uint16_t loc)
|
||||
{
|
||||
uint8_t value;
|
||||
_eeprom_open();
|
||||
assert(pread(_eeprom_fd, &value, 1, loc) == 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
uint16_t PX4EEPROMStorage::read_word(uint16_t loc)
|
||||
{
|
||||
uint16_t value;
|
||||
assert(loc < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pread(_eeprom_fd, &value, 2, loc) == 2);
|
||||
return value;
|
||||
}
|
||||
|
||||
uint32_t PX4EEPROMStorage::read_dword(uint16_t loc)
|
||||
{
|
||||
uint32_t value;
|
||||
assert(loc < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pread(_eeprom_fd, &value, 4, loc) == 4);
|
||||
return value;
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
|
||||
{
|
||||
assert(src < EEPROM_SIZE && src + n < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::write_byte(uint16_t loc, uint8_t value)
|
||||
{
|
||||
assert(loc < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pwrite(_eeprom_fd, &value, 1, loc) == 1);
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::write_word(uint16_t loc, uint16_t value)
|
||||
{
|
||||
assert(loc < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pwrite(_eeprom_fd, &value, 2, loc) == 2);
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::write_dword(uint16_t loc, uint32_t value)
|
||||
{
|
||||
assert(loc < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pwrite(_eeprom_fd, &value, 4, loc) == 4);
|
||||
}
|
||||
|
||||
void PX4EEPROMStorage::write_block(uint16_t dst, void *src, size_t n)
|
||||
{
|
||||
assert(dst < EEPROM_SIZE);
|
||||
_eeprom_open();
|
||||
assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif // CONFIG_HAL_BOARD
|
||||
|
||||
@ -6,10 +6,16 @@
|
||||
#include <AP_HAL.h>
|
||||
#include "AP_HAL_PX4_Namespace.h"
|
||||
|
||||
class PX4::PX4EEPROMStorage : public AP_HAL::Storage {
|
||||
#define PX4_STORAGE_SIZE 4096
|
||||
#define PX4_STORAGE_MAX_WRITE 1024
|
||||
#define PX4_STORAGE_LINE_SHIFT 7
|
||||
#define PX4_STORAGE_LINE_SIZE (1<<PX4_STORAGE_LINE_SHIFT)
|
||||
#define PX4_STORAGE_NUM_LINES (PX4_STORAGE_SIZE/PX4_STORAGE_LINE_SIZE)
|
||||
|
||||
class PX4::PX4Storage : public AP_HAL::Storage {
|
||||
public:
|
||||
PX4EEPROMStorage() {
|
||||
_eeprom_fd = -1;
|
||||
PX4Storage() {
|
||||
_fd = -1;
|
||||
}
|
||||
void init(void* machtnichts) {}
|
||||
uint8_t read_byte(uint16_t loc);
|
||||
@ -22,9 +28,15 @@ public:
|
||||
void write_dword(uint16_t loc, uint32_t value);
|
||||
void write_block(uint16_t dst, void* src, size_t n);
|
||||
|
||||
void _timer_tick(void);
|
||||
|
||||
private:
|
||||
int _eeprom_fd;
|
||||
void _eeprom_open(void);
|
||||
int _fd;
|
||||
void _storage_create(void);
|
||||
void _storage_open(void);
|
||||
void _mark_dirty(uint16_t loc, uint16_t length);
|
||||
uint8_t _buffer[PX4_STORAGE_SIZE];
|
||||
volatile uint32_t _dirty_mask;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_PX4_STORAGE_H__
|
||||
|
||||
Loading…
Reference in New Issue
Block a user