AP_AHRS: add video stab log message

libraries/AP_AHRS/AP_AHRS.h

@@ -432,6 +432,7 @@ public:
     };
     void Write_Origin(LogOriginType origin_type, const Location &loc) const; 
     void Write_POS(void) const;
+    void write_video_stabilisation() const;
 
     // return a smoothed and corrected gyro vector in radians/second
     // using the latest ins data (which may not have been consumed by

libraries/AP_AHRS/AP_AHRS_Logging.cpp

@@ -97,6 +97,29 @@ void AP_AHRS::Write_POS() const
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write a packet for video stabilisation
+void AP_AHRS::write_video_stabilisation() const
+{
+    Quaternion current_attitude;
+    get_quat_body_to_ned(current_attitude);
+    Vector3f accel = get_accel() - get_accel_bias();
+    const struct log_Video_Stabilisation pkt {
+        LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),
+        time_us         : AP_HAL::micros64(),
+        gyro_x          : _gyro_estimate.x,
+        gyro_y          : _gyro_estimate.y,
+        gyro_z          : _gyro_estimate.z,
+        accel_x         : accel.x,
+        accel_y         : accel.y,
+        accel_z         : accel.z,
+        Q1              : current_attitude.q1,
+        Q2              : current_attitude.q2,
+        Q3              : current_attitude.q3,
+        Q4              : current_attitude.q4,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write an attitude view packet
 void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
 {

libraries/AP_AHRS/LogStructure.h

@@ -153,7 +153,7 @@ struct PACKED log_Rate {
 // @Field: Q3: Estimated attitude quaternion component 3
 // @Field: Q4: Estimated attitude quaternion component 4
 
-struct PACKED log_Gyroflow {
+struct PACKED log_Video_Stabilisation {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     float gyro_x;
@@ -182,6 +182,6 @@ struct PACKED log_Gyroflow {
         "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \
     { LOG_RATE_MSG, sizeof(log_Rate), \
         "RATE", "Qffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut", "skk-kk-kk-oo-", "F?????????BB-" , true }, \
-    { LOG_GYROFLOW_MSG, sizeof(log_Gyroflow), \
+    { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \
         "VSTB", "Qffffffffff",  "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" },