DataFlash: DFMessageWriter; ability to trickle messages out to DF

libraries/DataFlash/DFMessageWriter.cpp

@@ -0,0 +1,203 @@
+#include "DFMessageWriter.h"
+
+#include "DataFlash.h"
+
+extern const AP_HAL::HAL& hal;
+
+/* LogStartup - these are simple state machines which allow us to
+ * trickle out messages to the log files
+ */
+
+void DFMessageWriter::reset()
+{
+    _finished = false;
+}
+
+void DFMessageWriter_DFLogStart::reset()
+{
+    DFMessageWriter::reset();
+
+    _writesysinfo.reset();
+    _writeentiremission.reset();
+
+    stage = ls_blockwriter_stage_init;
+    next_format_to_send = 0;
+    ap = AP_Param::first(&token, &type);
+}
+
+void DFMessageWriter_DFLogStart::process()
+{
+    switch(stage) {
+    case ls_blockwriter_stage_init:
+        stage = ls_blockwriter_stage_formats;
+        // fall through
+
+    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])) {
+                return; // call me again!
+            }
+            // provide hook to avoid corrupting the APM1/APM2
+            // dataflash by writing too fast:
+            _DataFlash.WroteStartupFormat();
+            next_format_to_send++;
+        }
+        stage = ls_blockwriter_stage_parms;
+        // fall through
+
+    case ls_blockwriter_stage_parms:
+        while (ap) {
+            if (!_DataFlash.Log_Write_Parameter(ap, token, type)) {
+                return;
+            }
+            ap = AP_Param::next_scalar(&token, &type);
+            _DataFlash.WroteStartupParam();
+        }
+
+        stage = ls_blockwriter_stage_sysinfo;
+        // fall through
+
+    case ls_blockwriter_stage_sysinfo:
+        _writesysinfo.process();
+        if (!_writesysinfo.finished()) {
+            return;
+        }
+        stage = ls_blockwriter_stage_write_entire_mission;
+        // fall through
+
+    case ls_blockwriter_stage_write_entire_mission:
+        _writeentiremission.process();
+        if (!_writeentiremission.finished()) {
+            return;
+        }
+        stage = ls_blockwriter_stage_vehicle_messages;
+        // fall through
+
+    case ls_blockwriter_stage_vehicle_messages:
+        // 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) {
+                return;
+            }
+            _DataFlash._vehicle_messages();
+        }
+        stage = ls_blockwriter_stage_done;
+        // fall through
+
+    case ls_blockwriter_stage_done:
+        break;
+    }
+
+    _finished = true;
+}
+
+void DFMessageWriter_WriteSysInfo::reset()
+{
+    DFMessageWriter::reset();
+    stage = ws_blockwriter_stage_init;
+}
+
+void DFMessageWriter_DFLogStart::set_mission(const AP_Mission *mission)
+{
+    _writeentiremission.set_mission(mission);
+}
+
+
+void DFMessageWriter_WriteSysInfo::process() {
+    switch(stage) {
+
+    case ws_blockwriter_stage_init:
+        stage = ws_blockwriter_stage_firmware_string;
+        // fall through
+
+    case ws_blockwriter_stage_firmware_string:
+        if (! _DataFlash.Log_Write_Message_P(_firmware_string)) {
+            return; // call me again
+        }
+        stage = ws_blockwriter_stage_git_versions;
+        // fall through
+
+    case ws_blockwriter_stage_git_versions:
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+        if (! _DataFlash.Log_Write_Message_P(PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION))) {
+            return; // call me again
+        }
+#endif
+        stage = ws_blockwriter_stage_system_id;
+        // fall through
+
+    case ws_blockwriter_stage_system_id:
+        char sysid[40];
+        if (hal.util->get_system_id(sysid)) {
+            if (! _DataFlash.Log_Write_Message(sysid)) {
+                return; // call me again
+            }
+        }
+        // fall through
+    }
+
+    _finished = true;  // all done!
+}
+
+// void DataFlash_Class::Log_Write_SysInfo(const prog_char_t *firmware_string)
+// {
+//     DFMessageWriter_WriteSysInfo writer(firmware_string);
+//     writer->process();
+// }
+
+void DFMessageWriter_WriteEntireMission::process() {
+    switch(stage) {
+
+    case em_blockwriter_stage_init:
+        if (_mission == NULL) {
+            stage = em_blockwriter_stage_done;
+            break;
+        } else {
+            stage = em_blockwriter_stage_write_new_mission_message;
+        }
+        // fall through
+
+    case em_blockwriter_stage_write_new_mission_message:
+        if (! _DataFlash.Log_Write_Message_P(PSTR("New mission"))) {
+            return; // call me again
+        }
+        stage = em_blockwriter_stage_write_mission_items;
+        // fall through
+
+    case em_blockwriter_stage_write_mission_items:
+        AP_Mission::Mission_Command cmd;
+        while (_mission_number_to_send < _mission->num_commands()) {
+            // 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)) {
+                    return; // call me again
+                }
+            }
+            _mission_number_to_send++;
+        }
+        stage = em_blockwriter_stage_done;
+        // fall through
+
+    case em_blockwriter_stage_done:
+        break;
+    }
+
+    _finished = true;
+}
+
+void DFMessageWriter_WriteEntireMission::reset()
+{
+    DFMessageWriter::reset();
+    stage = em_blockwriter_stage_init;
+    _mission_number_to_send = 0;
+}
+
+
+void DFMessageWriter_WriteEntireMission::set_mission(const AP_Mission *mission)
+{
+    _mission = mission;
+}

