From f4f13bbe6b6e14844bebd22bbe0d07e0a3c5b943 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 2 Aug 2016 14:41:39 +0900
Subject: [PATCH] Copter: add throw mode logging

---
 ArduCopter/Copter.h          |  5 ++++-
 ArduCopter/Log.cpp           | 36 ++++++++++++++++++++++++++++++++++++
 ArduCopter/control_throw.cpp | 24 ++++++++++++++++++++++++
 ArduCopter/defines.h         |  1 +
 4 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index c65667a991..4bc94ba349 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -277,8 +277,10 @@ private:
     // throw mode state
     struct {
         ThrowModeStage stage;
+        ThrowModeStage prev_stage;
+        uint32_t last_log_ms;
         bool nextmode_attempted;
-    } throw_state = {Throw_Disarmed, false};
+    } throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false};
 
     uint32_t precland_last_update_ms;
 
@@ -705,6 +707,7 @@ private:
 #endif
     void Log_Write_Precland();
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok);
     void Log_Write_Vehicle_Startup_Messages();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void start_logging() ;
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index 359f5f3812..ae2d047d16 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -714,6 +714,40 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+// precision landing logging
+struct PACKED log_Throw {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t stage;
+    float velocity;
+    float velocity_z;
+    float accel;
+    float ef_accel_z;
+    uint8_t throw_detect;
+    uint8_t attitude_ok;
+    uint8_t height_ok;
+    uint8_t pos_ok;
+};
+
+// Write a Throw mode details
+void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool pos_ok)
+{
+    struct log_Throw pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_THROW_MSG),
+        time_us         : AP_HAL::micros64(),
+        stage           : (uint8_t)stage,
+        velocity        : velocity,
+        velocity_z      : velocity_z,
+        accel           : accel,
+        ef_accel_z      : ef_accel_z,
+        throw_detect    : throw_detect,
+        attitude_ok     : attitude_ok,
+        height_ok       : height_ok,
+        pos_ok          : pos_ok
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
 const struct LogStructure Copter::log_structure[] = {
     LOG_COMMON_STRUCTURES,
 #if AUTOTUNE_ENABLED == ENABLED
@@ -754,6 +788,8 @@ const struct LogStructure Copter::log_structure[] = {
       "PL",    "QBffffff",    "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
       "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
+    { LOG_THROW_MSG, sizeof(log_Throw),
+      "THRO",  "QBffffbbbb",  "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk" },
 };
 
 #if CLI_ENABLED == ENABLED
diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp
index 5b5c88ac1f..348cc977b4 100644
--- a/ArduCopter/control_throw.cpp
+++ b/ArduCopter/control_throw.cpp
@@ -191,6 +191,30 @@ void Copter::throw_run()
 
         break;
     }
+
+    // log at 10hz or if stage changes
+    uint32_t now = AP_HAL::millis();
+    if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) {
+        throw_state.prev_stage = throw_state.stage;
+        throw_state.last_log_ms = now;
+        float velocity = inertial_nav.get_velocity().length();
+        float velocity_z = inertial_nav.get_velocity().z;
+        float accel = ins.get_accel().length();
+        float ef_accel_z = ahrs.get_accel_ef().z;
+        bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected();
+        bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good();
+        bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good();
+        bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good();
+        Log_Write_Throw(throw_state.stage,
+                        velocity,
+                        velocity_z,
+                        accel,
+                        ef_accel_z,
+                        throw_detect,
+                        attitude_ok,
+                        height_ok,
+                        pos_ok);
+    }
 }
 
 bool Copter::throw_detected()
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index ce5afec63b..93660fca7a 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -293,6 +293,7 @@ enum ThrowModeType {
 #define LOG_HELI_MSG                    0x20
 #define LOG_PRECLAND_MSG                0x21
 #define LOG_GUIDEDTARGET_MSG            0x22
+#define LOG_THROW_MSG                   0x23
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)