DataFlash: added Log_Write_Rate()

needed for quadplane as well as copter so should be in common code

libraries/DataFlash/DataFlash.h

@@ -20,6 +20,7 @@
 #include <AP_RPM/AP_RPM.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <DataFlash/LogStructure.h>
+#include <AP_Motors/AP_Motors.h>
 #include <stdint.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@@ -37,6 +38,10 @@ enum DataFlash_Backend_Type {
     DATAFLASH_BACKEND_BOTH = 3,
 };
 
+// fwd declarations to avoid include errors
+class AC_AttitudeControl;
+class AC_PosControl;
+
 class DataFlash_Class
 {
     friend class DataFlash_Backend; // for _num_types
@@ -123,6 +128,10 @@ public:
                                const AP_Mission::Mission_Command &cmd);
     void Log_Write_Origin(uint8_t origin_type, const Location &loc);
     void Log_Write_RPM(const AP_RPM &rpm_sensor);
+    void Log_Write_Rate(const AP_AHRS &ahrs,
+                        const AP_Motors &motors,
+                        const AC_AttitudeControl &attitude_control,
+                        const AC_PosControl &pos_control);
 
     // This structure provides information on the internal member data of a PID for logging purposes
     struct PID_Info {

libraries/DataFlash/LogFile.cpp

@@ -9,6 +9,9 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_Param/AP_Param.h>
+#include <AP_Motors/AP_Motors.h>
+#include <AC_AttitudeControl/AC_AttitudeControl.h>
+#include <AC_AttitudeControl/AC_PosControl.h>
 
 #include "DataFlash.h"
 #include "DataFlash_SITL.h"
@@ -1795,3 +1798,30 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
     };
     WriteBlock(&pkt, sizeof(pkt));
 }
+
+// Write an rate packet
+void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
+                                     const AP_Motors &motors,
+                                     const AC_AttitudeControl &attitude_control,
+                                     const AC_PosControl &pos_control)
+{
+    const Vector3f &rate_targets = attitude_control.rate_bf_targets();
+    const Vector3f &accel_target = pos_control.get_accel_target();
+    struct log_Rate pkt_rate = {
+        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
+        time_us         : AP_HAL::micros64(),
+        control_roll    : (float)rate_targets.x,
+        roll            : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
+        roll_out        : motors.get_roll(),
+        control_pitch   : (float)rate_targets.y,
+        pitch           : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
+        pitch_out       : motors.get_pitch(),
+        control_yaw     : (float)rate_targets.z,
+        yaw             : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
+        yaw_out         : motors.get_yaw(),
+        control_accel   : (float)accel_target.z,
+        accel           : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
+        accel_out       : motors.get_throttle()
+    };
+    WriteBlock(&pkt_rate, sizeof(pkt_rate));
+}

libraries/DataFlash/LogStructure.h

@@ -626,6 +626,23 @@ struct PACKED log_RPM {
     float rpm2;
 };
 
+struct PACKED log_Rate {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float   control_roll;
+    float   roll;
+    float   roll_out;
+    float   control_pitch;
+    float   pitch;
+    float   pitch_out;
+    float   control_yaw;
+    float   yaw;
+    float   yaw_out;
+    float   control_accel;
+    float   accel;
+    float   accel_out;
+};
+
 // #if SBP_HW_LOGGING
 
 struct PACKED log_SbpLLH {
@@ -860,7 +877,9 @@ Format characters in the format string for binary log messages
     { LOG_GIMBAL2_MSG, sizeof(log_Gimbal2), \
       "GMB2", "IBfffffffff", "TimeMS,es,ex,ey,ez,rx,ry,rz,tx,ty,tz" }, \
     { LOG_GIMBAL3_MSG, sizeof(log_Gimbal3), \
-      "GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }
+      "GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }, \
+    { LOG_RATE_MSG, sizeof(log_Rate), \
+      "RATE", "Qffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }
 
 // #if SBP_HW_LOGGING
 #define LOG_SBP_STRUCTURES \
@@ -973,9 +992,7 @@ enum LogMessages {
     LOG_GIMBAL1_MSG,
     LOG_GIMBAL2_MSG,
     LOG_GIMBAL3_MSG,
-
-// message types 211 to 220 reversed for autotune use
-
+    LOG_RATE_MSG,
 };
 
 enum LogOriginType {