Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: support multiple simultaneous backends

Browse Source
This commit is contained in:
Peter Barker 2015-11-10 09:14:22 +11:00 committed by Andrew Tridgell
parent 121967d1b1
commit faabe0b83d
15 changed files with 345 additions and 302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/DataFlash/DFMessageWriter.cpp
View File

@ -1,7 +1,5 @@
#include "DFMessageWriter.h"
#include "DataFlash.h"
extern const AP_HAL::HAL& hal;
/* LogStartup - these are simple state machines which allow us to
@ -35,13 +33,13 @@ void DFMessageWriter_DFLogStart::process()
case ls_blockwriter_stage_formats:
// write log formats so the log is self-describing
while (next_format_to_send < _DataFlash._num_types) {
if (!_DataFlash.Log_Write_Format(&_DataFlash._structures[next_format_to_send])) {
while (next_format_to_send < _dataflash_backend->num_types()) {
if (!_dataflash_backend->Log_Write_Format(_dataflash_backend->structure(next_format_to_send))) {
return; // call me again!
}
// provide hook to avoid corrupting the APM1/APM2
// dataflash by writing too fast:
_DataFlash.WroteStartupFormat();
_dataflash_backend->WroteStartupFormat();
next_format_to_send++;
}
_fmt_done = true;
@ -50,11 +48,11 @@ void DFMessageWriter_DFLogStart::process()
case ls_blockwriter_stage_parms:
while (ap) {
if (!_DataFlash.Log_Write_Parameter(ap, token, type)) {
if (!_dataflash_backend->Log_Write_Parameter(ap, token, type)) {
return;
}
ap = AP_Param::next_scalar(&token, &type);
_DataFlash.WroteStartupParam();
_dataflash_backend->WroteStartupParam();
}
stage = ls_blockwriter_stage_sysinfo;
@ -80,11 +78,11 @@ void DFMessageWriter_DFLogStart::process()
// we guarantee 200 bytes of space for the vehicle startup
// messages. This allows them to be simple functions rather
// than e.g. DFMessageWriter-based state machines
if (_DataFlash._vehicle_messages) {
if (_DataFlash.bufferspace_available() < 200) {
if (_dataflash_backend->vehicle_message_writer()) {
if (_dataflash_backend->bufferspace_available() < 200) {
return;
}
_DataFlash._vehicle_messages();
(_dataflash_backend->vehicle_message_writer())();
}
stage = ls_blockwriter_stage_done;
// fall through
@ -116,7 +114,7 @@ void DFMessageWriter_WriteSysInfo::process() {
// fall through
case ws_blockwriter_stage_firmware_string:
if (! _DataFlash.Log_Write_Message(_firmware_string)) {
if (! _dataflash_backend->Log_Write_Message(_firmware_string)) {
return; // call me again
}
stage = ws_blockwriter_stage_git_versions;
@ -124,7 +122,7 @@ void DFMessageWriter_WriteSysInfo::process() {
case ws_blockwriter_stage_git_versions:
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
if (! _DataFlash.Log_Write_Message("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)) {
if (! _dataflash_backend->Log_Write_Message("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)) {
return; // call me again
}
#endif
@ -134,7 +132,7 @@ void DFMessageWriter_WriteSysInfo::process() {
case ws_blockwriter_stage_system_id:
char sysid[40];
if (hal.util->get_system_id(sysid)) {
if (! _DataFlash.Log_Write_Message(sysid)) {
if (! _dataflash_backend->Log_Write_Message(sysid)) {
return; // call me again
}
}
@ -163,7 +161,7 @@ void DFMessageWriter_WriteEntireMission::process() {
// fall through
case em_blockwriter_stage_write_new_mission_message:
if (! _DataFlash.Log_Write_Message("New mission")) {
if (! _dataflash_backend->Log_Write_Message("New mission")) {
return; // call me again
}
stage = em_blockwriter_stage_write_mission_items;
@ -175,7 +173,7 @@ void DFMessageWriter_WriteEntireMission::process() {
// upon failure to write the mission we will re-read from
// storage; this could be improved.
if (_mission->read_cmd_from_storage(_mission_number_to_send,cmd)) {
if (!_DataFlash.Log_Write_Mission_Cmd(*_mission, cmd)) {
if (!_dataflash_backend->Log_Write_Mission_Cmd(*_mission, cmd)) {
return; // call me again
}
}

40
libraries/DataFlash/DFMessageWriter.h
View File

@ -1,31 +1,31 @@
#ifndef DF_LOGSTARTUP_H
#define DF_LOGSTARTUP_H
#include <AP_HAL/AP_HAL.h>
#include <AP_Mission/AP_Mission.h>
#include "DataFlash_Backend.h"
class DataFlash_Class;
#include <AP_Mission/AP_Mission.h>
class DFMessageWriter {
public:
DFMessageWriter(DataFlash_Class &DataFlash) :
_DataFlash(DataFlash) { }
virtual void reset() = 0;
virtual void process() = 0;
virtual bool finished() { return _finished; };
virtual void set_dataflash_backend(class DataFlash_Backend *backend) {
_dataflash_backend = backend;
}
protected:
bool _finished = false;
DataFlash_Class &_DataFlash;
DataFlash_Backend *_dataflash_backend = nullptr;
};
class DFMessageWriter_WriteSysInfo : public DFMessageWriter {
public:
DFMessageWriter_WriteSysInfo(DataFlash_Class &DataFlash,
const char *firmware_string) :
DFMessageWriter(DataFlash),
DFMessageWriter_WriteSysInfo(const char *firmware_string) :
DFMessageWriter(),
_firmware_string(firmware_string)
{ }
@ -39,16 +39,14 @@ private:
ws_blockwriter_stage_git_versions,
ws_blockwriter_stage_system_id
};
write_sysinfo_blockwriter_stage stage;
write_sysinfo_blockwriter_stage stage = ws_blockwriter_stage_init;
const char *_firmware_string;
};
class DFMessageWriter_WriteEntireMission : public DFMessageWriter {
public:
DFMessageWriter_WriteEntireMission(DataFlash_Class &DataFlash) :
DFMessageWriter(DataFlash)
{}
void reset();
void process();
@ -69,14 +67,18 @@ private:
class DFMessageWriter_DFLogStart : public DFMessageWriter {
public:
DFMessageWriter_DFLogStart(DataFlash_Class &DataFlash,
const char *firmware_string) :
DFMessageWriter{DataFlash},
_writesysinfo(DataFlash, firmware_string),
_writeentiremission(DataFlash)
DFMessageWriter_DFLogStart(const char *firmware_string) :
_writesysinfo(firmware_string),
_writeentiremission()
{
}
virtual void set_dataflash_backend(class DataFlash_Backend *backend) {
DFMessageWriter::set_dataflash_backend(backend);
_writesysinfo.set_dataflash_backend(backend);
_writeentiremission.set_dataflash_backend(backend);
}
void reset();
void process();
bool fmt_done() { return _fmt_done; };
@ -97,7 +99,7 @@ private:
bool _fmt_done = false;
log_start_blockwriter_stage stage;
log_start_blockwriter_stage stage = ls_blockwriter_stage_init;
uint16_t next_format_to_send;
AP_Param::ParamToken token;

175
libraries/DataFlash/DataFlash.cpp
View File

@ -2,113 +2,206 @@
#include "DataFlash_Backend.h"
const AP_Param::GroupInfo DataFlash_Class::var_info[] = {
// @Param: _BACKEND_TYPES
// @DisplayName: DataFlash Backend Storage type
// @Description: 0 for None, 1 for File
// @Values: 0:None,1:File
// @User: Standard
AP_GROUPINFO("_BACKEND_TYPE", 0, DataFlash_Class, _params.backend_types, DATAFLASH_BACKEND_FILE),
AP_GROUPEND
};
const struct LogStructure *DataFlash_Class::structure(uint16_t num) const
{
return &_structures[num];
}
#define FOR_EACH_BACKEND(methodcall) \
do { \
for (uint8_t i=0; i<_next_backend; i++) { \
backends[i]->methodcall; \
} \
} while (0)
void DataFlash_Class::setVehicle_Startup_Log_Writer(vehicle_startup_message_Log_Writer writer)
{
_vehicle_messages = writer;
}
void DataFlash_Class::set_mission(const AP_Mission *mission) {
_startup_messagewriter.set_mission(mission);
FOR_EACH_BACKEND(set_mission(mission));
}
// start functions pass straight through to backend:
bool DataFlash_Class::WriteBlock(const void *pBuffer, uint16_t size) {
return backend->WriteBlock(pBuffer, size);
void DataFlash_Class::WriteBlock(const void *pBuffer, uint16_t size) {
FOR_EACH_BACKEND(WriteBlock(pBuffer, size));
}
bool DataFlash_Class::WriteCriticalBlock(const void *pBuffer, uint16_t size) {
return backend->WriteCriticalBlock(pBuffer, size);
void DataFlash_Class::WriteCriticalBlock(const void *pBuffer, uint16_t size) {
FOR_EACH_BACKEND(WriteCriticalBlock(pBuffer, size));
}
bool DataFlash_Class::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) {
return backend->WritePrioritisedBlock(pBuffer, size, is_critical);
}
uint16_t DataFlash_Class::start_new_log() {
return backend->start_new_log();
void DataFlash_Class::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) {
FOR_EACH_BACKEND(WritePrioritisedBlock(pBuffer, size, is_critical));
}
// change me to "DoTimeConsumingPreparations"?
void DataFlash_Class::EraseAll() {
backend->EraseAll();
FOR_EACH_BACKEND(EraseAll());
}
// change me to "LoggingAvailable"?
bool DataFlash_Class::CardInserted(void) {
return backend->CardInserted();
for (uint8_t i=0; i< _next_backend; i++) {
if (backends[i]->CardInserted()) {
return true;
}
}
return false;
}
bool DataFlash_Class::NeedPrep() {
return backend->NeedPrep();
for (uint8_t i=0; i< _next_backend; i++) {
if (backends[i]->NeedPrep()) {
return true;
}
}
return false;
}
void DataFlash_Class::Prep() {
backend->Prep();
}
uint16_t DataFlash_Class::bufferspace_available(void) {
return backend->bufferspace_available();
FOR_EACH_BACKEND(Prep());
}
uint16_t DataFlash_Class::find_last_log() const {
return backend->find_last_log();
if (_next_backend == 0) {
return 0;
}
return backends[0]->find_last_log();
}
void DataFlash_Class::get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) {
backend->get_log_boundaries(log_num, start_page, end_page);
if (_next_backend == 0) {
return;
}
backends[0]->get_log_boundaries(log_num, start_page, end_page);
}
void DataFlash_Class::get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) {
backend->get_log_info(log_num, size, time_utc);
if (_next_backend == 0) {
return;
}
backends[0]->get_log_info(log_num, size, time_utc);
}
int16_t DataFlash_Class::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) {
return backend->get_log_data(log_num, page, offset, len, data);
if (_next_backend == 0) {
return 0;
}
return backends[0]->get_log_data(log_num, page, offset, len, data);
}
uint16_t DataFlash_Class::get_num_logs(void) {
return backend->get_num_logs();
if (_next_backend == 0) {
return 0;
}
return backends[0]->get_num_logs();
}
void DataFlash_Class::Log_Fill_Format(const struct LogStructure *s, struct log_Format &pkt) {
backend->Log_Fill_Format(s, pkt);
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->Log_Fill_Format(s, pkt);
}
void DataFlash_Class::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
print_mode_fn printMode,
AP_HAL::BetterStream *port) {
backend->LogReadProcess(log_num, start_page, end_page, printMode, port);
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->LogReadProcess(log_num, start_page, end_page, printMode, port);
}
void DataFlash_Class::DumpPageInfo(AP_HAL::BetterStream *port) {
backend->DumpPageInfo(port);
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->DumpPageInfo(port);
}
void DataFlash_Class::ShowDeviceInfo(AP_HAL::BetterStream *port) {
backend->ShowDeviceInfo(port);
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->ShowDeviceInfo(port);
}
void DataFlash_Class::ListAvailableLogs(AP_HAL::BetterStream *port) {
backend->ListAvailableLogs(port);
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->ListAvailableLogs(port);
}
/* we're started if any of the backends are started */
bool DataFlash_Class::logging_started(void) {
return backend->log_write_started;
for (uint8_t i=0; i< _next_backend; i++) {
if (backends[i]->logging_started()) {
return true;
}
}
return false;
}
void DataFlash_Class::EnableWrites(bool enable) {
backend->EnableWrites(enable);
FOR_EACH_BACKEND(EnableWrites(enable));
}
void DataFlash_Class::periodic_tasks() {
backend->periodic_tasks();
}
void DataFlash_Class::WroteStartupFormat() {
backend->WroteStartupFormat();
}
void DataFlash_Class::WroteStartupParam() {
backend->WroteStartupParam();
FOR_EACH_BACKEND(periodic_tasks());
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
// currently only DataFlash_File support this:
void DataFlash_Class::flush(void) {
backend->flush();
FOR_EACH_BACKEND(flush());
}
#endif
void DataFlash_Class::Log_Write_EntireMission(const AP_Mission &mission)
{
FOR_EACH_BACKEND(Log_Write_EntireMission(mission));
}
void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
{
FOR_EACH_BACKEND(Log_Write_Format(s));
}
void DataFlash_Class::Log_Write_Message(const char *message)
{
FOR_EACH_BACKEND(Log_Write_Message(message));
}
void DataFlash_Class::Log_Write_Mode(uint8_t mode)
{
FOR_EACH_BACKEND(Log_Write_Mode(mode));
}
void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
{
FOR_EACH_BACKEND(Log_Write_Parameter(name, value));
}
void DataFlash_Class::Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd)
{
FOR_EACH_BACKEND(Log_Write_Mission_Cmd(mission, cmd));
}
// end functions pass straight through to backend
#undef FOR_EACH_BACKEND