libraries/DataFlash/DFMessageWriter.h

@@ -0,0 +1,115 @@
+#ifndef DF_LOGSTARTUP_H
+#define DF_LOGSTARTUP_H
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Mission/AP_Mission.h>
+
+class DataFlash_Class;
+
+class DFMessageWriter {
+public:
+    DFMessageWriter(DataFlash_Class &DataFlash) :
+        _DataFlash(DataFlash) { }
+
+    virtual void reset() = 0;
+    virtual void process() = 0;
+    virtual bool finished() { return _finished; };
+
+protected:
+    bool _finished;
+    DataFlash_Class &_DataFlash;
+};
+
+
+class DFMessageWriter_WriteSysInfo : public DFMessageWriter {
+public:
+    DFMessageWriter_WriteSysInfo(DataFlash_Class &DataFlash,
+                                 const prog_char_t *firmware_string) :
+        DFMessageWriter(DataFlash),
+        _firmware_string(firmware_string)
+        { }
+
+    void reset();
+    void process();
+
+private:
+    enum write_sysinfo_blockwriter_stage {
+        ws_blockwriter_stage_init,
+        ws_blockwriter_stage_firmware_string,
+        ws_blockwriter_stage_git_versions,
+        ws_blockwriter_stage_system_id
+    };
+    write_sysinfo_blockwriter_stage stage;
+
+    const prog_char_t *_firmware_string;
+};
+
+class DFMessageWriter_WriteEntireMission : public DFMessageWriter {
+public:
+    DFMessageWriter_WriteEntireMission(DataFlash_Class &DataFlash) :
+        DFMessageWriter(DataFlash),
+        _mission_number_to_send(0),
+        stage(em_blockwriter_stage_init),
+        _mission(NULL)
+        { }
+    void reset();
+    void process();
+
+    void set_mission(const AP_Mission *mission);
+
+private:
+    enum entire_mission_blockwriter_stage {
+        em_blockwriter_stage_init,
+        em_blockwriter_stage_write_new_mission_message,
+        em_blockwriter_stage_write_mission_items,
+        em_blockwriter_stage_done
+    };
+
+    const AP_Mission *_mission;
+    uint16_t _mission_number_to_send;
+    entire_mission_blockwriter_stage stage;
+};
+
+class DFMessageWriter_DFLogStart : public DFMessageWriter {
+public:
+    DFMessageWriter_DFLogStart(DataFlash_Class &DataFlash,
+                               const prog_char_t *firmware_string) :
+        DFMessageWriter{DataFlash},
+        _writesysinfo(DataFlash, firmware_string),
+        _writeentiremission(DataFlash)
+        {
+        }
+
+    void reset();
+    void process();
+
+    void set_mission(const AP_Mission *mission);
+
+private:
+
+    enum log_start_blockwriter_stage {
+        ls_blockwriter_stage_init,
+        ls_blockwriter_stage_formats,
+        ls_blockwriter_stage_parms,
+        ls_blockwriter_stage_sysinfo,
+        ls_blockwriter_stage_write_entire_mission,
+        ls_blockwriter_stage_vehicle_messages,
+        ls_blockwriter_stage_done,
+    };
+
+    log_start_blockwriter_stage stage;
+
+    uint16_t next_format_to_send;
+    AP_Param::ParamToken token;
+    AP_Param *ap;
+    enum ap_var_type type;
+    uint16_t num_format_types;
+    const struct LogStructure *_structures;
+
+
+    DFMessageWriter_WriteSysInfo _writesysinfo;
+    DFMessageWriter_WriteEntireMission _writeentiremission;
+};
+
+#endif
+

