From 825b360fb0fa98edea218e81b9bb35071b61bba6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 26 Nov 2013 08:44:00 +1100
Subject: [PATCH] DataFlash: added RCIN and SRVO log methods

---
 libraries/DataFlash/DataFlash.h | 36 ++++++++++++++++++++++++++++++++-
 libraries/DataFlash/LogFile.cpp | 36 +++++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index d2e76da947..26de011437 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -47,6 +47,8 @@ public:
     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);
+    void Log_Write_RCIN(void);
+    void Log_Write_SERVO(void);
     void Log_Write_Message(const char *message);
     void Log_Write_Message_P(const prog_char_t *message);
 
@@ -167,6 +169,32 @@ struct PACKED log_IMU {
     float accel_x, accel_y, accel_z;
 };
 
+struct PACKED log_RCIN {
+    LOG_PACKET_HEADER;
+    uint32_t timestamp;
+    uint16_t chan1;
+    uint16_t chan2;
+    uint16_t chan3;
+    uint16_t chan4;
+    uint16_t chan5;
+    uint16_t chan6;
+    uint16_t chan7;
+    uint16_t chan8;
+};
+
+struct PACKED log_SERVO {
+    LOG_PACKET_HEADER;
+    uint32_t timestamp;
+    uint16_t chan1;
+    uint16_t chan2;
+    uint16_t chan3;
+    uint16_t chan4;
+    uint16_t chan5;
+    uint16_t chan6;
+    uint16_t chan7;
+    uint16_t chan8;
+};
+
 #define LOG_COMMON_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
       "FMT", "BBnNZ",      "Type,Length,Name,Format" },    \
@@ -177,7 +205,11 @@ struct PACKED log_IMU {
     { LOG_IMU_MSG, sizeof(log_IMU), \
       "IMU",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
     { LOG_MESSAGE_MSG, sizeof(log_Message), \
-      "MSG",  "Z",     "Message" }
+      "MSG",  "Z",     "Message"}, \
+    { LOG_RCIN_MSG, sizeof(log_RCIN), \
+      "RCIN",  "Ihhhhhhhh",     "TimeMS,Chan1,Chan2,Chan3,Chan4,Chan5,Chan6,Chan7,Chan8" }, \
+    { LOG_SERVO_MSG, sizeof(log_SERVO), \
+      "SRVO",  "Ihhhhhhhh",     "TimeMS,Chan1,Chan2,Chan3,Chan4,Chan5,Chan6,Chan7,Chan8" }
 
 // message types for common messages
 #define LOG_FORMAT_MSG	  128
@@ -185,6 +217,8 @@ struct PACKED log_IMU {
 #define LOG_GPS_MSG		  130
 #define LOG_IMU_MSG		  131
 #define LOG_MESSAGE_MSG	  132
+#define LOG_RCIN_MSG      133
+#define LOG_SERVO_MSG     134
 
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 9eb9e8b148..32aad24835 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -647,6 +647,42 @@ void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt)
     WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write an RCIN packet
+void DataFlash_Class::Log_Write_RCIN(void)
+{
+    struct log_RCIN pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG),
+        timestamp     : hal.scheduler->millis(),
+        chan1         : hal.rcin->read(0),
+        chan2         : hal.rcin->read(1),
+        chan3         : hal.rcin->read(2),
+        chan4         : hal.rcin->read(3),
+        chan5         : hal.rcin->read(4),
+        chan6         : hal.rcin->read(5),
+        chan7         : hal.rcin->read(6),
+        chan8         : hal.rcin->read(7)
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write an SERVO packet
+void DataFlash_Class::Log_Write_SERVO(void)
+{
+    struct log_SERVO pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_SERVO_MSG),
+        timestamp     : hal.scheduler->millis(),
+        chan1         : hal.rcout->read(0),
+        chan2         : hal.rcout->read(1),
+        chan3         : hal.rcout->read(2),
+        chan4         : hal.rcout->read(3),
+        chan5         : hal.rcout->read(4),
+        chan6         : hal.rcout->read(5),
+        chan7         : hal.rcout->read(6),
+        chan8         : hal.rcout->read(7)
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 
 // Write an raw accel/gyro data packet
 void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)