diff --git a/libraries/DataFlash/DFMessageWriter.cpp b/libraries/DataFlash/DFMessageWriter.cpp index dec37581a8..853f9b9a61 100644 --- a/libraries/DataFlash/DFMessageWriter.cpp +++ b/libraries/DataFlash/DFMessageWriter.cpp @@ -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 } } diff --git a/libraries/DataFlash/DFMessageWriter.h b/libraries/DataFlash/DFMessageWriter.h index 26562955bd..53f37e5565 100644 --- a/libraries/DataFlash/DFMessageWriter.h +++ b/libraries/DataFlash/DFMessageWriter.h @@ -1,31 +1,31 @@ #ifndef DF_LOGSTARTUP_H #define DF_LOGSTARTUP_H -#include -#include +#include "DataFlash_Backend.h" -class DataFlash_Class; +#include 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; diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 5f6cc616b7..10cd7403ff 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -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 diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index b86686ac7a..447d870247 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -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 ¤t_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 diff --git a/libraries/DataFlash/DataFlash_Backend.cpp b/libraries/DataFlash/DataFlash_Backend.cpp index 3a7e21f864..edcaa6d2fe 100644 --- a/libraries/DataFlash/DataFlash_Backend.cpp +++ b/libraries/DataFlash/DataFlash_Backend.cpp @@ -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; } diff --git a/libraries/DataFlash/DataFlash_Backend.h b/libraries/DataFlash/DataFlash_Backend.h index a184e6a0a0..10e8571ab3 100644 --- a/libraries/DataFlash/DataFlash_Backend.h +++ b/libraries/DataFlash/DataFlash_Backend.h @@ -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; }; diff --git a/libraries/DataFlash/DataFlash_Block.cpp b/libraries/DataFlash/DataFlash_Block.cpp index 736f47c879..b8381cf6e2 100644 --- a/libraries/DataFlash/DataFlash_Block.cpp +++ b/libraries/DataFlash/DataFlash_Block.cpp @@ -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; diff --git a/libraries/DataFlash/DataFlash_Block.h b/libraries/DataFlash/DataFlash_Block.h index 65593d5efd..3eb10231dd 100644 --- a/libraries/DataFlash/DataFlash_Block.h +++ b/libraries/DataFlash/DataFlash_Block.h @@ -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 diff --git a/libraries/DataFlash/DataFlash_Empty.cpp b/libraries/DataFlash/DataFlash_Empty.cpp deleted file mode 100644 index 249279e929..0000000000 --- a/libraries/DataFlash/DataFlash_Empty.cpp +++ /dev/null @@ -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() { } - - diff --git a/libraries/DataFlash/DataFlash_Empty.h b/libraries/DataFlash/DataFlash_Empty.h deleted file mode 100644 index 2a81fadc55..0000000000 --- a/libraries/DataFlash/DataFlash_Empty.h +++ /dev/null @@ -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 -#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__ diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index c18bd83972..89de6a89fb 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -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 diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index f452b2b05e..e03fd2b6bd 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -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); diff --git a/libraries/DataFlash/DataFlash_SITL.cpp b/libraries/DataFlash/DataFlash_SITL.cpp index 9bec892b61..a14bbfef10 100644 --- a/libraries/DataFlash/DataFlash_SITL.cpp +++ b/libraries/DataFlash/DataFlash_SITL.cpp @@ -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) { diff --git a/libraries/DataFlash/DataFlash_SITL.h b/libraries/DataFlash/DataFlash_SITL.h index 833a3ea0f5..0a01f38f43 100644 --- a/libraries/DataFlash/DataFlash_SITL.h +++ b/libraries/DataFlash/DataFlash_SITL.h @@ -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(); }; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 8a3a23e15a..5b5ffa3964 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -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; imsg_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; ofsformat[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; iprintf("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),