DataFlash: new dataflash logging system

this allows us to remove the display functions in the vehicle code,
and also allows us to store the format of a log file in the log. It
also stores the parameters used in a flight, rather than the
parameters set at the time the log is dumped

libraries/DataFlash/DataFlash.h

@@ -6,6 +6,11 @@
 #ifndef DataFlash_h
 #define DataFlash_h
 
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_GPS.h>
+#include <AP_InertialSensor.h>
+#include <AP_AHRS.h>
 #include <stdint.h>
 
 // the last page holds the log format in first 4 bytes. Please change
@@ -39,19 +44,49 @@ public:
     virtual void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) = 0;
     virtual uint16_t get_num_logs(void) = 0;
     virtual uint16_t start_new_log(void) = 0;
-    virtual void log_read_process(uint16_t log_num,
+    void log_read_process(uint16_t log_num,
                           uint16_t start_page, uint16_t end_page, 
-                                  void (*callback)(uint8_t msgid)) = 0;
+                          void (*callback)(uint8_t msgid));
+    virtual void LogReadProcess(uint16_t log_num,
+                                uint16_t start_page, uint16_t end_page, 
+                                uint8_t num_types,
+                                const struct LogStructure *structure,
+                                AP_HAL::BetterStream *port) = 0;
     virtual void DumpPageInfo(AP_HAL::BetterStream *port) = 0;
     virtual void ShowDeviceInfo(AP_HAL::BetterStream *port) = 0;
     virtual void ListAvailableLogs(AP_HAL::BetterStream *port) = 0;
 
+    /* logging methods common to all vehicles */
+    void Log_Write_Parameter(const char *name, float value);
+    void Log_Write_GPS(const GPS *gps, int32_t relative_alt);
+    void Log_Write_IMU(const AP_InertialSensor *ins);
+
 	/*
       every logged packet starts with 3 bytes
     */
     struct log_Header {
         uint8_t head1, head2, msgid;
     };