libraries/DataFlash/DataFlash.cpp

@@ -1,9 +1,29 @@
 #include "DataFlash.h"
 
-// start functions pass straight through to backend:
-void DataFlash_Class::WriteBlock(const void *pBuffer, uint16_t size) {
-    backend->WriteBlock(pBuffer, size);
+#include "DataFlash_Backend.h"
+
+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);
+}
+
+// start functions pass straight through to backend:
+bool DataFlash_Class::WriteBlock(const void *pBuffer, uint16_t size) {
+    return backend->WriteBlock(pBuffer, size);
+}
+
+bool DataFlash_Class::WriteCriticalBlock(const void *pBuffer, uint16_t size) {
+    return 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();
 }
@@ -21,6 +41,10 @@ bool DataFlash_Class::NeedErase(void) {
     return backend->NeedErase();
 }
 
+uint16_t DataFlash_Class::bufferspace_available(void) {
+    return backend->bufferspace_available();
+}
+
 uint16_t DataFlash_Class::find_last_log(void) {
     return backend->find_last_log();
 }
@@ -66,6 +90,18 @@ void DataFlash_Class::EnableWrites(bool enable) {
     backend->EnableWrites(enable);
 }
 
+void DataFlash_Class::periodic_tasks() {
+    backend->periodic_tasks();
+}
+
+void DataFlash_Class::WroteStartupFormat() {
+    backend->WroteStartupFormat();
+}
+
+void DataFlash_Class::WroteStartupParam() {
+    backend->WroteStartupParam();
+}
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
     // currently only DataFlash_File support this:
 void DataFlash_Class::flush(void) {

libraries/DataFlash/DataFlash.h

@@ -19,19 +19,29 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_RPM/AP_RPM.h>
 #include <stdint.h>
-#include "DataFlash_Backend.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 #include <uORB/topics/esc_status.h>
 #endif
 
+#include "DFMessageWriter.h"
 
 class DataFlash_Backend;
+class DFMessageWriter;
 
 class DataFlash_Class
 {
+    friend class DFMessageWriter_DFLogStart; // for access to _num_types etc
+
 public:
     FUNCTOR_TYPEDEF(print_mode_fn, void, AP_HAL::BetterStream*, uint8_t);
+    FUNCTOR_TYPEDEF(vehicle_startup_message_Log_Writer, void);
+    DataFlash_Class(const prog_char_t *firmware_string) :
+        _startup_messagewriter(DFMessageWriter_DFLogStart(*this,firmware_string)),
+        _vehicle_messages(NULL)
+        { }
+
+    void set_mission(const AP_Mission *mission);
 
     // initialisation
     void Init(const struct LogStructure *structure, uint8_t num_types);
@@ -42,7 +52,9 @@ public:
     void EraseAll();
 
     /* Write a block of data at current offset */
-    void WriteBlock(const void *pBuffer, uint16_t size);
+    bool WriteBlock(const void *pBuffer, uint16_t size);
+    /* Write an *important* block of data at current offset */
+    bool WriteCriticalBlock(const void *pBuffer, uint16_t size);
 
     // high level interface
     uint16_t find_last_log(void);
@@ -60,13 +72,16 @@ public:
     void ListAvailableLogs(AP_HAL::BetterStream *port);
 #endif // DATAFLASH_NO_CLI
 
-    /* logging methods common to all vehicles */
+    uint16_t bufferspace_available();
+
+    void setVehicle_Startup_Log_Writer(vehicle_startup_message_Log_Writer writer);
+
     uint16_t StartNewLog(void);
     void AddLogFormats(const struct LogStructure *structures, uint8_t num_types);
     void EnableWrites(bool enable);
     void Log_Write_SysInfo(const prog_char_t *firmware_string);
-    void Log_Write_Format(const struct LogStructure *structure);
-    void Log_Write_Parameter(const char *name, float value);
+    bool Log_Write_Format(const struct LogStructure *structure);
+    bool 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_IMU(const AP_InertialSensor &ins);
     void Log_Write_IMUDT(const AP_InertialSensor &ins);
@@ -80,21 +95,21 @@ public:
 #if AP_AHRS_NAVEKF_AVAILABLE
     void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
 #endif
-    void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
+    bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
     void Log_Write_Radio(const mavlink_radio_t &packet);
-    void Log_Write_Message(const char *message);
-    void Log_Write_Message_P(const prog_char_t *message);
+    bool Log_Write_Message(const char *message);
+    bool Log_Write_Message_P(const prog_char_t *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);
-    void Log_Write_Mode(uint8_t mode);
+    bool Log_Write_Mode(uint8_t mode);
     void Log_Write_Parameters(void);
 
     void Log_Write_EntireMission(const AP_Mission &mission);
-    void Log_Write_Mission_Cmd(const AP_Mission &mission,
+    bool 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);
@@ -118,15 +133,32 @@ public:
     void flush(void);
 #endif
 
+    void periodic_tasks(); // may want to split this into GCS/non-GCS duties
+
+    // this is out here for the trickle-startup-messages logging.
+    // Think before calling.
+    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;
+
 protected:
     void Log_Fill_Format(const struct LogStructure *structure, struct log_Format &pkt);
-    void Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, 
-                             enum ap_var_type type);
     uint16_t start_new_log(void);
 
+    void WroteStartupFormat();
+    void WroteStartupParam();
+
     const struct LogStructure *_structures;
     uint8_t _num_types;
 
+    /* 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,
+                               bool is_critical);
+
 private:
     DataFlash_Backend *backend;
 };
@@ -848,7 +880,4 @@ enum LogOriginType {
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use
 
-#include "DataFlash_Block.h"
-#include "DataFlash_File.h"
-
 #endif

libraries/DataFlash/DataFlash_APM1.cpp

@@ -401,3 +401,18 @@ void DataFlash_APM1::ChipErase()
     _spi_sem->give();
     
 }
+
+// Writing too quickly to DF on APM1/APM2 corrupts the flash.  This
+// could be done at a lower level to more generally restrict bandwidth
+void DataFlash_APM1::WroteStartupFormat()
+{
+    if (! hal.util->get_soft_armed()) {
+        hal.scheduler->delay(10);
+    }
+}
+void DataFlash_APM1::WroteStartupParam()
+{
+    if (! hal.util->get_soft_armed()) {
+        hal.scheduler->delay(2);
+    }
+}

libraries/DataFlash/DataFlash_APM1.h

@@ -49,6 +49,9 @@ public:
     void        Init(const struct LogStructure *structure, uint8_t num_types);
     void        ReadManufacturerID();
     bool        CardInserted();
+
+    void WroteStartupFormat();
+    void WroteStartupParam();
 };
 
 #endif

libraries/DataFlash/DataFlash_APM2.cpp

@@ -407,3 +407,18 @@ void DataFlash_APM2::ChipErase()
     // release SPI bus for use by other sensors
     _spi_sem->give();
 }
+
+// Writing too quickly to DF on APM1/APM2 corrupts the flash.  This
+// could be done at a lower level to more generally restrict bandwidth
+void DataFlash_APM2::WroteStartupFormat()
+{
+    if (! hal.util->get_soft_armed()) {
+        hal.scheduler->delay(10);
+    }
+}
+void DataFlash_APM2::WroteStartupParam()
+{
+    if (! hal.util->get_soft_armed()) {
+        hal.scheduler->delay(2);
+    }
+}

libraries/DataFlash/DataFlash_APM2.h

@@ -50,6 +50,9 @@ public:
     void        Init(const struct LogStructure *structure, uint8_t num_types);
     void        ReadManufacturerID();
     bool        CardInserted();
+
+    void WroteStartupFormat();
+    void WroteStartupParam();
 };
 
 #endif

libraries/DataFlash/DataFlash_Backend.cpp

@@ -0,0 +1,81 @@
+#include "DataFlash_Backend.h"
+
+#include "DFMessageWriter.h"
+
+extern const AP_HAL::HAL& hal;
+
+void DataFlash_Backend::periodic_10Hz(const uint32_t now)
+{
+}
+void DataFlash_Backend::periodic_1Hz(const uint32_t now)
+{
+}
+void DataFlash_Backend::periodic_fullrate(const uint32_t now)
+{
+}
+
+void DataFlash_Backend::periodic_tasks()
+{
+    uint32_t now = hal.scheduler->millis();
+    if (now - _last_periodic_1Hz > 1000) {
+        periodic_1Hz(now);
+        _last_periodic_1Hz = now;
+    }
+    if (now - _last_periodic_10Hz > 100) {
+        periodic_10Hz(now);
+        _last_periodic_10Hz = now;
+    }
+    periodic_fullrate(now);
+}
+
+
+void DataFlash_Backend::internal_error() {
+    _internal_errors++;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    abort();
+#endif
+}
+
+// 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() {
+    WriteMoreStartupMessages();
+}
+
+// returns true if all startup messages have been written (and thus it
+// is OK for other messages to go to the log)
+bool DataFlash_Backend::WriteBlockCheckStartupMessages()
+{
+    if (_front._startup_messagewriter.finished()) {
+        return true;
+    }
+
+    if (_writing_startup_messages) {
+        // we have been called by a messagewriter, so writing is OK
+        return true;
+    }
+
+    // we're not writing startup messages, so this must be some random
+    // caller hoping to write blocks out.  Push out log blocks - we
+    // might end up clearing the buffer.....
+    push_log_blocks();
+    if (_front._startup_messagewriter.finished()) {
+        return true;
+    }
+
+    // sorry!  currently busy writing out startup messages...
+    return false;
+}
+
+// source more messages from the startup message writer:
+void DataFlash_Backend::WriteMoreStartupMessages()
+{
+
+    if (_front._startup_messagewriter.finished()) {
+        return;
+    }
+
+    _writing_startup_messages = true;
+    _front._startup_messagewriter.process();
+    _writing_startup_messages = false;
+}

libraries/DataFlash/DataFlash_Backend.h

@@ -5,17 +5,7 @@
 #define DATAFLASH_NO_CLI
 #endif
 
-#include <AP_HAL/AP_HAL.h>
-#include <AP_Common/AP_Common.h>
-#include <AP_Param/AP_Param.h>
-#include <AP_GPS/AP_GPS.h>
-#include <AP_InertialSensor/AP_InertialSensor.h>
-#include <AP_Baro/AP_Baro.h>
-#include <AP_AHRS/AP_AHRS.h>
-#include <AP_Vehicle/AP_Vehicle.h>
-#include <AP_Airspeed/AP_Airspeed.h>
-#include <AP_BattMonitor/AP_BattMonitor.h>
-#include <stdint.h>
+#include "DataFlash.h"
 
 class DataFlash_Backend
 {
@@ -24,9 +14,15 @@ public:
 
     DataFlash_Backend(DataFlash_Class &front) :
         _front(front),
-        _structures(NULL)
+        _writing_startup_messages(false),
+        _internal_errors(0),
+        _dropped(0),
+        _last_periodic_1Hz(0),
+        _last_periodic_10Hz(0)
         { }
 
+    void internal_error();
+
     virtual bool CardInserted(void) = 0;
 
     // erase handling
@@ -34,7 +30,15 @@ public:
     virtual void EraseAll() = 0;
 
     /* Write a block of data at current offset */
-    virtual void WriteBlock(const void *pBuffer, uint16_t size) = 0;
+    bool WriteBlock(const void *pBuffer, uint16_t size) {
+        return WritePrioritisedBlock(pBuffer, size, false);
+    }
+
+    bool WriteCriticalBlock(const void *pBuffer, uint16_t size) {
+        return WritePrioritisedBlock(pBuffer, size, true);
+    }
+
+    virtual bool WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) = 0;
 
     // high level interface
     virtual uint16_t find_last_log(void) = 0;