50
libraries/DataFlash/DataFlash.h
View File

@ -30,19 +30,24 @@
#include "DFMessageWriter.h"
class DataFlash_Backend;
class DFMessageWriter;
enum DataFlash_Backend_Type {
DATAFLASH_BACKEND_NONE = 0,
DATAFLASH_BACKEND_FILE = 1,
};
class DataFlash_Class
{
friend class DFMessageWriter_DFLogStart; // for access to _num_types etc
friend class DataFlash_Backend; // for _num_types
public:
FUNCTOR_TYPEDEF(print_mode_fn, void, AP_HAL::BetterStream*, uint8_t);
FUNCTOR_TYPEDEF(vehicle_startup_message_Log_Writer, void);
DataFlash_Class(const char *firmware_string) :
_startup_messagewriter(DFMessageWriter_DFLogStart(*this,firmware_string)),
_vehicle_messages(NULL)
{ }
_firmware_string(firmware_string)
{
AP_Param::setup_object_defaults(this, var_info);
}
void set_mission(const AP_Mission *mission);
@ -59,9 +64,9 @@ public:
void Prep();
/* Write a block of data at current offset */
bool WriteBlock(const void *pBuffer, uint16_t size);
void WriteBlock(const void *pBuffer, uint16_t size);
/* Write an *important* block of data at current offset */
bool WriteCriticalBlock(const void *pBuffer, uint16_t size);
void WriteCriticalBlock(const void *pBuffer, uint16_t size);
// high level interface
uint16_t find_last_log() const;
@ -77,16 +82,14 @@ public:
void ShowDeviceInfo(AP_HAL::BetterStream *port);
void ListAvailableLogs(AP_HAL::BetterStream *port);
uint16_t bufferspace_available();
void setVehicle_Startup_Log_Writer(vehicle_startup_message_Log_Writer writer);
uint16_t StartNewLog(void);
void StartNewLog(void);
void AddLogFormats(const struct LogStructure *structures, uint8_t num_types);
void EnableWrites(bool enable);
void Log_Write_SysInfo(const char *firmware_string);
bool Log_Write_Format(const struct LogStructure *structure);
bool Log_Write_Parameter(const char *name, float value);
void Log_Write_Format(const struct LogStructure *structure);
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_RFND(const RangeFinder &rangefinder);
void Log_Write_IMU(const AP_InertialSensor &ins);
@ -105,18 +108,18 @@ public:
#endif
bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
void Log_Write_Radio(const mavlink_radio_t &packet);
bool Log_Write_Message(const char *message);
void Log_Write_Message(const char *message);
void Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
void Log_Write_ESC(void);
void Log_Write_Airspeed(AP_Airspeed &airspeed);
void Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets);
void Log_Write_Current(const AP_BattMonitor &battery, int16_t throttle);
void Log_Write_Compass(const Compass &compass);
bool Log_Write_Mode(uint8_t mode);
void Log_Write_Mode(uint8_t mode);
void Log_Write_Parameters(void);
void Log_Write_EntireMission(const AP_Mission &mission);
bool Log_Write_Mission_Cmd(const AP_Mission &mission,
void Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd);
void Log_Write_Origin(uint8_t origin_type, const Location &loc);
void Log_Write_RPM(const AP_RPM &rpm_sensor);
@ -147,12 +150,18 @@ public:
bool Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token,
enum ap_var_type type);
DFMessageWriter_DFLogStart _startup_messagewriter;
vehicle_startup_message_Log_Writer _vehicle_messages;
// parameter support
static const struct AP_Param::GroupInfo var_info[];
struct {
AP_Int8 backend_types;
} _params;
const struct LogStructure *structure(uint16_t num) const;
protected:
void Log_Fill_Format(const struct LogStructure *structure, struct log_Format &pkt);
uint16_t start_new_log(void);
void WroteStartupFormat();
void WroteStartupParam();
@ -163,11 +172,14 @@ protected:
/* Write a block with specified importance */
/* might be useful if you have a boolean indicating a message is
* important... */
bool WritePrioritisedBlock(const void *pBuffer, uint16_t size,
void WritePrioritisedBlock(const void *pBuffer, uint16_t size,
bool is_critical);
private:
DataFlash_Backend *backend;
#define DATAFLASH_MAX_BACKENDS 2
uint8_t _next_backend;
DataFlash_Backend *backends[DATAFLASH_MAX_BACKENDS];
const char *_firmware_string;
};
#endif