+
+protected:
+    /*
+    print column headers
+    */
+    void _print_format_headers(uint8_t num_types, 
+                               const struct LogStructure *structure,
+                               AP_HAL::BetterStream *port);
+    
+    /*
+    read and print a log entry using the format strings from the given structure
+    */
+    void _print_log_entry(uint8_t msg_type, 
+                          uint8_t num_types, 
+                          const struct LogStructure *structure,
+                          AP_HAL::BetterStream *port);
+    
+    void Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, 
+                             enum ap_var_type type);
+    void Log_Write_Parameters(void);
 };
 
 /*
@@ -66,6 +101,74 @@ public:
 #define HEAD_BYTE1  0xA3    // Decimal 163
 #define HEAD_BYTE2  0x95    // Decimal 149
 
+/*
+Format characters in the format string for binary log messages
+  b   : int8_t
+  B   : uint8_t
+  h   : int16_t
+  H   : uint16_t
+  i   : int32_t
+  I   : uint32_t
+  f   : float
+  N   : char[16]
+  c   : int16_t * 100
+  C   : uint16_t * 100
+  e   : int32_t * 100
+  E   : uint32_t * 100
+  L   : uint32_t latitude/longitude
+ */
+
+// structure used to define logging format
+struct LogStructure {
+    uint8_t msg_type;
+    uint8_t msg_len;
+    const char name[5];
+    const char format[16];
+    const char labels[64];
+};
+
+/*
+  log structures common to all vehicle types
+ */
+struct PACKED log_Parameter {
+    LOG_PACKET_HEADER;
+    char name[16];
+    float value;
+};
+
+struct PACKED log_GPS {
+    LOG_PACKET_HEADER;
+    uint8_t  status;
+    uint32_t gps_time;
+    uint8_t  num_sats;
+    int16_t  hdop;
+    int32_t  latitude;
+    int32_t  longitude;
+    int32_t  rel_altitude;
+    int32_t  altitude;
+    uint32_t ground_speed;
+    int32_t  ground_course;
+};
+
+struct PACKED log_IMU {
+    LOG_PACKET_HEADER;
+    float gyro_x, gyro_y, gyro_z;
+    float accel_x, accel_y, accel_z;
+};
+
+#define LOG_COMMON_STRUCTURES \
+    { LOG_PARAMETER_MSG, sizeof(log_Parameter), \
+      "PARM", "Nf",        "Name,Value" },    \
+    { LOG_GPS_MSG, sizeof(log_GPS), \
+      "GPS",  "BIBcLLeeEe", "Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs" }, \
+    { LOG_IMU_MSG, sizeof(log_IMU), \
+      "IMU",  "ffffff",     "GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }
+
+// message types for common messages
+#define LOG_PARAMETER_MSG 128
+#define LOG_GPS_MSG		  129
+#define LOG_IMU_MSG		  130
+
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
 

libraries/DataFlash/DataFlash_Block.h

@@ -16,7 +16,7 @@
 // we use an invalie logging format to test the chip erase
 #define DF_LOGGING_FORMAT_INVALID   0x28122012
 
-class DataFlash_Block : DataFlash_Class
+class DataFlash_Block : public DataFlash_Class
 {
 public:
     // initialisation
@@ -43,6 +43,11 @@ public:
     void log_read_process(uint16_t log_num,
                           uint16_t start_page, uint16_t end_page, 
                           void (*callback)(uint8_t msgid));
+    void LogReadProcess(uint16_t log_num,
+                        uint16_t start_page, uint16_t end_page, 
+                        uint8_t num_types,
+                        const struct LogStructure *structure,
+                        AP_HAL::BetterStream *port);
     void DumpPageInfo(AP_HAL::BetterStream *port);
     void ShowDeviceInfo(AP_HAL::BetterStream *port);
     void ListAvailableLogs(AP_HAL::BetterStream *port);

libraries/DataFlash/DataFlash_File.cpp

@@ -288,9 +288,82 @@ uint16_t DataFlash_File::start_new_log(void)
     fprintf(f, "%u\n", (unsigned)log_num);
     fclose(f);    
     free(fname);
+
+    Log_Write_Parameters();
+
     return log_num;
 }
 
+/*
+  Read the log and print it on port
+*/
+void DataFlash_File::LogReadProcess(uint16_t log_num,
+                                    uint16_t start_page, uint16_t end_page, 
+                                    uint8_t num_types,
+                                    const struct LogStructure *structure,
+                                    AP_HAL::BetterStream *port)
+{
+    uint8_t log_step = 0;
+    if (!_initialised) {
+        return;
+    }
+    if (_read_fd != -1) {
+        ::close(_read_fd);
+    }
+    char *fname = _log_file_name(log_num);
+    if (fname == NULL) {
+        return;
+    }
+    _read_fd = ::open(fname, O_RDONLY);
+    free(fname);
+    if (_read_fd == -1) {
+        return;
+    }
+    _read_offset = 0;
+    if (start_page != 0) {
+        ::lseek(_read_fd, start_page * DATAFLASH_PAGE_SIZE, SEEK_SET);
+    }
+
+    _print_format_headers(num_types, structure, port);
+
+    while (true) {
+        uint8_t data;
+        if (::read(_read_fd, &data, 1) != 1) {
+            // reached end of file
+            break;
+        }
+        _read_offset++;
+
+        // This is a state machine to read the packets
+        switch(log_step) {
+            case 0:
+                if (data == HEAD_BYTE1) {
+                    log_step++;
+                }
+                break;
+
+            case 1:
+                if (data == HEAD_BYTE2) {
+                    log_step++;
+                } else {
+                    log_step = 0;
+                }
+                break;
+
+            case 2:
+                log_step = 0;
+                _print_log_entry(data, num_types, structure, port);
+                break;
+        }
+        if (_read_offset >= (end_page+1) * DATAFLASH_PAGE_SIZE) {
+            break;
+        }
+    }
+
+    ::close(_read_fd);
+    _read_fd = -1;
+}
+
 /*
   start processing a log, called by CLI to display logs
  */

libraries/DataFlash/DataFlash_File.h

@@ -10,7 +10,7 @@
 #ifndef DataFlash_File_h
 #define DataFlash_File_h
 
-class DataFlash_File : DataFlash_Class
+class DataFlash_File : public DataFlash_Class
 {
 public:
     // constructor
@@ -40,6 +40,11 @@ public:
     void log_read_process(uint16_t log_num,
                           uint16_t start_page, uint16_t end_page, 
                           void (*callback)(uint8_t msgid));
+    void LogReadProcess(uint16_t log_num,
+                        uint16_t start_page, uint16_t end_page, 
+                        uint8_t num_types,
+                        const struct LogStructure *structure,
+                        AP_HAL::BetterStream *port);
     void DumpPageInfo(AP_HAL::BetterStream *port);
     void ShowDeviceInfo(AP_HAL::BetterStream *port);
     void ListAvailableLogs(AP_HAL::BetterStream *port);

libraries/DataFlash/LogFile.cpp

@@ -2,6 +2,10 @@
 
 #include <AP_HAL.h>
 #include "DataFlash.h"
+#include <stdlib.h>
+#include <AP_Param.h>
+
+extern const AP_HAL::HAL& hal;
 
 // This function determines the number of whole or partial log files in the DataFlash
 // Wholly overwritten files are (of course) lost.
@@ -53,6 +57,7 @@ uint16_t DataFlash_Block::start_new_log(void)
         SetFileNumber(1);
         StartWrite(1);
         //Serial.println("start log from 0");
+        Log_Write_Parameters();
         return 1;
     }
 
@@ -73,6 +78,7 @@ uint16_t DataFlash_Block::start_new_log(void)
         SetFileNumber(new_log_num);
         StartWrite(last_page + 1);
     }
