Rover: Added logging of the guided commands

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;
 

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)

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);

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;
 }
 

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