From c29d870497db69764e06a23f84bb95480f949e70 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 19 Apr 2013 23:19:16 +1000
Subject: [PATCH] DataFlash: dump the format of logs at the start of the log

---
 libraries/DataFlash/DataFlash.h        | 21 +++++++++--
 libraries/DataFlash/DataFlash_File.cpp |  2 --
 libraries/DataFlash/LogFile.cpp        | 50 +++++++++++++++++++++++---
 3 files changed, 63 insertions(+), 10 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 433bf38772..45b0bf9dee 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -57,6 +57,9 @@ public:
     virtual void ListAvailableLogs(AP_HAL::BetterStream *port) = 0;
 
     /* logging methods common to all vehicles */
+    uint16_t StartNewLog(uint8_t num_types,
+                         const struct LogStructure *structure);
+    void Log_Write_Format(const struct LogStructure *structure);
     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);
@@ -110,6 +113,7 @@ Format characters in the format string for binary log messages
   i   : int32_t
   I   : uint32_t
   f   : float
+  n   : char[4]
   N   : char[16]
   c   : int16_t * 100
   C   : uint16_t * 100
@@ -130,6 +134,14 @@ struct LogStructure {
 /*
   log structures common to all vehicle types
  */
+struct PACKED log_Format {
+    LOG_PACKET_HEADER;
+    uint8_t type;
+    uint8_t length;
+    char name[4];
+    char format[16];
+};
+
 struct PACKED log_Parameter {
     LOG_PACKET_HEADER;
     char name[16];
@@ -157,6 +169,8 @@ struct PACKED log_IMU {
 };
 
 #define LOG_COMMON_STRUCTURES \
+    { LOG_FORMAT_MSG, sizeof(log_Parameter), \
+      "FMT", "BBnN",        "Type,Length,Name,Format" },    \
     { LOG_PARAMETER_MSG, sizeof(log_Parameter), \
       "PARM", "Nf",        "Name,Value" },    \
     { LOG_GPS_MSG, sizeof(log_GPS), \
@@ -165,9 +179,10 @@ struct PACKED 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
+#define LOG_FORMAT_MSG	  128
+#define LOG_PARAMETER_MSG 129
+#define LOG_GPS_MSG		  130
+#define LOG_IMU_MSG		  131
 
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp
index fd1f4b740e..e08aa05edf 100644
--- a/libraries/DataFlash/DataFlash_File.cpp
+++ b/libraries/DataFlash/DataFlash_File.cpp
@@ -289,8 +289,6 @@ uint16_t DataFlash_File::start_new_log(void)
     fclose(f);    
     free(fname);
 
-    Log_Write_Parameters();
-
     return log_num;
 }
 
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 81d812c038..7907f03406 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -57,7 +57,6 @@ uint16_t DataFlash_Block::start_new_log(void)
         SetFileNumber(1);
         StartWrite(1);
         //Serial.println("start log from 0");
-        Log_Write_Parameters();
         return 1;
     }
 
@@ -78,7 +77,6 @@ uint16_t DataFlash_Block::start_new_log(void)
         SetFileNumber(new_log_num);
         StartWrite(last_page + 1);
     }
-    Log_Write_Parameters();
     return new_log_num;
 }
 
@@ -257,7 +255,7 @@ void DataFlash_Class::_print_format_headers(uint8_t num_types,
 {
     uint8_t i;
     for (i=0; i<num_types; i++) {
-        port->printf_P(PSTR("FMT, %S, %u, %S, %S\n"),
+        port->printf_P(PSTR("LABL, %S, %u, %S, %S\n"),
                        structure[i].name,
                        (unsigned)PGM_UINT8(&structure[i].msg_type),
                        structure[i].format,
@@ -383,12 +381,20 @@ void DataFlash_Class::_print_log_entry(uint8_t msg_type,
             ofs += sizeof(v);
             break;
         }
+        case 'n': {
+            char v[5];
+            memcpy(&v, &pkt[ofs], sizeof(v));
+            v[sizeof(v)-1] = 0;
+            port->printf_P(PSTR("%s"), v);
+            ofs += sizeof(v)-1;
+            break;
+        }
         case 'N': {
             char v[17];
             memcpy(&v, &pkt[ofs], sizeof(v));
-            v[16] = 0;
+            v[sizeof(v)-1] = 0;
             port->printf_P(PSTR("%s"), v);
-            ofs += 16;
+            ofs += sizeof(v)-1;
             break;
         }
         default:
@@ -573,6 +579,40 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
     port->println();
 }
 
+// This function starts a new log file in the DataFlash, and writes
+// the format of supported messages in the log, plus all parameters
+uint16_t DataFlash_Class::StartNewLog(uint8_t num_types, const struct LogStructure *structures)
+{
+    uint16_t ret;
+    ret = start_new_log();
+
+    // write log formats so the log is self-describing
+    for (uint8_t i=0; i<num_types; i++) {
+        Log_Write_Format(&structures[i]);
+    }
+
+    // and all current parameters
+    Log_Write_Parameters();
+    return ret;
+}
+
+/*
+  write a structure format to the log
+ */
+void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
+{
+    struct log_Format pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
+        type   : PGM_UINT8(&s->msg_type),
+        length : PGM_UINT8(&s->msg_len),
+        name   : {},
+        format : {}
+    };
+    strncpy_P(pkt.name, s->name, sizeof(pkt.name));
+    strncpy_P(pkt.format, s->format, sizeof(pkt.format));
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 /*
   write a parameter to the log
  */