+    Log_Write_Parameters();
     return new_log_num;
 }
 
@@ -240,13 +246,221 @@ uint16_t DataFlash_Block::find_last_page_of_log(uint16_t log_number)
     return -1;
 }
 
+#define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr)
 
 /*
-  Read the DataFlash log memory
-  Call the callback() function on each log message found in the page
-  range. Return the number of log messages found
+  print the log column names
+ */
+void DataFlash_Class::_print_format_headers(uint8_t num_types, 
+                                            const struct LogStructure *structure,
+                                            AP_HAL::BetterStream *port)
+{
+    uint8_t i;
+    for (i=0; i<num_types; i++) {
+        port->printf_P(PSTR("FMT, %S, %u, %S, %S\n"),
+                       structure[i].name,
+                       (unsigned)PGM_UINT8(&structure[i].msg_type),
+                       structure[i].format,
+                       structure[i].labels);
+    }
+}
 
-  Note that for the block oriented backend the log_num is ignored
+/*
+  read and print a log entry using the format strings from the given structure
+ */
+void DataFlash_Class::_print_log_entry(uint8_t msg_type, 
+                                       uint8_t num_types, 
+                                       const struct LogStructure *structure,
+                                       AP_HAL::BetterStream *port)
+{
+    uint8_t i;
+    for (i=0; i<num_types; i++) {
+        if (msg_type == PGM_UINT8(&structure[i].msg_type)) {
+            break;
+        }
+    }
+    if (i == num_types) {
+        port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type);
+        return;
+    }
+    uint8_t msg_len = PGM_UINT8(&structure[i].msg_len);
+    uint8_t pkt[msg_len];
+    ReadPacket(pkt, msg_len);
+    port->printf_P(PSTR("%S, "), structure[i].name);
+    for (uint8_t ofs=3, fmt_ofs=0; ofs<msg_len; fmt_ofs++) {
+        char fmt = PGM_UINT8(&structure[i].format[fmt_ofs]);
+        switch (fmt) {
+        case 'b': {
+            port->printf_P(PSTR("%d"), (int)pkt[ofs]);
+            ofs += 1;
+            break;
+        }
+        case 'B': {
+            port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]);
+            ofs += 1;
+            break;
+        }
+        case 'h': {
+            int16_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%d"), (int)v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'H': {
+            uint16_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%u"), (unsigned)v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'i': {
+            int32_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%ld"), (long)v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'I': {
+            uint32_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%lu"), (unsigned long)v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'f': {
+            float v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%f"), v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'c': {
+            int16_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%.2f"), 0.01f*v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'C': {
+            uint16_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%.2f"), 0.01f*v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'e': {
+            int32_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%.2f"), 0.01f*v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'E': {
+            uint32_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            port->printf_P(PSTR("%.2f"), 0.01f*v);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'L': {
+            int32_t v;
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            int32_t dec_portion, frac_portion;
+            int32_t abs_lat_or_lon = labs(v);
+
+            // extract decimal portion (special handling of negative numbers to ensure we round towards zero)
+            dec_portion = abs_lat_or_lon / 10000000UL;
+
+            // extract fractional portion
+            frac_portion = abs_lat_or_lon - dec_portion*10000000UL;
+
+            // print output including the minus sign
+            if (v < 0) {
+                port->printf_P(PSTR("-"));
+            }
+            port->printf_P(PSTR("%ld.%07ld"),(long)dec_portion,(long)frac_portion);
+            ofs += sizeof(v);
+            break;
+        }
+        case 'N': {
+            char v[17];
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            v[16] = 0;
+            port->printf_P(PSTR("%s"), v);
+            ofs += 16;
+            break;
+        }
+        default:
+            ofs = msg_len;
+            break;
+        }
+        if (ofs < msg_len) {
+            port->printf_P(PSTR(", "));
+        }
+    }
+    port->println();
+}
+
+/*
+  Read the log and print it on port
+*/
+void DataFlash_Block::LogReadProcess(uint16_t log_num,
+                                     uint16_t start_page, uint16_t end_page, 
+                                     uint8_t num_types,
+                                     const struct LogStructure *structure,
+                                     AP_HAL::BetterStream *port)
+{
+    uint8_t log_step = 0;
+    uint16_t page = start_page;
+
+    if (df_BufferIdx != 0) {
+        FinishWrite();
+    }
+
+    _print_format_headers(num_types, structure, port);
+
+    StartRead(start_page);
+
+	while (true) {
+		uint8_t data;
+        ReadBlock(&data, 1);
+
+		// This is a state machine to read the packets
+		switch(log_step) {
+			case 0:
+				if (data == HEAD_BYTE1) {
+					log_step++;
+                }
+				break;
+
+			case 1:
+				if (data == HEAD_BYTE2) {
+					log_step++;
+                } else {
+					log_step = 0;
+				}
+				break;
+
+			case 2:
+				log_step = 0;
+                _print_log_entry(data, num_types, structure, port);
+                break;
+		}
+        uint16_t new_page = GetPage();
+        if (new_page != page) {
+            if (new_page == end_page || new_page == start_page) {
+                return;
+            }
+            page = new_page;
+        }
+	}
+}
+
+/*
+  Read the DataFlash log memory
+  This is obsolete and will be removed when plane and copter are
+  converted to LogReadProcess()
 */
 void DataFlash_Block::log_read_process(uint16_t log_num,
                                        uint16_t start_page, uint16_t end_page, 
@@ -358,3 +572,89 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
     }
     port->println();
 }
