mirror of https://github.com/ArduPilot/ardupilot
AP_Logger: revived block based logging
This commit is contained in:
Andrew Tridgell
parent
2882e5d5e1
commit
e3c9f10e91
|
|
@ -4,6 +4,8 @@
|
|||
|
||||
#include "AP_Logger_File.h"
|
||||
#include "AP_Logger_File_sd.h"
|
||||
#include "AP_Logger_SITL.h"
|
||||
#include "AP_Logger_DataFlash.h"
|
||||
#include "AP_Logger_MAVLink.h"
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
|
||||
|
|
@ -28,6 +30,7 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = {
|
|||
// @DisplayName: AP_Logger Backend Storage type
|
||||
// @Description: 0 for None, 1 for File, 2 for dataflash mavlink, 3 for both file and dataflash
|
||||
// @Values: 0:None,1:File,2:MAVLink,3:BothFileAndMAVLink
|
||||
// @Bitmask: 0:File,1:MAVLink,2:Block
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_BACKEND_TYPE", 0, AP_Logger, _params.backend_types, DATAFLASH_BACKEND_FILE),
|
||||
|
||||
|
|
@ -97,8 +100,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
|||
|
||||
#if defined(HAL_BOARD_LOG_DIRECTORY)
|
||||
#if HAL_OS_POSIX_IO || HAL_OS_FATFS_IO
|
||||
if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
|
||||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
|
||||
if (_params.backend_types & DATAFLASH_BACKEND_FILE) {
|
||||
LoggerMessageWriter_DFLogStart *message_writer =
|
||||
new LoggerMessageWriter_DFLogStart();
|
||||
if (message_writer != nullptr) {
|
||||
|
|
@ -112,34 +114,11 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
|||
_next_backend++;
|
||||
}
|
||||
}
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
|
||||
|
||||
if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
|
||||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
|
||||
|
||||
LoggerMessageWriter_DFLogStart *message_writer =
|
||||
new LoggerMessageWriter_DFLogStart();
|
||||
if (message_writer != nullptr) {
|
||||
|
||||
#if defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)
|
||||
backends[_next_backend] = new AP_Logger_File(*this, message_writer, HAL_BOARD_LOG_DIRECTORY);
|
||||
#else
|
||||
backends[_next_backend] = new AP_Logger_Revo(*this, message_writer); // restore dataflash logs
|
||||
#endif
|
||||
}
|
||||
|
||||
if (backends[_next_backend] == nullptr) {
|
||||
printf("Unable to open AP_Logger_Revo");
|
||||
} else {
|
||||
_next_backend++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif // HAL_BOARD_LOG_DIRECTORY
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK ||
|
||||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
|
||||
if (_params.backend_types & DATAFLASH_BACKEND_MAVLINK) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
|
|
@ -158,6 +137,44 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (_params.backend_types & DATAFLASH_BACKEND_BLOCK) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
LoggerMessageWriter_DFLogStart *message_writer =
|
||||
new LoggerMessageWriter_DFLogStart();
|
||||
if (message_writer != nullptr) {
|
||||
backends[_next_backend] = new AP_Logger_SITL(*this, message_writer);
|
||||
}
|
||||
if (backends[_next_backend] == nullptr) {
|
||||
hal.console->printf("Unable to open AP_Logger_SITL");
|
||||
} else {
|
||||
_next_backend++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef HAL_LOGGING_DATAFLASH
|
||||
if (_params.backend_types & DATAFLASH_BACKEND_BLOCK) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
LoggerMessageWriter_DFLogStart *message_writer =
|
||||
new LoggerMessageWriter_DFLogStart();
|
||||
if (message_writer != nullptr) {
|
||||
backends[_next_backend] = new AP_Logger_DataFlash(*this, message_writer);
|
||||
}
|
||||
if (backends[_next_backend] == nullptr) {
|
||||
hal.console->printf("Unable to open AP_Logger_DataFlash");
|
||||
} else {
|
||||
_next_backend++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
for (uint8_t i=0; i<_next_backend; i++) {
|
||||
backends[i]->Init();
|
||||
}
|
||||
|
|
@ -556,7 +573,7 @@ uint16_t AP_Logger::find_last_log() const {
|
|||
}
|
||||
return backends[0]->find_last_log();
|
||||
}
|
||||
void AP_Logger::get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) {
|
||||
void AP_Logger::get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) {
|
||||
if (_next_backend == 0) {
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,10 +31,10 @@
|
|||
class AP_Logger_Backend;
|
||||
|
||||
enum AP_Logger_Backend_Type {
|
||||
DATAFLASH_BACKEND_NONE = 0,
|
||||
DATAFLASH_BACKEND_FILE = 1,
|
||||
DATAFLASH_BACKEND_MAVLINK = 2,
|
||||
DATAFLASH_BACKEND_BOTH = 3,
|
||||
DATAFLASH_BACKEND_NONE = 0,
|
||||
DATAFLASH_BACKEND_FILE = (1<<0),
|
||||
DATAFLASH_BACKEND_MAVLINK = (1<<1),
|
||||
DATAFLASH_BACKEND_BLOCK = (1<<2),
|
||||
};
|
||||
|
||||
// fwd declarations to avoid include errors
|
||||
|
|
@ -75,7 +75,7 @@ public:
|
|||
|
||||
// high level interface
|
||||
uint16_t find_last_log() const;
|
||||
void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page);
|
||||
void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page);
|
||||
uint16_t get_num_logs(void);
|
||||
|
||||
void setVehicle_Startup_Writer(vehicle_startup_message_Writer writer);
|
||||
|
|
@ -363,7 +363,7 @@ private:
|
|||
uint32_t _log_data_remaining;
|
||||
|
||||
// start page of log data
|
||||
uint16_t _log_data_page;
|
||||
uint32_t _log_data_page;
|
||||
|
||||
GCS_MAVLINK *_log_sending_link;
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ public:
|
|||
|
||||
// high level interface
|
||||
virtual uint16_t find_last_log() = 0;
|
||||
virtual void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) = 0;
|
||||
virtual void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) = 0;
|
||||
virtual void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) = 0;
|
||||
virtual int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) = 0;
|
||||
virtual uint16_t get_num_logs() = 0;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,626 @@
|
|||
/*
|
||||
block based logging, for boards with flash logging
|
||||
*/
|
||||
|
||||
#include "AP_Logger_Block.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <stdio.h>
|
||||
|
||||
extern AP_HAL::HAL& hal;
|
||||
|
||||
// the last page holds the log format in first 4 bytes. Please change
|
||||
// this if (and only if!) the low level format changes
|
||||
#define DF_LOGGING_FORMAT 0x19012019
|
||||
|
||||
AP_Logger_Block::AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
|
||||
writebuf(0),
|
||||
AP_Logger_Backend(front, writer)
|
||||
{
|
||||
buffer = (uint8_t *)hal.util->malloc_type(page_size_max, AP_HAL::Util::MEM_DMA_SAFE);
|
||||
if (buffer == nullptr) {
|
||||
AP_HAL::panic("Out of DMA memory for logging");
|
||||
}
|
||||
}
|
||||
|
||||
// init is called after backend init
|
||||
void AP_Logger_Block::Init(void)
|
||||
{
|
||||
if (CardInserted()) {
|
||||
// reserve space for version in last sector
|
||||
df_NumPages -= df_PagePerSector;
|
||||
|
||||
// determine and limit file backend buffersize
|
||||
uint32_t bufsize = _front._params.file_bufsize;
|
||||
if (bufsize > 64) {
|
||||
bufsize = 64;
|
||||
}
|
||||
bufsize *= 1024;
|
||||
|
||||
// If we can't allocate the full size, try to reduce it until we can allocate it
|
||||
while (!writebuf.set_size(bufsize) && bufsize >= df_PageSize * df_PagePerSector) {
|
||||
hal.console->printf("AP_Logger_Block: Couldn't set buffer size to=%u\n", (unsigned)bufsize);
|
||||
bufsize >>= 1;
|
||||
}
|
||||
|
||||
if (!writebuf.get_size()) {
|
||||
hal.console->printf("Out of memory for logging\n");
|
||||
return;
|
||||
}
|
||||
|
||||
hal.console->printf("AP_Logger_Block: buffer size=%u\n", (unsigned)bufsize);
|
||||
_initialised = true;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Logger_Block::io_timer, void));
|
||||
|
||||
AP_Logger_Backend::Init();
|
||||
}
|
||||
|
||||
uint32_t AP_Logger_Block::bufferspace_available()
|
||||
{
|
||||
// because AP_Logger_Block devices are ring buffers, we *always*
|
||||
// have room...
|
||||
return df_NumPages * df_PageSize;
|
||||
}
|
||||
|
||||
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
||||
void AP_Logger_Block::StartWrite(uint32_t PageAdr)
|
||||
{
|
||||
df_PageAdr = PageAdr;
|
||||
log_write_started = true;
|
||||
}
|
||||
|
||||
void AP_Logger_Block::FinishWrite(void)
|
||||
{
|
||||
// Write Buffer to flash
|
||||
BufferToPage(df_PageAdr);
|
||||
df_PageAdr++;
|
||||
|
||||
// If we reach the end of the memory, start from the beginning
|
||||
if (df_PageAdr > df_NumPages) {
|
||||
df_PageAdr = 1;
|
||||
}
|
||||
|
||||
// when starting a new sector, erase it
|
||||
if ((df_PageAdr-1) % df_PagePerSector == 0) {
|
||||
SectorErase(df_PageAdr / df_PagePerSector);
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_Logger_Block::WritesOK() const
|
||||
{
|
||||
if (!CardInserted()) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
|
||||
{
|
||||
// is_critical is ignored - we're a ring buffer and never run out
|
||||
// of space. possibly if we do more complicated bandwidth
|
||||
// limiting we can reserve bandwidth based on is_critical
|
||||
if (!WritesOK()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (! WriteBlockCheckStartupMessages()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (writebuf.space() < size) {
|
||||
// no room in buffer
|
||||
return false;
|
||||
}
|
||||
|
||||
writebuf.write((uint8_t*)pBuffer, size);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_Block::StartRead(uint32_t PageAdr)
|
||||
{
|
||||
df_Read_PageAdr = PageAdr;
|
||||
|
||||
// copy flash page to buffer
|
||||
if (erase_started) {
|
||||
memset(buffer, 0xff, df_PageSize);
|
||||
} else {
|
||||
PageToBuffer(df_Read_PageAdr);
|
||||
}
|
||||
|
||||
// We are starting a new page - read FileNumber and FilePage
|
||||
struct PageHeader ph;
|
||||
BlockRead(0, &ph, sizeof(ph));
|
||||
df_FileNumber = ph.FileNumber;
|
||||
df_FilePage = ph.FilePage;
|
||||
df_Read_BufferIdx = sizeof(ph);
|
||||
}
|
||||
|
||||
bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size)
|
||||
{
|
||||
if (erase_started) {
|
||||
return false;
|
||||
}
|
||||
while (size > 0) {
|
||||
uint16_t n = df_PageSize - df_Read_BufferIdx;
|
||||
if (n > size) {
|
||||
n = size;
|
||||
}
|
||||
|
||||
if (!BlockRead(df_Read_BufferIdx, pBuffer, n)) {
|
||||
return false;
|
||||
}
|
||||
size -= n;
|
||||
pBuffer = (void *)(n + (uintptr_t)pBuffer);
|
||||
|
||||
df_Read_BufferIdx += n;
|
||||
|
||||
if (df_Read_BufferIdx == df_PageSize) {
|
||||
df_Read_PageAdr++;
|
||||
if (df_Read_PageAdr > df_NumPages) {
|
||||
df_Read_PageAdr = 1;
|
||||
}
|
||||
if (erase_started) {
|
||||
memset(buffer, 0xff, df_PageSize);
|
||||
} else {
|
||||
PageToBuffer(df_Read_PageAdr);
|
||||
}
|
||||
|
||||
// We are starting a new page - read FileNumber and FilePage
|
||||
struct PageHeader ph;
|
||||
if (!BlockRead(0, &ph, sizeof(ph))) {
|
||||
return false;
|
||||
}
|
||||
df_FileNumber = ph.FileNumber;
|
||||
df_FilePage = ph.FilePage;
|
||||
|
||||
df_Read_BufferIdx = sizeof(ph);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_Logger_Block::SetFileNumber(uint16_t FileNumber)
|
||||
{
|
||||
df_FileNumber = FileNumber;
|
||||
df_FilePage = 1;
|
||||
}
|
||||
|
||||
uint16_t AP_Logger_Block::GetFileNumber()
|
||||
{
|
||||
return df_FileNumber;
|
||||
}
|
||||
|
||||
void AP_Logger_Block::EraseAll()
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
if (erase_started) {
|
||||
// already erasing
|
||||
return;
|
||||
}
|
||||
|
||||
log_write_started = false;
|
||||
|
||||
StartErase();
|
||||
erase_started = true;
|
||||
}
|
||||
|
||||
bool AP_Logger_Block::NeedPrep(void)
|
||||
{
|
||||
return NeedErase();
|
||||
}
|
||||
|
||||
void AP_Logger_Block::Prep()
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
if (hal.util->get_soft_armed()) {
|
||||
// do not want to do any filesystem operations while we are e.g. flying
|
||||
return;
|
||||
}
|
||||
if (NeedErase()) {
|
||||
EraseAll();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* we need to erase if the logging format has changed
|
||||
*/
|
||||
bool AP_Logger_Block::NeedErase(void)
|
||||
{
|
||||
uint32_t version = 0;
|
||||
StartRead(df_NumPages+1); // last page
|
||||
|
||||
BlockRead(0, &version, sizeof(version));
|
||||
StartRead(1);
|
||||
if (version == DF_LOGGING_FORMAT) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
get raw data from a log
|
||||
*/
|
||||
int16_t AP_Logger_Block::get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
|
||||
|
||||
if (offset >= data_page_size) {
|
||||
page += offset / data_page_size;
|
||||
offset = offset % data_page_size;
|
||||
if (page > df_NumPages) {
|
||||
// pages are one based, not zero
|
||||
page = 1 + page - df_NumPages;
|
||||
}
|
||||
}
|
||||
if (log_write_started || df_Read_PageAdr != page) {
|
||||
StartRead(page);
|
||||
}
|
||||
|
||||
df_Read_BufferIdx = offset + sizeof(struct PageHeader);
|
||||
if (!ReadBlock(data, len)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return (int16_t)len;
|
||||
}
|
||||
|
||||
/**
|
||||
get data from a log, accounting for adding FMT headers
|
||||
*/
|
||||
int16_t AP_Logger_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
if (offset == 0) {
|
||||
uint8_t header[3];
|
||||
get_log_data_raw(log_num, page, 0, 3, header);
|
||||
adding_fmt_headers = (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2 || header[2] != LOG_FORMAT_MSG);
|
||||
}
|
||||
uint16_t ret = 0;
|
||||
|
||||
if (adding_fmt_headers) {
|
||||
// the log doesn't start with a FMT message, we need to add
|
||||
// them
|
||||
const uint16_t fmt_header_size = num_types() * sizeof(struct log_Format);
|
||||
while (offset < fmt_header_size && len > 0) {
|
||||
struct log_Format pkt;
|
||||
uint8_t t = offset / sizeof(pkt);
|
||||
uint8_t ofs = offset % sizeof(pkt);
|
||||
Fill_Format(structure(t), pkt);
|
||||
uint8_t n = sizeof(pkt) - ofs;
|
||||
if (n > len) {
|
||||
n = len;
|
||||
}
|
||||
memcpy(data, ofs + (uint8_t *)&pkt, n);
|
||||
data += n;
|
||||
offset += n;
|
||||
len -= n;
|
||||
ret += n;
|
||||
}
|
||||
offset -= fmt_header_size;
|
||||
}
|
||||
|
||||
if (len > 0) {
|
||||
ret += get_log_data_raw(log_num, page, offset, len, data);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
// This function determines the number of whole or partial log files in the AP_Logger
|
||||
// Wholly overwritten files are (of course) lost.
|
||||
uint16_t AP_Logger_Block::get_num_logs(void)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
uint32_t lastpage;
|
||||
uint32_t last;
|
||||
uint32_t first;
|
||||
|
||||
if (!CardInserted() || find_last_page() == 1) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
StartRead(1);
|
||||
|
||||
if (GetFileNumber() == 0xFFFF) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
lastpage = find_last_page();
|
||||
StartRead(lastpage);
|
||||
last = GetFileNumber();
|
||||
StartRead(lastpage + 2);
|
||||
if (GetFileNumber() == 0xFFFF) {
|
||||
StartRead(((lastpage >> 8) + 1) << 8); // next sector
|
||||
}
|
||||
first = GetFileNumber();
|
||||
if (first > last) {
|
||||
StartRead(1);
|
||||
first = GetFileNumber();
|
||||
}
|
||||
|
||||
if (last == first) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
return (last - first + 1);
|
||||
}
|
||||
|
||||
|
||||
// This function starts a new log file in the AP_Logger
|
||||
uint16_t AP_Logger_Block::start_new_log(void)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
uint32_t last_page = find_last_page();
|
||||
|
||||
StartRead(last_page);
|
||||
|
||||
if (find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
|
||||
SetFileNumber(1);
|
||||
StartWrite(1);
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint16_t new_log_num;
|
||||
|
||||
// Check for log of length 1 page and suppress
|
||||
if (df_FilePage <= 1) {
|
||||
new_log_num = GetFileNumber();
|
||||
// Last log too short, reuse its number
|
||||
// and overwrite it
|
||||
SetFileNumber(new_log_num);
|
||||
StartWrite(last_page);
|
||||
} else {
|
||||
new_log_num = GetFileNumber()+1;
|
||||
if (last_page == 0xFFFF) {
|
||||
last_page=0;
|
||||
}
|
||||
SetFileNumber(new_log_num);
|
||||
StartWrite(last_page + 1);
|
||||
}
|
||||
return new_log_num;
|
||||
}
|
||||
|
||||
// This function finds the first and last pages of a log file
|
||||
// The first page may be greater than the last page if the AP_Logger has been filled and partially overwritten.
|
||||
void AP_Logger_Block::get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
uint16_t num = get_num_logs();
|
||||
uint32_t look;
|
||||
|
||||
if (num == 1) {
|
||||
StartRead(df_NumPages);
|
||||
if (GetFileNumber() == 0xFFFF) {
|
||||
start_page = 1;
|
||||
end_page = find_last_page_of_log((uint16_t)log_num);
|
||||
} else {
|
||||
end_page = find_last_page_of_log((uint16_t)log_num);
|
||||
start_page = end_page + 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (log_num==1) {
|
||||
StartRead(df_NumPages);
|
||||
if (GetFileNumber() == 0xFFFF) {
|
||||
start_page = 1;
|
||||
} else {
|
||||
start_page = find_last_page() + 1;
|
||||
}
|
||||
} else {
|
||||
if (log_num == find_last_log() - num + 1) {
|
||||
start_page = find_last_page() + 1;
|
||||
} else {
|
||||
look = log_num-1;
|
||||
do {
|
||||
start_page = find_last_page_of_log(look) + 1;
|
||||
look--;
|
||||
} while (start_page <= 0 && look >=1);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (start_page == df_NumPages+1 || start_page == 0) {
|
||||
start_page = 1;
|
||||
}
|
||||
end_page = find_last_page_of_log(log_num);
|
||||
if (end_page == 0) {
|
||||
end_page = start_page;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool AP_Logger_Block::check_wrapped(void)
|
||||
{
|
||||
StartRead(df_NumPages);
|
||||
return GetFileNumber() != 0xFFFF;
|
||||
}
|
||||
|
||||
|
||||
// This funciton finds the last log number
|
||||
uint16_t AP_Logger_Block::find_last_log(void)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
uint32_t last_page = find_last_page();
|
||||
StartRead(last_page);
|
||||
return GetFileNumber();
|
||||
}
|
||||
|
||||
// This function finds the last page of the last file
|
||||
uint32_t AP_Logger_Block::find_last_page(void)
|
||||
{
|
||||
uint32_t look;
|
||||
uint32_t bottom = 1;
|
||||
uint32_t top = df_NumPages;
|
||||
uint32_t look_hash;
|
||||
uint32_t bottom_hash;
|
||||
uint32_t top_hash;
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
StartRead(bottom);
|
||||
bottom_hash = ((int32_t)GetFileNumber()<<16) | df_FilePage;
|
||||
|
||||
while (top-bottom > 1) {
|
||||
look = (top+bottom)/2;
|
||||
StartRead(look);
|
||||
look_hash = (int32_t)GetFileNumber()<<16 | df_FilePage;
|
||||
if (look_hash >= 0xFFFF0000) {
|
||||
look_hash = 0;
|
||||
}
|
||||
|
||||
if (look_hash < bottom_hash) {
|
||||
// move down
|
||||
top = look;
|
||||
} else {
|
||||
// move up
|
||||
bottom = look;
|
||||
bottom_hash = look_hash;
|
||||
}
|
||||
}
|
||||
|
||||
StartRead(top);
|
||||
top_hash = ((int32_t)GetFileNumber()<<16) | df_FilePage;
|
||||
if (top_hash >= 0xFFFF0000) {
|
||||
top_hash = 0;
|
||||
}
|
||||
if (top_hash > bottom_hash) {
|
||||
return top;
|
||||
}
|
||||
|
||||
return bottom;
|
||||
}
|
||||
|
||||
// This function finds the last page of a particular log file
|
||||
uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number)
|
||||
{
|
||||
uint32_t look;
|
||||
uint32_t bottom;
|
||||
uint32_t top;
|
||||
uint32_t look_hash;
|
||||
uint32_t check_hash;
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
if (check_wrapped()) {
|
||||
StartRead(1);
|
||||
bottom = GetFileNumber();
|
||||
if (bottom > log_number) {
|
||||
bottom = find_last_page();
|
||||
top = df_NumPages;
|
||||
} else {
|
||||
bottom = 1;
|
||||
top = find_last_page();
|
||||
}
|
||||
} else {
|
||||
bottom = 1;
|
||||
top = find_last_page();
|
||||
}
|
||||
|
||||
check_hash = (int32_t)log_number<<16 | 0xFFFF;
|
||||
|
||||
while (top-bottom > 1) {
|
||||
look = (top+bottom)/2;
|
||||
StartRead(look);
|
||||
look_hash = (int32_t)GetFileNumber()<<16 | df_FilePage;
|
||||
if (look_hash >= 0xFFFF0000) {
|
||||
look_hash = 0;
|
||||
}
|
||||
|
||||
if (look_hash > check_hash) {
|
||||
// move down
|
||||
top = look;
|
||||
} else {
|
||||
// move up
|
||||
bottom = look;
|
||||
}
|
||||
}
|
||||
|
||||
StartRead(top);
|
||||
if (GetFileNumber() == log_number) {
|
||||
return top;
|
||||
}
|
||||
|
||||
StartRead(bottom);
|
||||
if (GetFileNumber() == log_number) {
|
||||
return bottom;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_Block::get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc)
|
||||
{
|
||||
uint32_t start, end;
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
get_log_boundaries(log_num, start, end);
|
||||
if (end >= start) {
|
||||
size = (end + 1 - start) * (uint32_t)df_PageSize;
|
||||
} else {
|
||||
size = (df_NumPages + end - start) * (uint32_t)df_PageSize;
|
||||
}
|
||||
time_utc = 0;
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_Block::PrepForArming()
|
||||
{
|
||||
if (logging_started()) {
|
||||
return;
|
||||
}
|
||||
start_new_log();
|
||||
}
|
||||
|
||||
// read size bytes of data from the buffer
|
||||
bool AP_Logger_Block::BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t size)
|
||||
{
|
||||
memcpy(pBuffer, &buffer[IntPageAdr], size);
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
IO timer running on IO thread
|
||||
*/
|
||||
void AP_Logger_Block::io_timer(void)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (erase_started) {
|
||||
if (InErase()) {
|
||||
return;
|
||||
}
|
||||
// write the logging format in the last page
|
||||
StartWrite(df_NumPages+1);
|
||||
uint32_t version = DF_LOGGING_FORMAT;
|
||||
memset(buffer, 0, df_PageSize);
|
||||
memcpy(buffer, &version, sizeof(version));
|
||||
FinishWrite();
|
||||
erase_started = false;
|
||||
}
|
||||
|
||||
if (!CardInserted() || !log_write_started) {
|
||||
return;
|
||||
}
|
||||
|
||||
while (writebuf.available() >= df_PageSize - sizeof(struct PageHeader)) {
|
||||
WITH_SEMAPHORE(sem);
|
||||
struct PageHeader ph;
|
||||
ph.FileNumber = df_FileNumber;
|
||||
ph.FilePage = df_FilePage;
|
||||
memcpy(buffer, &ph, sizeof(ph));
|
||||
writebuf.read(&buffer[sizeof(ph)], df_PageSize - sizeof(ph));
|
||||
FinishWrite();
|
||||
df_FilePage++;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
/*
|
||||
AP_Logger logging - block oriented variant
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_Logger_Backend.h"
|
||||
|
||||
class AP_Logger_Block : public AP_Logger_Backend {
|
||||
public:
|
||||
AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer);
|
||||
|
||||
virtual void Init(void) override;
|
||||
virtual bool CardInserted(void) const override = 0;
|
||||
|
||||
// erase handling
|
||||
void EraseAll() override;
|
||||
|
||||
bool NeedPrep(void) override;
|
||||
void Prep() override;
|
||||
void PrepForArming() override;
|
||||
|
||||
// high level interface
|
||||
uint16_t find_last_log() override;
|
||||
void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) override;
|
||||
void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override;
|
||||
int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override;
|
||||
uint16_t get_num_logs() override;
|
||||
uint16_t start_new_log(void) override;
|
||||
uint32_t bufferspace_available() override;
|
||||
void stop_logging(void) override { log_write_started = false; }
|
||||
bool logging_enabled() const override { return true; }
|
||||
bool logging_failed() const override { return false; }
|
||||
bool logging_started(void) const override { return log_write_started; }
|
||||
|
||||
protected:
|
||||
/* Write a block of data at current offset */
|
||||
bool _WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) override;
|
||||
|
||||
private:
|
||||
/*
|
||||
functions implemented by the board specific backends
|
||||
*/
|
||||
virtual void BufferToPage(uint32_t PageAdr) = 0;
|
||||
virtual void PageToBuffer(uint32_t PageAdr) = 0;
|
||||
virtual void SectorErase(uint32_t PageAdr) = 0;
|
||||
virtual void StartErase() = 0;
|
||||
virtual bool InErase() = 0;
|
||||
|
||||
struct PageHeader {
|
||||
uint16_t FileNumber;
|
||||
uint16_t FilePage;
|
||||
};
|
||||
|
||||
HAL_Semaphore_Recursive sem;
|
||||
ByteBuffer writebuf;
|
||||
|
||||
// state variables
|
||||
uint16_t df_Read_BufferIdx;
|
||||
uint32_t df_PageAdr;
|
||||
uint32_t df_Read_PageAdr;
|
||||
uint16_t df_FileNumber;
|
||||
uint32_t df_FilePage;
|
||||
|
||||
// offset from adding FMT messages to log data
|
||||
bool adding_fmt_headers;
|
||||
|
||||
// are we waiting on an erase to finish?
|
||||
bool erase_started;
|
||||
|
||||
// read size bytes of data to a page. The caller must ensure that
|
||||
// the data fits within the page, otherwise it will wrap to the
|
||||
// start of the page
|
||||
bool BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t size);
|
||||
|
||||
// erase handling
|
||||
bool NeedErase(void);
|
||||
|
||||
// internal high level functions
|
||||
int16_t get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data);
|
||||
void StartRead(uint32_t PageAdr);
|
||||
uint32_t find_last_page(void);
|
||||
uint32_t find_last_page_of_log(uint16_t log_number);
|
||||
bool check_wrapped(void);
|
||||
void StartWrite(uint32_t PageAdr);
|
||||
void FinishWrite(void);
|
||||
|
||||
// Read methods
|
||||
bool ReadBlock(void *pBuffer, uint16_t size);
|
||||
|
||||
// file numbers
|
||||
void SetFileNumber(uint16_t FileNumber);
|
||||
uint16_t GetFileNumber();
|
||||
|
||||
void _print_log_formats(AP_HAL::BetterStream *port);
|
||||
|
||||
// callback on IO thread
|
||||
void io_timer(void);
|
||||
|
||||
protected:
|
||||
// page handling
|
||||
uint32_t df_PageSize;
|
||||
uint16_t df_PagePerSector;
|
||||
uint32_t df_NumPages;
|
||||
bool log_write_started;
|
||||
|
||||
static const uint16_t page_size_max = 256;
|
||||
uint8_t *buffer;
|
||||
|
||||
bool WritesOK() const override;
|
||||
};
|
||||
|
|
@ -0,0 +1,303 @@
|
|||
/*
|
||||
logging to a DataFlash block based storage device on SPI
|
||||
*/
|
||||
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#ifdef HAL_LOGGING_DATAFLASH
|
||||
|
||||
#include "AP_Logger_DataFlash.h"
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define JEDEC_WRITE_ENABLE 0x06
|
||||
#define JEDEC_WRITE_DISABLE 0x04
|
||||
#define JEDEC_READ_STATUS 0x05
|
||||
#define JEDEC_WRITE_STATUS 0x01
|
||||
#define JEDEC_READ_DATA 0x03
|
||||
#define JEDEC_FAST_READ 0x0b
|
||||
#define JEDEC_DEVICE_ID 0x9F
|
||||
#define JEDEC_PAGE_WRITE 0x02
|
||||
|
||||
#define JEDEC_BULK_ERASE 0xC7
|
||||
#define JEDEC_SECTOR4_ERASE 0x20 // 4k erase
|
||||
#define JEDEC_BLOCK32_ERASE 0x52 // 32K erase
|
||||
#define JEDEC_BLOCK64_ERASE 0xD8 // 64K erase
|
||||
|
||||
#define JEDEC_STATUS_BUSY 0x01
|
||||
#define JEDEC_STATUS_WRITEPROTECT 0x02
|
||||
#define JEDEC_STATUS_BP0 0x04
|
||||
#define JEDEC_STATUS_BP1 0x08
|
||||
#define JEDEC_STATUS_BP2 0x10
|
||||
#define JEDEC_STATUS_TP 0x20
|
||||
#define JEDEC_STATUS_SEC 0x40
|
||||
#define JEDEC_STATUS_SRP0 0x80
|
||||
|
||||
/*
|
||||
flash device IDs taken from betaflight flash_m25p16.c
|
||||
|
||||
Format is manufacturer, memory type, then capacity
|
||||
*/
|
||||
#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016
|
||||
#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017
|
||||
#define JEDEC_ID_MACRONIX_MX25L25635E 0xC22019
|
||||
#define JEDEC_ID_MICRON_M25P16 0x202015
|
||||
#define JEDEC_ID_MICRON_N25Q064 0x20BA17
|
||||
#define JEDEC_ID_MICRON_N25Q128 0x20ba18
|
||||
#define JEDEC_ID_WINBOND_W25Q16 0xEF4015
|
||||
#define JEDEC_ID_WINBOND_W25Q32 0xEF4016
|
||||
#define JEDEC_ID_WINBOND_W25Q64 0xEF4017
|
||||
#define JEDEC_ID_WINBOND_W25Q128 0xEF4018
|
||||
#define JEDEC_ID_WINBOND_W25Q256 0xEF4019
|
||||
#define JEDEC_ID_CYPRESS_S25FL128L 0x016018
|
||||
|
||||
void AP_Logger_DataFlash::Init()
|
||||
{
|
||||
dev = hal.spi->get_device("dataflash");
|
||||
if (!dev) {
|
||||
AP_HAL::panic("PANIC: AP_Logger SPIDeviceDriver not found");
|
||||
return;
|
||||
}
|
||||
|
||||
dev_sem = dev->get_semaphore();
|
||||
|
||||
if (!getSectorCount()) {
|
||||
flash_died = true;
|
||||
return;
|
||||
}
|
||||
|
||||
flash_died = false;
|
||||
|
||||
AP_Logger_Block::Init();
|
||||
|
||||
//flash_test();
|
||||
}
|
||||
|
||||
/*
|
||||
wait for busy flag to be cleared
|
||||
*/
|
||||
void AP_Logger_DataFlash::WaitReady()
|
||||
{
|
||||
if (flash_died) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t t = AP_HAL::millis();
|
||||
while (Busy()) {
|
||||
hal.scheduler->delay_microseconds(100);
|
||||
if (AP_HAL::millis() - t > 5000) {
|
||||
printf("DataFlash: flash_died\n");
|
||||
flash_died = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_Logger_DataFlash::getSectorCount(void)
|
||||
{
|
||||
WaitReady();
|
||||
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
|
||||
// Read manufacturer ID
|
||||
uint8_t cmd = JEDEC_DEVICE_ID;
|
||||
dev->transfer(&cmd, 1, buffer, 4);
|
||||
|
||||
uint32_t id = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
|
||||
|
||||
uint32_t sectors = 0;
|
||||
|
||||
switch (id) {
|
||||
case JEDEC_ID_WINBOND_W25Q16:
|
||||
case JEDEC_ID_MICRON_M25P16:
|
||||
sectors = 32;
|
||||
df_PagePerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_WINBOND_W25Q32:
|
||||
case JEDEC_ID_MACRONIX_MX25L3206E:
|
||||
sectors = 64;
|
||||
df_PagePerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_MICRON_N25Q064:
|
||||
case JEDEC_ID_WINBOND_W25Q64:
|
||||
case JEDEC_ID_MACRONIX_MX25L6406E:
|
||||
sectors = 128;
|
||||
df_PagePerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_MICRON_N25Q128:
|
||||
case JEDEC_ID_WINBOND_W25Q128:
|
||||
case JEDEC_ID_CYPRESS_S25FL128L:
|
||||
sectors = 256;
|
||||
df_PagePerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_WINBOND_W25Q256:
|
||||
case JEDEC_ID_MACRONIX_MX25L25635E:
|
||||
sectors = 512;
|
||||
df_PagePerSector = 256;
|
||||
use_32bit_address = true;
|
||||
break;
|
||||
default:
|
||||
hal.scheduler->delay(2000);
|
||||
printf("Unknown SPI Flash 0x%08x\n", id);
|
||||
return false;
|
||||
}
|
||||
|
||||
df_PageSize = 256;
|
||||
df_NumPages = sectors * df_PagePerSector;
|
||||
erase_cmd = JEDEC_BLOCK64_ERASE;
|
||||
|
||||
hal.scheduler->delay(2000);
|
||||
printf("SPI Flash 0x%08x found pages=%u erase=%uk\n",
|
||||
id, df_NumPages, (df_PagePerSector * (uint32_t)df_PageSize)/1024);
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
// Read the status register
|
||||
uint8_t AP_Logger_DataFlash::ReadStatusReg()
|
||||
{
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
uint8_t cmd = JEDEC_READ_STATUS;
|
||||
uint8_t status;
|
||||
dev->transfer(&cmd, 1, &status, 1);
|
||||
return status;
|
||||
}
|
||||
|
||||
bool AP_Logger_DataFlash::Busy()
|
||||
{
|
||||
int32_t status = ReadStatusReg();
|
||||
if (status < 0) {
|
||||
return true;
|
||||
}
|
||||
return (status & JEDEC_STATUS_BUSY) != 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a command with an address
|
||||
*/
|
||||
void AP_Logger_DataFlash::send_command_addr(uint8_t command, uint32_t PageAdr)
|
||||
{
|
||||
uint8_t cmd[5];
|
||||
cmd[0] = command;
|
||||
if (use_32bit_address) {
|
||||
cmd[1] = (PageAdr >> 24) & 0xff;
|
||||
cmd[2] = (PageAdr >> 16) & 0xff;
|
||||
cmd[3] = (PageAdr >> 8) & 0xff;
|
||||
cmd[4] = (PageAdr >> 0) & 0xff;
|
||||
} else {
|
||||
cmd[1] = (PageAdr >> 16) & 0xff;
|
||||
cmd[2] = (PageAdr >> 8) & 0xff;
|
||||
cmd[3] = (PageAdr >> 0) & 0xff;
|
||||
}
|
||||
|
||||
dev->transfer(cmd, use_32bit_address?5:4, NULL, 0);
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_DataFlash::PageToBuffer(uint32_t pageNum)
|
||||
{
|
||||
if (pageNum == 0 || pageNum > df_NumPages+1) {
|
||||
printf("Invalid page read %u\n", pageNum);
|
||||
memset(buffer, 0xFF, df_PageSize);
|
||||
return;
|
||||
}
|
||||
WaitReady();
|
||||
|
||||
uint32_t PageAdr = (pageNum-1) * df_PageSize;
|
||||
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
dev->set_chip_select(true);
|
||||
send_command_addr(JEDEC_READ_DATA, PageAdr);
|
||||
dev->transfer(NULL, 0, buffer, df_PageSize);
|
||||
dev->set_chip_select(false);
|
||||
}
|
||||
|
||||
void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum)
|
||||
{
|
||||
if (pageNum == 0 || pageNum > df_NumPages+1) {
|
||||
printf("Invalid page write %u\n", pageNum);
|
||||
return;
|
||||
}
|
||||
WriteEnable();
|
||||
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
|
||||
uint32_t PageAdr = (pageNum-1) * df_PageSize;
|
||||
|
||||
dev->set_chip_select(true);
|
||||
send_command_addr(JEDEC_PAGE_WRITE, PageAdr);
|
||||
dev->transfer(buffer, df_PageSize, NULL, 0);
|
||||
dev->set_chip_select(false);
|
||||
}
|
||||
|
||||
/*
|
||||
erase one sector (sizes varies with hw)
|
||||
*/
|
||||
void AP_Logger_DataFlash::SectorErase(uint32_t sectorNum)
|
||||
{
|
||||
WriteEnable();
|
||||
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
|
||||
uint32_t PageAdr = sectorNum * df_PageSize * df_PagePerSector;
|
||||
send_command_addr(erase_cmd, PageAdr);
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_DataFlash::StartErase()
|
||||
{
|
||||
WriteEnable();
|
||||
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
|
||||
uint8_t cmd = JEDEC_BULK_ERASE;
|
||||
dev->transfer(&cmd, 1, NULL, 0);
|
||||
|
||||
erase_start_ms = AP_HAL::millis();
|
||||
printf("Dataflash: erase started\n");
|
||||
}
|
||||
|
||||
bool AP_Logger_DataFlash::InErase()
|
||||
{
|
||||
if (erase_start_ms && !Busy()) {
|
||||
printf("Dataflash: erase done (%u ms)\n", AP_HAL::millis() - erase_start_ms);
|
||||
erase_start_ms = 0;
|
||||
}
|
||||
return erase_start_ms != 0;
|
||||
}
|
||||
|
||||
|
||||
void AP_Logger_DataFlash::WriteEnable(void)
|
||||
{
|
||||
WaitReady();
|
||||
WITH_SEMAPHORE(dev_sem);
|
||||
uint8_t b = JEDEC_WRITE_ENABLE;
|
||||
dev->transfer(&b, 1, NULL, 0);
|
||||
}
|
||||
|
||||
void AP_Logger_DataFlash::flash_test()
|
||||
{
|
||||
for (uint8_t i=1; i<=20; i++) {
|
||||
printf("Flash fill %u\n", i);
|
||||
if (i % df_PagePerSector == 0) {
|
||||
SectorErase(i / df_PagePerSector);
|
||||
}
|
||||
memset(buffer, i, df_PageSize);
|
||||
BufferToPage(i);
|
||||
}
|
||||
for (uint8_t i=1; i<=20; i++) {
|
||||
printf("Flash check %u\n", i);
|
||||
PageToBuffer(i);
|
||||
for (uint16_t j=0; j<df_PageSize; j++) {
|
||||
if (buffer[j] != i) {
|
||||
printf("Test error: page %u j=%u v=%u\n", i, j, buffer[j]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_LOGGING_DATAFLASH
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
logging for block based dataflash devices on SPI
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#ifdef HAL_LOGGING_DATAFLASH
|
||||
#include "AP_Logger_Backend.h"
|
||||
#include "AP_Logger_Block.h"
|
||||
|
||||
|
||||
class AP_Logger_DataFlash : public AP_Logger_Block {
|
||||
public:
|
||||
AP_Logger_DataFlash(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
|
||||
AP_Logger_Block(front, writer) {}
|
||||
void Init(void) override;
|
||||
bool CardInserted() const override { return !flash_died && df_NumPages > 0; }
|
||||
|
||||
private:
|
||||
void BufferToPage(uint32_t PageAdr) override;
|
||||
void PageToBuffer(uint32_t PageAdr) override;
|
||||
void SectorErase(uint32_t SectorAdr) override;
|
||||
void StartErase() override;
|
||||
bool InErase() override;
|
||||
void send_command_addr(uint8_t cmd, uint32_t address);
|
||||
void WaitReady();
|
||||
bool Busy();
|
||||
uint8_t ReadStatusReg();
|
||||
|
||||
void WriteEnable();
|
||||
bool getSectorCount(void);
|
||||
void flash_test(void);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev;
|
||||
AP_HAL::Semaphore *dev_sem;
|
||||
|
||||
bool flash_died;
|
||||
uint32_t erase_start_ms;
|
||||
uint8_t erase_cmd;
|
||||
bool use_32bit_address;
|
||||
};
|
||||
|
||||
#endif // HAL_LOGGING_DATAFLASH
|
||||
|
|
@ -655,7 +655,7 @@ uint16_t AP_Logger_File::_log_num_from_list_entry(const uint16_t list_entry)
|
|||
/*
|
||||
find the number of pages in a log
|
||||
*/
|
||||
void AP_Logger_File::get_log_boundaries(const uint16_t list_entry, uint16_t & start_page, uint16_t & end_page)
|
||||
void AP_Logger_File::get_log_boundaries(const uint16_t list_entry, uint32_t & start_page, uint32_t & end_page)
|
||||
{
|
||||
const uint16_t log_num = _log_num_from_list_entry(list_entry);
|
||||
if (log_num == 0) {
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ public:
|
|||
|
||||
// high level interface
|
||||
uint16_t find_last_log() override;
|
||||
void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) override;
|
||||
void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) override;
|
||||
void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override;
|
||||
int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override;
|
||||
uint16_t get_num_logs() override;
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ public:
|
|||
|
||||
// high level interface
|
||||
uint16_t find_last_log(void) override { return 0; }
|
||||
void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) override {}
|
||||
void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) override {}
|
||||
void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override {}
|
||||
int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override { return 0; }
|
||||
uint16_t get_num_logs(void) override { return 0; }
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms
|
|||
_log_num_data = packet.id;
|
||||
_log_data_size = size;
|
||||
|
||||
uint16_t end;
|
||||
uint32_t end;
|
||||
get_log_boundaries(packet.id, _log_data_page, end);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ void AP_Logger_Revo::StartWrite(uint16_t PageAdr)
|
|||
void AP_Logger_Revo::FinishWrite(void)
|
||||
{
|
||||
// Write Buffer to flash, NO WAIT
|
||||
BufferToPage(df_BufferNum, df_PageAdr, 0);
|
||||
BufferToPage(df_BufferNum, df_PageAdr);
|
||||
df_PageAdr++;
|
||||
// If we reach the end of the memory, start from the beginning
|
||||
if (df_PageAdr > df_NumPages) {
|
||||
|
|
@ -596,7 +596,7 @@ void AP_Logger_Revo::PageToBuffer(unsigned char BufferNum, uint16_t pageNum)
|
|||
cs_release();
|
||||
}
|
||||
|
||||
void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum, unsigned char wait)
|
||||
void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum)
|
||||
{
|
||||
uint32_t PageAdr = pageNum * DF_PAGE_SIZE;
|
||||
|
||||
|
|
@ -614,9 +614,6 @@ void AP_Logger_Revo::BufferToPage (unsigned char BufferNum, uint16_t pageNum, un
|
|||
_spi->transfer(buffer[BufferNum], DF_PAGE_SIZE, NULL, 0);
|
||||
|
||||
cs_release();
|
||||
|
||||
if(wait) WaitReady();
|
||||
|
||||
}
|
||||
|
||||
void AP_Logger_Revo::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr, unsigned char Data)
|
||||
|
|
|
|||
|
|
@ -48,13 +48,12 @@ class AP_Logger_Revo : public AP_Logger_Backend
|
|||
private:
|
||||
//Methods
|
||||
void BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data);
|
||||
void BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait);
|
||||
void BufferToPage (uint8_t BufferNum, uint16_t PageAdr);
|
||||
void PageToBuffer(uint8_t BufferNum, uint16_t PageAdr);
|
||||
void WaitReady();
|
||||
uint8_t ReadStatusReg();
|
||||
uint16_t PageSize() { return df_PageSize; }
|
||||
void PageErase (uint16_t PageAdr);
|
||||
void BlockErase (uint16_t BlockAdr);
|
||||
void ChipErase();
|
||||
|
||||
void Flash_Jedec_WriteEnable();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,99 @@
|
|||
/*
|
||||
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>
|
||||
|
||||
#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("dataflash.bin", O_RDWR|O_CLOEXEC, 0777);
|
||||
if (flash_fd == -1) {
|
||||
flash_fd = open("dataflash.bin", 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 dataflash.bin");
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
block based logging backend for SITL, simulating a flash storage
|
||||
chip
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
||||
#include "AP_Logger_Block.h"
|
||||
|
||||
class AP_Logger_SITL : public AP_Logger_Block {
|
||||
public:
|
||||
AP_Logger_SITL(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
|
||||
AP_Logger_Block(front, writer) {}
|
||||
void Init() override;
|
||||
bool CardInserted() const override;
|
||||
|
||||
private:
|
||||
void BufferToPage(uint32_t PageAdr) override;
|
||||
void PageToBuffer(uint32_t PageAdr) override;
|
||||
void SectorErase(uint32_t SectorAdr) override;
|
||||
void StartErase() override;
|
||||
bool InErase() override;
|
||||
|
||||
int flash_fd;
|
||||
uint32_t erase_started_ms;
|
||||
};
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
Loading…
Reference in New Issue