From ce43e674fee8303f7cf9bc51a41667ec824c4810 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 13 Feb 2014 22:07:32 +1100
Subject: [PATCH] DataFlash: added power status logging

---
 libraries/DataFlash/DataFlash.h | 14 +++++++++++++-
 libraries/DataFlash/LogFile.cpp | 16 ++++++++++++++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 5c8e829116..ab4baab861 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -51,6 +51,7 @@ public:
     void Log_Write_RCIN(void);
     void Log_Write_RCOUT(void);
     void Log_Write_Baro(AP_Baro &baro);
+    void Log_Write_Power(void);
     void Log_Write_Message(const char *message);
     void Log_Write_Message_P(const prog_char_t *message);
 
@@ -211,6 +212,14 @@ struct PACKED log_BARO {
     int16_t temperature;
 };
 
+struct PACKED log_POWR {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t Vcc;
+    uint16_t Vservo;
+    uint16_t flags;
+};
+
 #define LOG_COMMON_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
       "FMT", "BBnNZ",      "Type,Length,Name,Format" },    \
@@ -229,7 +238,9 @@ struct PACKED log_BARO {
     { LOG_RCOUT_MSG, sizeof(log_RCOUT), \
       "RCOU",  "Ihhhhhhhh",     "TimeMS,Chan1,Chan2,Chan3,Chan4,Chan5,Chan6,Chan7,Chan8" }, \
     { LOG_BARO_MSG, sizeof(log_BARO), \
-      "BARO",  "Iffc",     "TimeMS,Alt,Press,Temp" }
+      "BARO",  "Iffc",     "TimeMS,Alt,Press,Temp" }, \
+    { LOG_POWR_MSG, sizeof(log_POWR), \
+      "POWR","ICCH","TimeMS,Vcc,VServo,Flags" }
 
 // message types for common messages
 #define LOG_FORMAT_MSG	  128
@@ -241,6 +252,7 @@ struct PACKED log_BARO {
 #define LOG_RCOUT_MSG     134
 #define LOG_IMU2_MSG	  135
 #define LOG_BARO_MSG	  136
+#define LOG_POWR_MSG	  137
 
 #include "DataFlash_Block.h"
 #include "DataFlash_File.h"
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 055bfe4dcf..f6aaf69abe 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -777,3 +777,19 @@ void DataFlash_Class::Log_Write_Message_P(const prog_char_t *message)
     strncpy_P(pkt.msg, message, sizeof(pkt.msg));
     WriteBlock(&pkt, sizeof(pkt));
 }
+
+// Write a POWR packet
+void DataFlash_Class::Log_Write_Power(void)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    struct log_POWR pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_POWR_MSG),
+        time_ms : hal.scheduler->millis(),
+        Vcc     : (uint16_t)(hal.analogin->board_voltage() * 100),
+        Vservo  : (uint16_t)(hal.analogin->servorail_voltage() * 100),
+        flags   : hal.analogin->power_status_flags()
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+#endif
+}
+