+
+/*
+  write a parameter to the log
+ */
+void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
+{
+    struct log_Parameter pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
+        name  : {},
+        value : value
+    };
+    strncpy(pkt.name, name, sizeof(pkt.name));
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
+/*
+  write a parameter to the log
+ */
+void 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));
+}
+
+/*
+  write all parameters to the log - used when starting a new log so
+  the log file has a full record of the parameters
+ */
+void DataFlash_Class::Log_Write_Parameters(void)
+{
+    AP_Param::ParamToken token;
+    AP_Param *ap;
+    enum ap_var_type type;
+
+    for (ap=AP_Param::first(&token, &type);
+         ap;
+         ap=AP_Param::next_scalar(&token, &type)) {
+        Log_Write_Parameter(ap, token, type);
+        // slow down the parameter dump to prevent saturating
+        // the dataflash write bandwidth
+        hal.scheduler->delay(1);
+    }
+}
+
+
+
+// Write an GPS packet
+void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt)
+{
+    struct log_GPS pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
+    	status        : (uint8_t)gps->status(),
+    	gps_time      : gps->time,
+        num_sats      : gps->num_sats,
+        hdop          : gps->hdop,
+        latitude      : gps->latitude,
+        longitude     : gps->longitude,
+        rel_altitude  : relative_alt,
+        altitude      : gps->altitude,
+        ground_speed  : gps->ground_speed,
+        ground_course : gps->ground_course
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
+
+// Write an raw accel/gyro data packet
+void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor *ins)
+{
+    Vector3f gyro = ins->get_gyro();
+    Vector3f accel = ins->get_accel();
+    struct log_IMU pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
+        gyro_x  : gyro.x,
+        gyro_y  : gyro.y,
+        gyro_z  : gyro.z,
+        accel_x : accel.x,
+        accel_y : accel.y,
+        accel_z : accel.z
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+