@@ -64,6 +68,8 @@ public:
         _structures = structure;
     }
 
+    virtual uint16_t bufferspace_available() = 0;
+
     virtual uint16_t start_new_log(void) = 0;
     bool log_write_started;
 
@@ -74,9 +80,18 @@ public:
     virtual void flush(void) { }
 #endif
 
+    virtual void periodic_tasks();
+
+    virtual void WroteStartupFormat() { }
+    virtual void WroteStartupParam() { }
+
 protected:
     DataFlash_Class &_front;
 
+    virtual void periodic_10Hz(const uint32_t now);
+    virtual void periodic_1Hz(const uint32_t now);
+    virtual void periodic_fullrate(const uint32_t now);
+
     /*
     read and print a log entry using the format strings from the given structure
     */
@@ -93,6 +108,16 @@ protected:
     */
     virtual bool ReadBlock(void *pkt, uint16_t size) = 0;
 
+    virtual bool WriteBlockCheckStartupMessages();
+    virtual void WriteMoreStartupMessages();
+    virtual void push_log_blocks();
+    bool _writing_startup_messages = false;
+
+    uint32_t _internal_errors;
+    uint32_t _dropped;
+private:
+    uint32_t _last_periodic_1Hz;
+    uint32_t _last_periodic_10Hz;
 };
 
 #endif

