From f0f262eb045116b4c3157bc29ba4a9693b33d6d8 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 11 Jun 2015 22:26:45 +0900
Subject: [PATCH] DataFlash: add Log_Write_Vibration

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

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 3bc426d0a0..2076218ab2 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -67,6 +67,7 @@ public:
     void 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_Vibration(const AP_InertialSensor &ins);
     void Log_Write_RCIN(void);
     void Log_Write_RCOUT(void);
     void Log_Write_Baro(AP_Baro &baro);
@@ -226,6 +227,13 @@ struct PACKED log_IMU {
     uint8_t gyro_health, accel_health;
 };
 
+struct PACKED log_Vibe {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float vibe_x, vibe_y, vibe_z;
+    uint32_t clipping_0, clipping_1, clipping_2;
+};
+
 struct PACKED log_RCIN {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -707,7 +715,10 @@ Format characters in the format string for binary log messages
     { LOG_PIDA_MSG, sizeof(log_PID), \
       "PIDA", "Qffffff",  "TimeUS,Des,P,I,D,FF,AFF" }, \
     { LOG_BAR2_MSG, sizeof(log_BARO), \
-      "BAR2",  "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }
+      "BAR2",  "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
+    { LOG_VIBE_MSG, sizeof(log_Vibe), \
+      "VIBE", "QfffIII",     "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }
+
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@@ -773,6 +784,7 @@ Format characters in the format string for binary log messages
 #define LOG_PIDP_MSG      180
 #define LOG_PIDY_MSG      181
 #define LOG_PIDA_MSG      182
+#define LOG_VIBE_MSG      183
 
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index c614dae6ba..b83d3fccae 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -9,6 +9,7 @@
 #include <AP_AHRS.h>
 #include "../AP_BattMonitor/AP_BattMonitor.h"
 #include <AP_Compass.h>
+#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -868,6 +869,23 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
 #endif
 }
 
+void DataFlash_Class::Log_Write_Vibration(const AP_InertialSensor &ins)
+{
+    uint64_t time_us = hal.scheduler->micros64();
+    Vector3f vibration = ins.get_vibration_levels();
+    struct log_Vibe pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
+        time_us     : time_us,
+        vibe_x      : vibration.x,
+        vibe_y      : vibration.y,
+        vibe_z      : vibration.z,
+        clipping_0  : ins.get_accel_clip_count(0),
+        clipping_1  : ins.get_accel_clip_count(1),
+        clipping_2  : ins.get_accel_clip_count(2)
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write a text message to the log
 void DataFlash_Class::Log_Write_Message(const char *message)
 {