From 515ceb3ccd98ced58eabe8117bc4d5b2b5278a42 Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Thu, 24 Nov 2016 21:29:50 +1100
Subject: [PATCH] Rover: Added logging of the guided commands

---
 APMrover2/APMrover2.cpp      |  1 +
 APMrover2/Log.cpp            | 33 +++++++++++++++++++++++++++++++++
 APMrover2/Rover.h            |  1 +
 APMrover2/commands_logic.cpp |  4 +++-
 APMrover2/defines.h          |  1 +
 5 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 8fb2de0dad..3756853ac2 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -447,6 +447,7 @@ void Rover::update_current_mode(void)
                 calc_lateral_acceleration();
                 calc_nav_steer();
                 calc_throttle(g.speed_cruise);
+                Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), Vector3f(g.speed_cruise, channel_throttle->get_servo_out(), 0));
             }
             break;
 
diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
index 5e4c2fc262..3069264e3d 100644
--- a/APMrover2/Log.cpp
+++ b/APMrover2/Log.cpp
@@ -387,6 +387,37 @@ void Rover::Log_Write_Home_And_Origin()
     }
 }
 
+// guided mode logging
+struct PACKED log_GuidedTarget {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t type;
+    float pos_target_x;
+    float pos_target_y;
+    float pos_target_z;
+    float vel_target_x;
+    float vel_target_y;
+    float vel_target_z;
+};
+
+// Write a Guided mode target
+void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
+{
+    struct log_GuidedTarget pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
+        time_us         : AP_HAL::micros64(),
+        type            : target_type,
+        pos_target_x    : pos_target.x,
+        pos_target_y    : pos_target.y,
+        pos_target_z    : pos_target.z,
+        vel_target_x    : vel_target.x,
+        vel_target_y    : vel_target.y,
+        vel_target_z    : vel_target.z
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+
 const LogStructure Rover::log_structure[] = {
     LOG_COMMON_STRUCTURES,
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
@@ -403,6 +434,8 @@ const LogStructure Rover::log_structure[] = {
       "ARM", "QBH", "TimeUS,ArmState,ArmChecks" },
     { LOG_STEERING_MSG, sizeof(log_Steering),             
       "STER", "Qff",   "TimeUS,Demanded,Achieved" },
+    { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
+      "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
 };
 
 void Rover::log_init(void)
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 37f6ca5bd9..4e980da091 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -545,6 +545,7 @@ private:
     void nav_set_yaw_speed();
     bool in_stationary_loiter(void);
     void set_loiter_active(const AP_Mission::Mission_Command& cmd);
+    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
 
 public:
     bool print_log_menu(void);
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 4935842c42..fe17429b5c 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -375,7 +375,8 @@ void Rover::nav_set_yaw_speed()
         return;
     }
 
-    channel_steer->set_servo_out(steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle));
+    int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
+    channel_steer->set_servo_out(steering);
 
     // speed param in the message gives speed as a proportion of cruise speed.
     // 0.5 would set speed to the cruise speed
@@ -383,6 +384,7 @@ void Rover::nav_set_yaw_speed()
     float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
     calc_throttle(target_speed);
 
+    Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0, 0), Vector3f(target_speed, 0, 0));
     return;
 }
 
diff --git a/APMrover2/defines.h b/APMrover2/defines.h
index c9a0f1ad00..018bd20152 100644
--- a/APMrover2/defines.h
+++ b/APMrover2/defines.h
@@ -59,6 +59,7 @@ enum GuidedMode {
 #define LOG_SONAR_MSG 		    0x07
 #define LOG_ARM_DISARM_MSG      0x08
 #define LOG_STEERING_MSG        0x0D
+#define LOG_GUIDEDTARGET_MSG    0x0E
 
 #define TYPE_AIRSTART_MSG		0x00
 #define TYPE_GROUNDSTART_MSG	0x01