libraries/DataFlash/DataFlash_Block.cpp

@@ -3,8 +3,9 @@
  *       DataFlash.cpp - DataFlash log library generic code
  */
 
+#include "DataFlash_Block.h"
+
 #include <AP_HAL/AP_HAL.h>
-#include "DataFlash.h"
 
 extern AP_HAL::HAL& hal;
 
@@ -12,6 +13,13 @@ extern AP_HAL::HAL& hal;
 // this if (and only if!) the low level format changes
 #define DF_LOGGING_FORMAT    0x28122013
 
+uint16_t DataFlash_Block::bufferspace_available()
+{
+    // because DataFlash_Block devices are ring buffers, we *always*
+    // have room...
+    return df_NumPages * df_PageSize; 
+}
+
 // *** DATAFLASH PUBLIC FUNCTIONS ***
 void DataFlash_Block::StartWrite(uint16_t PageAdr)
 {
@@ -35,11 +43,20 @@ void DataFlash_Block::FinishWrite(void)
     df_BufferIdx = 0;
 }
 
-void DataFlash_Block::WriteBlock(const void *pBuffer, uint16_t size)
+bool DataFlash_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 reservice bandwidth based on is_critical
     if (!CardInserted() || !log_write_started || !_writes_enabled) {
-        return;
+        return false;
     }