34
libraries/DataFlash/DataFlash_Backend.cpp
View File

@ -4,6 +4,28 @@
extern const AP_HAL::HAL& hal;
DataFlash_Backend::DataFlash_Backend(DataFlash_Class &front,
class DFMessageWriter_DFLogStart *writer) :
_front(front),
_startup_messagewriter(writer)
{
writer->set_dataflash_backend(this);
}
uint8_t DataFlash_Backend::num_types() const
{
return _front._num_types;
}
const struct LogStructure *DataFlash_Backend::structure(uint8_t num) const
{
return _front.structure(num);
}
DataFlash_Backend::vehicle_startup_message_Log_Writer DataFlash_Backend::vehicle_message_writer() {
return _front._vehicle_messages;
}
void DataFlash_Backend::periodic_10Hz(const uint32_t now)
{
}
@ -36,6 +58,10 @@ void DataFlash_Backend::internal_error() {
#endif
}
void DataFlash_Backend::set_mission(const AP_Mission *mission) {
_startup_messagewriter->set_mission(mission);
}
// this method can be overridden to do extra things with your buffer.
// for example, in DataFlash_MAVLink we may push messages into the UART.
void DataFlash_Backend::push_log_blocks() {
@ -46,7 +72,7 @@ void DataFlash_Backend::push_log_blocks() {
// for other messages to go out to the log
bool DataFlash_Backend::WriteBlockCheckStartupMessages()
{
if (_front._startup_messagewriter.fmt_done()) {
if (_startup_messagewriter->fmt_done()) {
return true;
}
@ -59,7 +85,7 @@ bool DataFlash_Backend::WriteBlockCheckStartupMessages()
// caller hoping to write blocks out. Push out log blocks - we
// might end up clearing the buffer.....
push_log_blocks();
if (_front._startup_messagewriter.fmt_done()) {
if (_startup_messagewriter->fmt_done()) {
return true;
}
@ -71,11 +97,11 @@ bool DataFlash_Backend::WriteBlockCheckStartupMessages()
void DataFlash_Backend::WriteMoreStartupMessages()
{
if (_front._startup_messagewriter.finished()) {
if (_startup_messagewriter->finished()) {
return;
}
_writing_startup_messages = true;
_front._startup_messagewriter.process();
_startup_messagewriter->process();
_writing_startup_messages = false;
}

50
libraries/DataFlash/DataFlash_Backend.h
View File

@ -3,19 +3,19 @@
#include "DataFlash.h"
class DFMessageWriter_DFLogStart;
class DataFlash_Backend
{
public:
FUNCTOR_TYPEDEF(print_mode_fn, void, AP_HAL::BetterStream*, uint8_t);
FUNCTOR_TYPEDEF(vehicle_startup_message_Log_Writer, void);
DataFlash_Backend(DataFlash_Class &front) :
_front(front),
_writing_startup_messages(false),
_internal_errors(0),
_dropped(0),
_last_periodic_1Hz(0),
_last_periodic_10Hz(0)
{ }
DataFlash_Backend(DataFlash_Class &front,
class DFMessageWriter_DFLogStart *writer);
vehicle_startup_message_Log_Writer vehicle_message_writer();
void internal_error();
@ -58,12 +58,12 @@ public:
// initialisation this really shouldn't take structure and
// num_types, however the CLI LogReadProcess function requires it.
// That function needs to be split.
virtual void Init(const struct LogStructure *structure, uint8_t num_types) {
virtual void Init() {
_writes_enabled = true;
_num_types = num_types;
_structures = structure;
}
void set_mission(const AP_Mission *mission);
virtual uint16_t bufferspace_available() = 0;
virtual uint16_t start_new_log(void) = 0;
@ -76,12 +76,30 @@ public:
virtual void flush(void) { }
#endif
virtual void periodic_tasks();
virtual void periodic_tasks();
uint8_t num_types() const;
const struct LogStructure *structure(uint8_t structure) const;
virtual void WroteStartupFormat() { }
virtual void WroteStartupParam() { }
void Log_Write_EntireMission(const AP_Mission &mission);
bool Log_Write_Format(const struct LogStructure *structure);
bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
bool Log_Write_Message(const char *message);
bool Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd);
bool Log_Write_Mode(uint8_t mode);;
bool Log_Write_Parameter(const char *name, float value);
bool Log_Write_Parameter(const AP_Param *ap,
const AP_Param::ParamToken &token,
enum ap_var_type type);
protected:
uint32_t dropped;
uint8_t internal_errors; // uint8_t - wishful thinking?
DataFlash_Class &_front;
virtual void periodic_10Hz(const uint32_t now);
@ -95,8 +113,6 @@ protected:
print_mode_fn print_mode,
AP_HAL::BetterStream *port);
const struct LogStructure *_structures;
uint8_t _num_types = 0;
bool _writes_enabled = false;
/*
@ -107,11 +123,15 @@ protected:
virtual bool WriteBlockCheckStartupMessages();
virtual void WriteMoreStartupMessages();
virtual void push_log_blocks();
bool _writing_startup_messages = false;
DFMessageWriter_DFLogStart *_startup_messagewriter;
bool _writing_startup_messages;
uint32_t _internal_errors;
uint32_t _dropped;
private:
uint32_t _last_periodic_1Hz;
uint32_t _last_periodic_10Hz;
};

4
libraries/DataFlash/DataFlash_Block.cpp
View File

@ -271,12 +271,12 @@ int16_t DataFlash_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t
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);
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);
Log_Fill_Format(&_structures[t], pkt);
Log_Fill_Format(structure(t), pkt);
uint8_t n = sizeof(pkt) - ofs;
if (n > len) {
n = len;

5
libraries/DataFlash/DataFlash_Block.h
View File

@ -14,8 +14,8 @@
class DataFlash_Block : public DataFlash_Backend
{
public:
DataFlash_Block(DataFlash_Class &front) :
DataFlash_Backend(front) { }
DataFlash_Block(DataFlash_Class &front, DFMessageWriter_DFLogStart *writer) :
DataFlash_Backend(front, writer) { }
// initialisation
virtual void Init(const struct LogStructure *structure, uint8_t num_types) = 0;
@ -125,7 +125,6 @@ protected:
#include "DataFlash_SITL.h"
#include "DataFlash_Empty.h"
#endif // DataFlash_block_h

83
libraries/DataFlash/DataFlash_Empty.cpp
View File

@ -1,83 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
hacked up DataFlash library for Desktop support
*/
#include "DataFlash_Empty.h"
#define DF_PAGE_SIZE 512
#define DF_NUM_PAGES 4096
// Public Methods //////////////////////////////////////////////////////////////
void DataFlash_Empty::Init(const struct LogStructure *structure, uint8_t num_types)
{
DataFlash_Backend::Init(structure, num_types);
df_PageSize = DF_PAGE_SIZE;
// reserve last page for config information
df_NumPages = DF_NUM_PAGES - 1;
}
// This function is mainly to test the device
void DataFlash_Empty::ReadManufacturerID()
{
df_manufacturer = 1;
df_device = 0x0203;
}
bool DataFlash_Empty::CardInserted(void)
{
return true;
}
// Read the status register
uint8_t DataFlash_Empty::ReadStatusReg()
{
return 0;
}
// Read the status of the DataFlash
inline
uint8_t DataFlash_Empty::ReadStatus()
{
return 1;
}
inline uint16_t DataFlash_Empty::PageSize()
{ return 0; }
// Wait until DataFlash is in ready state...
void DataFlash_Empty::WaitReady()
{ }
void DataFlash_Empty::PageToBuffer(unsigned char BufferNum, uint16_t PageAdr)
{ }
void DataFlash_Empty::BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait)
{ }
void DataFlash_Empty::BufferWrite (unsigned char BufferNum,
uint16_t IntPageAdr, unsigned char Data)
{ }
void DataFlash_Empty::BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr,
const void *pHeader, uint8_t hdr_size,
const void *pBuffer, uint16_t size)
{ }
bool DataFlash_Empty::BlockRead(uint8_t BufferNum, uint16_t IntPageAdr,
void *pBuffer, uint16_t size)
{
memset(pBuffer, 0, size);
return false;
}
// *** END OF INTERNAL FUNCTIONS ***
void DataFlash_Empty::PageErase (uint16_t PageAdr) { }
void DataFlash_Empty::BlockErase (uint16_t BlockAdr) { }
void DataFlash_Empty::ChipErase() { }

40
libraries/DataFlash/DataFlash_Empty.h
View File

@ -1,40 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* ************************************************************ */
/* DataFlash_EMPTY Log library */
/* ************************************************************ */
#ifndef __DATAFLASH_EMPTY_H__
#define __DATAFLASH_EMPTY_H__
#include <AP_HAL/AP_HAL.h>
#include "DataFlash_Block.h"
class DataFlash_Empty : public DataFlash_Block
{
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 PageToBuffer(uint8_t BufferNum, uint16_t PageAdr);
void WaitReady();
uint8_t ReadStatusReg();
uint8_t ReadStatus();
uint16_t PageSize();
void PageErase (uint16_t PageAdr);
void BlockErase (uint16_t BlockAdr);
void ChipErase();
void BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr,
const void *pHeader, uint8_t hdr_size,
const void *pBuffer, uint16_t size);
bool BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size);
public:
DataFlash_Empty(DataFlash_Class &front) :
DataFlash_Block(front) { }
void Init(const struct LogStructure *structure, uint8_t num_types);
void ReadManufacturerID();
bool CardInserted();
};
#endif // __DATAFLASH_EMPTY_H__

14
libraries/DataFlash/DataFlash_File.cpp
View File

@ -46,8 +46,10 @@ extern const AP_HAL::HAL& hal;
/*
constructor
*/
DataFlash_File::DataFlash_File(DataFlash_Class &front, const char *log_directory) :
DataFlash_Backend(front),
DataFlash_File::DataFlash_File(DataFlash_Class &front,
DFMessageWriter_DFLogStart *writer,
const char *log_directory) :
DataFlash_Backend(front, writer),
_write_fd(-1),
_read_fd(-1),
_read_fd_log_num(0),
@ -88,9 +90,9 @@ DataFlash_File::DataFlash_File(DataFlash_Class &front, const char *log_directory
// initialisation
void DataFlash_File::Init(const struct LogStructure *structure, uint8_t num_types)
void DataFlash_File::Init()
{
DataFlash_Backend::Init(structure, num_types);
DataFlash_Backend::Init();
// create the log directory if need be
int ret;
struct stat st;
@ -435,7 +437,7 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b
uint16_t space = BUF_SPACE(_writebuf);
if (_writing_startup_messages &&
_front._startup_messagewriter.fmt_done()) {
_startup_messagewriter->fmt_done()) {
// the state machine has called us, and it has finished
// writing format messages out. It can always get back to us
// with more messages later, so let's leave room for other
@ -727,6 +729,8 @@ uint16_t DataFlash_File::start_new_log(void)
{
stop_logging();
_startup_messagewriter->reset();
if (_open_error) {
// we have previously failed to open a file - don't try again
// to prevent us trying to open files while in flight

10
libraries/DataFlash/DataFlash_File.h
View File

@ -18,10 +18,12 @@ class DataFlash_File : public DataFlash_Backend
{
public:
// constructor
DataFlash_File(DataFlash_Class &front, const char *log_directory);
DataFlash_File(DataFlash_Class &front,
DFMessageWriter_DFLogStart *,
const char *log_directory);
// initialisation
void Init(const struct LogStructure *structure, uint8_t num_types);
void Init() override;
bool CardInserted(void);
// erase handling
@ -41,7 +43,7 @@ public:
void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc);
int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data);
uint16_t get_num_logs() override;
uint16_t start_new_log(void);
uint16_t start_new_log(void) override;
void LogReadProcess(const uint16_t log_num,
uint16_t start_page, uint16_t end_page,
print_mode_fn print_mode,
@ -70,7 +72,7 @@ private:
/*
read a block
*/
bool ReadBlock(void *pkt, uint16_t size);
bool ReadBlock(void *pkt, uint16_t size) override;
uint16_t _log_num_from_list_entry(const uint16_t list_entry);

4
libraries/DataFlash/DataFlash_SITL.cpp
View File

@ -28,9 +28,9 @@ static int flash_fd;
static uint8_t buffer[2][DF_PAGE_SIZE];
// Public Methods //////////////////////////////////////////////////////////////
void DataFlash_SITL::Init(const struct LogStructure *structure, uint8_t num_types)
void DataFlash_SITL::Init()
{
DataFlash_Backend::Init(structure, num_types);
DataFlash_Backend::Init();
if (flash_fd == 0) {
flash_fd = open("dataflash.bin", O_RDWR, 0777);
if (flash_fd == -1) {

6
libraries/DataFlash/DataFlash_SITL.h
View File

@ -40,9 +40,9 @@ private:
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
public:
DataFlash_SITL(DataFlash_Class &front) :
DataFlash_Block(front) { }
void Init(const struct LogStructure *structure, uint8_t num_types);
DataFlash_SITL(DataFlash_Class &front, DFMessageWriter_DFLogStart *writer) :
DataFlash_Block(front, writer) { }
void Init() override;
void ReadManufacturerID();
bool CardInserted();
};

104
libraries/DataFlash/LogFile.cpp
View File

@ -13,28 +13,43 @@
#include "DataFlash.h"
#include "DataFlash_SITL.h"
#include "DataFlash_Block.h"
#include "DataFlash_File.h"
#include "DataFlash_Empty.h"
#include "DFMessageWriter.h"
extern const AP_HAL::HAL& hal;
void DataFlash_Class::Init(const struct LogStructure *structure, uint8_t num_types)
void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types)
{
_num_types = num_types;
_structures = structure;
// DataFlash
#if defined(HAL_BOARD_LOG_DIRECTORY)
backend = new DataFlash_File(*this, HAL_BOARD_LOG_DIRECTORY);
#else
// no dataflash driver
backend = new DataFlash_Empty(*this);
#endif
if (backend == NULL) {
AP_HAL::panic("Unable to open dataflash");
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
hal.scheduler->panic("Too many backends");
return;
}
_num_types = num_types;
_structures = structures;
;
#if defined(HAL_BOARD_LOG_DIRECTORY)
if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
DFMessageWriter_DFLogStart *message_writer =
new DFMessageWriter_DFLogStart(_firmware_string);
if (message_writer != NULL) {
backends[_next_backend] = new DataFlash_File(*this,
message_writer,
HAL_BOARD_LOG_DIRECTORY);
}
if (backends[_next_backend] == NULL) {
hal.console->printf("Unable to open DataFlash_File");
} else {
_next_backend++;
}
}
#endif
for (uint8_t i=0; i<_next_backend; i++) {
backends[i]->Init();
}
backend->Init(structure, num_types);
}
// This function determines the number of whole or partial log files in the DataFlash
@ -75,6 +90,8 @@ uint16_t DataFlash_Block::get_num_logs(void)
// This function starts a new log file in the DataFlash
uint16_t DataFlash_Block::start_new_log(void)
{
_startup_messagewriter->reset();
uint16_t last_page = find_last_page();
StartRead(last_page);
@ -292,30 +309,30 @@ uint16_t DataFlash_Block::find_last_page_of_log(uint16_t log_number)
/*
read and print a log entry using the format strings from the given structure
- this really should in in the frontend, not the backend
*/
void DataFlash_Backend::_print_log_entry(uint8_t msg_type,
print_mode_fn print_mode,
AP_HAL::BetterStream *port)
{
uint8_t i;
for (i=0; i<_num_types; i++) {
if (msg_type == PGM_UINT8(&_structures[i].msg_type)) {
for (i=0; i<num_types(); i++) {
if (msg_type == structure(i)->msg_type) {
break;
}
}
if (i == _num_types) {
if (i == num_types()) {
port->printf("UNKN, %u\n", (unsigned)msg_type);
return;
}
uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3;
const struct LogStructure *log_structure = structure(i);
uint8_t msg_len = log_structure->msg_len - 3;
uint8_t pkt[msg_len];
if (!ReadBlock(pkt, msg_len)) {
return;
}
port->printf("%s, ", _structures[i].name);
port->printf("%s, ", log_structure->name);
for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) {
char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]);
char fmt = log_structure->format[fmt_ofs];
switch (fmt) {
case 'b': {
port->printf("%d", (int)pkt[ofs]);
@ -469,8 +486,8 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type,
*/
void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port)
{
for (uint8_t i=0; i<_num_types; i++) {
const struct LogStructure *s = &_structures[i];
for (uint8_t i=0; i<num_types(); i++) {
const struct LogStructure *s = structure(i);
port->printf("FMT, %u, %u, %s, %s, %s\n",
(unsigned)PGM_UINT8(&s->msg_type),
(unsigned)PGM_UINT8(&s->msg_len),
@ -603,19 +620,11 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
// This function starts a new log file in the DataFlash, and writes
// the format of supported messages in the log
uint16_t DataFlash_Class::StartNewLog(void)
void DataFlash_Class::StartNewLog(void)
{
uint16_t ret;
ret = start_new_log();
if (ret == 0xFFFF) {
// don't write out formats if we fail to open the log
return ret;
for (uint8_t i=0; i<_next_backend; i++) {
backends[i]->start_new_log();
}
_startup_messagewriter.reset();
return ret;
}
// add new logging formats to the log. Used by libraries that want to
@ -647,7 +656,7 @@ void DataFlash_Backend::Log_Fill_Format(const struct LogStructure *s, struct log
/*
write a structure format to the log
*/
bool DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
bool DataFlash_Backend::Log_Write_Format(const struct LogStructure *s)
{
struct log_Format pkt;
Log_Fill_Format(s, pkt);
@ -657,7 +666,7 @@ bool DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
/*
write a parameter to the log
*/
bool DataFlash_Class::Log_Write_Parameter(const char *name, float value)
bool DataFlash_Backend::Log_Write_Parameter(const char *name, float value)
{
struct log_Parameter pkt = {
LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
@ -672,9 +681,9 @@ bool DataFlash_Class::Log_Write_Parameter(const char *name, float value)
/*
write a parameter to the log
*/
bool DataFlash_Class::Log_Write_Parameter(const AP_Param *ap,
const AP_Param::ParamToken &token,
enum ap_var_type type)
bool DataFlash_Backend::Log_Write_Parameter(const AP_Param *ap,
const AP_Param::ParamToken &token,
enum ap_var_type type)
{
char name[16];
ap->copy_name_token(token, &name[0], sizeof(name), true);
@ -1026,23 +1035,24 @@ void DataFlash_Class::Log_Write_SysInfo(const char *firmware_string)
}
// Write a mission command. Total length : 36 bytes
bool DataFlash_Class::Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd)
bool DataFlash_Backend::Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd)
{
mavlink_mission_item_t mav_cmd = {};
AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
return Log_Write_MavCmd(mission.num_commands(),mav_cmd);
}
void DataFlash_Class::Log_Write_EntireMission(const AP_Mission &mission)
void DataFlash_Backend::Log_Write_EntireMission(const AP_Mission &mission)
{
DFMessageWriter_WriteEntireMission writer(*this);
DFMessageWriter_WriteEntireMission writer;
writer.set_dataflash_backend(this);
writer.set_mission(&mission);
writer.process();
}
// Write a text message to the log
bool DataFlash_Class::Log_Write_Message(const char *message)
bool DataFlash_Backend::Log_Write_Message(const char *message)
{
struct log_Message pkt = {
LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG),
@ -1521,7 +1531,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
#endif
// Write a command processing packet
bool DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd)
bool DataFlash_Backend::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd)
{
struct log_Cmd pkt = {
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
@ -1685,7 +1695,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass)
}
// Write a mode packet.
bool DataFlash_Class::Log_Write_Mode(uint8_t mode)
bool DataFlash_Backend::Log_Write_Mode(uint8_t mode)
{
struct log_Mode pkt = {
LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),