You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
100 lines
2.3 KiB
100 lines
2.3 KiB
/* |
|
backend for SITL emulation of block logging |
|
*/ |
|
|
|
#include "AP_Logger_SITL.h" |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
#include <unistd.h> |
|
#include <stdlib.h> |
|
#include <sys/types.h> |
|
#include <sys/stat.h> |
|
#include <fcntl.h> |
|
#include <assert.h> |
|
#include <stdio.h> |
|
|
|
#define DF_PAGE_SIZE 256UL |
|
#define DF_PAGE_PER_SECTOR 16 // 4k sectors |
|
#define DF_NUM_PAGES 65536UL |
|
|
|
#define ERASE_TIME_MS 10000 |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
void AP_Logger_SITL::Init() |
|
{ |
|
if (flash_fd == 0) { |
|
flash_fd = open(filename, O_RDWR|O_CLOEXEC, 0777); |
|
if (flash_fd == -1) { |
|
flash_fd = open(filename, O_RDWR | O_CREAT | O_CLOEXEC, 0777); |
|
StartErase(); |
|
erase_started_ms = 0; |
|
} |
|
if (ftruncate(flash_fd, DF_PAGE_SIZE*uint32_t(DF_NUM_PAGES)) != 0) { |
|
AP_HAL::panic("Failed to create %s", filename); |
|
} |
|
} |
|
|
|
df_PageSize = DF_PAGE_SIZE; |
|
df_PagePerSector = DF_PAGE_PER_SECTOR; |
|
df_NumPages = DF_NUM_PAGES; |
|
|
|
AP_Logger_Block::Init(); |
|
} |
|
|
|
bool AP_Logger_SITL::CardInserted(void) const |
|
{ |
|
return df_NumPages > 0; |
|
} |
|
|
|
void AP_Logger_SITL::PageToBuffer(uint32_t PageAdr) |
|
{ |
|
assert(PageAdr>0 && PageAdr <= df_NumPages+1); |
|
if (pread(flash_fd, buffer, DF_PAGE_SIZE, (PageAdr-1)*DF_PAGE_SIZE) != DF_PAGE_SIZE) { |
|
printf("Failed flash read"); |
|
} |
|
} |
|
|
|
void AP_Logger_SITL::BufferToPage(uint32_t PageAdr) |
|
{ |
|
assert(PageAdr>0 && PageAdr <= df_NumPages+1); |
|
if (pwrite(flash_fd, buffer, DF_PAGE_SIZE, (PageAdr-1)*DF_PAGE_SIZE) != DF_PAGE_SIZE) { |
|
printf("Failed flash write"); |
|
} |
|
} |
|
|
|
void AP_Logger_SITL::SectorErase(uint32_t SectorAdr) |
|
{ |
|
uint8_t fill[DF_PAGE_SIZE*DF_PAGE_PER_SECTOR]; |
|
memset(fill, 0xFF, sizeof(fill)); |
|
if (pwrite(flash_fd, fill, sizeof(fill), SectorAdr*DF_PAGE_PER_SECTOR*DF_PAGE_SIZE) != sizeof(fill)) { |
|
printf("Failed sector erase"); |
|
} |
|
} |
|
|
|
void AP_Logger_SITL::StartErase() |
|
{ |
|
for (uint32_t i=0; i<DF_NUM_PAGES/DF_PAGE_PER_SECTOR; i++) { |
|
SectorErase(i); |
|
} |
|
erase_started_ms = AP_HAL::millis(); |
|
} |
|
|
|
bool AP_Logger_SITL::InErase() |
|
{ |
|
if (erase_started_ms == 0) { |
|
return false; |
|
} |
|
uint32_t now = AP_HAL::millis(); |
|
if (now - erase_started_ms < ERASE_TIME_MS) { |
|
return true; |
|
} |
|
erase_started_ms = 0; |
|
return false; |
|
} |
|
|
|
#endif // HAL_BOARD_SITL |
|
|
|
|