+
+    if (! WriteBlockCheckStartupMessages()) {
+        return false;
+    }
+
     while (size > 0) {
         uint16_t n = df_PageSize - df_BufferIdx;
         if (n > size) {
@@ -68,6 +85,8 @@ void DataFlash_Block::WriteBlock(const void *pBuffer, uint16_t size)
             df_FilePage++;
         }
     }
+
+    return true;
 }
 
 

libraries/DataFlash/DataFlash_Block.h

@@ -26,7 +26,7 @@ public:
     void EraseAll();
 
     /* Write a block of data at current offset */
-    void WriteBlock(const void *pBuffer, uint16_t size);
+    bool WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical);
 
     // high level interface
     uint16_t find_last_log(void);
@@ -46,6 +46,8 @@ public:
     void ListAvailableLogs(AP_HAL::BetterStream *port);
 #endif
 
+    uint16_t bufferspace_available();
+
 private:
     struct PageHeader {
         uint16_t FileNumber;

libraries/DataFlash/DataFlash_File.cpp

@@ -10,7 +10,8 @@
 #include <AP_HAL/AP_HAL.h>
 
 #if HAL_OS_POSIX_IO
-#include "DataFlash.h"
+#include "DataFlash_File.h"
+
 #include <sys/stat.h>
 #include <sys/types.h>
 #include <unistd.h>
@@ -136,6 +137,17 @@ void DataFlash_File::Init(const struct LogStructure *structure, uint8_t num_type
     hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&DataFlash_File::_io_timer, void));
 }
 
+void DataFlash_File::periodic_fullrate(const uint32_t now)
+{
+    DataFlash_Backend::push_log_blocks();
+}
+
+uint16_t DataFlash_File::bufferspace_available()
+{
+    uint16_t _head;
+    return (BUF_SPACE(_writebuf)) - critical_message_reserved_space();
+}
+
 // return true for CardInserted() if we successfully initialised
 bool DataFlash_File::CardInserted(void)
 {
@@ -198,17 +210,31 @@ void DataFlash_File::EraseAll()
 }
 
 /* Write a block of data at current offset */
-void DataFlash_File::WriteBlock(const void *pBuffer, uint16_t size)
+bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
 {
     if (_write_fd == -1 || !_initialised || _open_error || !_writes_enabled) {
-        return;
+        return false;
     }
+
+    if (! WriteBlockCheckStartupMessages()) {
+        _dropped++;
+        return false;
+    }
+
     uint16_t _head;
     uint16_t space = BUF_SPACE(_writebuf);
+
+    // we reserve some amount of space for critical messages:
+    if (!is_critical && space < critical_message_reserved_space()) {
+        _dropped++;
+        return false;
+    }
+
+    // if no room for entire message - drop it:
     if (space < size) {
-        // discard the whole write, to keep the log consistent
         perf_count(_perf_overruns);
-        return;
+        _dropped++;
+        return false;
     }
 
     if (_writebuf_tail < _head) {
@@ -231,6 +257,7 @@ void DataFlash_File::WriteBlock(const void *pBuffer, uint16_t size)
             BUF_ADVANCETAIL(_writebuf, n);
         }
     }
+    return true;
 }
 
 /*

libraries/DataFlash/DataFlash_File.h

@@ -36,7 +36,8 @@ public:
     void EraseAll();
 
     /* Write a block of data at current offset */
-    void WriteBlock(const void *pBuffer, uint16_t size);
+    bool WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical);
+    uint16_t bufferspace_available();
 
     // high level interface
     uint16_t find_last_log(void);
@@ -57,7 +58,8 @@ public:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
     void flush(void);
 #endif
-
+    void periodic_fullrate(const uint32_t now);
+    
 private:
     int _write_fd;
     int _read_fd;
@@ -91,6 +93,11 @@ private:
 
     void _io_timer(void);
 
+    uint16_t critical_message_reserved_space() const {
+        // possibly make this a proportional to buffer size?
+        return 1024;
+    };
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // performance counters
     perf_counter_t  _perf_write;

libraries/DataFlash/DataFlash_SITL.cpp

@@ -3,6 +3,8 @@
   hacked up DataFlash library for Desktop support
 */
 
+#include "DataFlash_SITL.h"
+
 #include <AP_HAL/AP_HAL.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -14,7 +16,6 @@
 #include <fcntl.h>
 #include <stdint.h>
 #include <assert.h>
-#include "DataFlash.h"
 
 #pragma GCC diagnostic ignored "-Wunused-result"
 
@@ -160,5 +161,3 @@ void DataFlash_SITL::ChipErase()
 
 
 #endif
-
-

libraries/DataFlash/LogFile.cpp

@@ -10,6 +10,14 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Compass/AP_Compass.h>
 
+#include "DataFlash_SITL.h"
+#include "DataFlash_File.h"
+#include "DataFlash_Empty.h"
+#include "DataFlash_APM1.h"
+#include "DataFlash_APM2.h"
+
+#include "DFMessageWriter.h"
+
 extern const AP_HAL::HAL& hal;
 
 void DataFlash_Class::Init(const struct LogStructure *structure, uint8_t num_types)
@@ -605,17 +613,14 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
 uint16_t 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;
     }
-    // write log formats so the log is self-describing
-    for (uint8_t i=0; i<_num_types; i++) {
-        Log_Write_Format(&_structures[i]);
-        // avoid corrupting the APM1/APM2 dataflash by writing too fast
-        hal.scheduler->delay(10);
-    }
+
+    _startup_messagewriter.reset();
 
     return ret;
 }
@@ -649,17 +654,17 @@ void DataFlash_Backend::Log_Fill_Format(const struct LogStructure *s, struct log
 /*
   write a structure format to the log
  */
-void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
+bool DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
 {
     struct log_Format pkt;
     Log_Fill_Format(s, pkt);
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
 /*
   write a parameter to the log
  */
-void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
+bool DataFlash_Class::Log_Write_Parameter(const char *name, float value)
 {
     struct log_Parameter pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
@@ -668,19 +673,19 @@ void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
         value : value
     };
     strncpy(pkt.name, name, sizeof(pkt.name));
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
 /*
   write a parameter to the log
  */
-void DataFlash_Class::Log_Write_Parameter(const AP_Param *ap,
+bool DataFlash_Class::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);
-    Log_Write_Parameter(name, ap->cast_to_float(type));
+    return Log_Write_Parameter(name, ap->cast_to_float(type));
 }
 
 /*
@@ -982,28 +987,23 @@ void DataFlash_Class::Log_Write_SysInfo(const prog_char_t *firmware_string)
 }
 
 // Write a mission command. Total length : 36 bytes
-void DataFlash_Class::Log_Write_Mission_Cmd(const AP_Mission &mission,
+bool DataFlash_Class::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);
-    Log_Write_MavCmd(mission.num_commands(),mav_cmd);
+    return Log_Write_MavCmd(mission.num_commands(),mav_cmd);
 }
 
 void DataFlash_Class::Log_Write_EntireMission(const AP_Mission &mission)
 {
-    Log_Write_Message_P(PSTR("New mission"));
-
-    AP_Mission::Mission_Command cmd;
-    for (uint16_t i = 0; i < mission.num_commands(); i++) {
-        if (mission.read_cmd_from_storage(i,cmd)) {
-            Log_Write_Mission_Cmd(mission, cmd);
-        }
-    }
+    DFMessageWriter_WriteEntireMission writer(*this);
+    writer.set_mission(&mission);
+    writer.process();
 }
 
 // Write a text message to the log
-void DataFlash_Class::Log_Write_Message(const char *message)
+bool DataFlash_Class::Log_Write_Message(const char *message)
 {
     struct log_Message pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG),
@@ -1011,11 +1011,11 @@ void DataFlash_Class::Log_Write_Message(const char *message)
         msg  : {}
     };
     strncpy(pkt.msg, message, sizeof(pkt.msg));
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteBlock(&pkt, sizeof(pkt));
 }
 
 // Write a text message to the log
-void DataFlash_Class::Log_Write_Message_P(const prog_char_t *message)
+bool DataFlash_Class::Log_Write_Message_P(const prog_char_t *message)
 {
     struct log_Message pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG),
@@ -1023,7 +1023,7 @@ void DataFlash_Class::Log_Write_Message_P(const prog_char_t *message)
         msg  : {}
     };
     strncpy_P(pkt.msg, message, sizeof(pkt.msg));
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteBlock(&pkt, sizeof(pkt));
 }
 
 // Write a POWR packet
@@ -1226,7 +1226,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
 #endif
 
 // Write a command processing packet
-void DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd)
+bool DataFlash_Class::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),
@@ -1242,7 +1242,7 @@ void DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission
         longitude       : (float)mav_cmd.y,
         altitude        : (float)mav_cmd.z
     };
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteBlock(&pkt, sizeof(pkt));
 }
 
 void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet) 
@@ -1393,7 +1393,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass)
 }
 
 // Write a mode packet.
-void DataFlash_Class::Log_Write_Mode(uint8_t mode)
+bool DataFlash_Class::Log_Write_Mode(uint8_t mode)
 {
     struct log_Mode pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
@@ -1401,7 +1401,7 @@ void DataFlash_Class::Log_Write_Mode(uint8_t mode)
         mode     : mode,
         mode_num : mode
     };
-    WriteBlock(&pkt, sizeof(pkt));
+    return WriteBlock(&pkt, sizeof(pkt));
 }
 
 // Write ESC status messages