From 9142f877e765588c38e876dda9a4fb929e437aeb Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 26 Apr 2020 11:51:49 +1000
Subject: [PATCH] AP_Mount: use logger Write method for logging Solo gimbal

Also document the messages
---
 libraries/AP_Mount/SoloGimbal.cpp | 103 +++++++++++++++++++-----------
 1 file changed, 67 insertions(+), 36 deletions(-)

diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp
index 7110fd003a..d26132fac4 100644
--- a/libraries/AP_Mount/SoloGimbal.cpp
+++ b/libraries/AP_Mount/SoloGimbal.cpp
@@ -384,49 +384,80 @@ void SoloGimbal::update_target(const Vector3f &newTarget)
 
 void SoloGimbal::write_logs()
 {
-    AP_Logger *logger = AP_Logger::get_singleton();
-    if (logger == nullptr) {
-        return;
-    }
+    AP_Logger &logger = AP::logger();
 
     const uint64_t tstamp = AP_HAL::micros64();
-    Vector3f eulerEst;
+
+    // @LoggerMessage: GMB1
+    // @Vehicles: Copter
+    // @Description: Solo Gimbal measurements
+    // @Field: TimeUS: Time since system startup
+    // @Field: dt: sum of time across measurements in this packet
+    // @Field: dax: delta-angle sum, x-axis
+    // @Field: day: delta-angle sum, y-axis
+    // @Field: daz: delta-angle sum, z-axis
+    // @Field: dvx: delta-velocity sum, x-axis
+    // @Field: dvy: delta-velocity sum, y-axis
+    // @Field: dvz: delta-velocity sum, z-axis
+    // @Field: jx: joint angle, x
+    // @Field: jy: joint angle, y
+    // @Field: jz: joint angle, z
+    logger.Write(
+        "GMB1",
+        "TimeUS,dt,dax,day,daz,dvx,dvy,dvz,jx,jy,jz",
+        "ssrrrEEELLL",
+        "FC000000000",
+        "Qffffffffff",
+        tstamp,
+        _log_dt,
+        _log_del_ang.x,
+        _log_del_ang.y,
+        _log_del_ang.z,
+        _log_del_vel.x,
+        _log_del_vel.y,
+        _log_del_vel.z,
+        _measurement.joint_angles.x,
+        _measurement.joint_angles.y,
+        _measurement.joint_angles.z
+        );
 
     Quaternion quatEst;
     _ekf.getQuat(quatEst);
+    Vector3f eulerEst;
     quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
 
-    struct log_Gimbal1 pkt1{
-        LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
-        time_us : tstamp,
-        delta_time      : _log_dt,
-        delta_angles_x  : _log_del_ang.x,
-        delta_angles_y  : _log_del_ang.y,
-        delta_angles_z  : _log_del_ang.z,
-        delta_velocity_x : _log_del_vel.x,
-        delta_velocity_y : _log_del_vel.y,
-        delta_velocity_z : _log_del_vel.z,
-        joint_angles_x  : _measurement.joint_angles.x,
-        joint_angles_y  : _measurement.joint_angles.y,
-        joint_angles_z  : _measurement.joint_angles.z
-    };
-    logger->WriteBlock(&pkt1, sizeof(pkt1));
-
-    struct log_Gimbal2 pkt2{
-        LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
-        time_us : tstamp,
-        est_sta : (uint8_t) _ekf.getStatus(),
-        est_x   : eulerEst.x,
-        est_y   : eulerEst.y,
-        est_z   : eulerEst.z,
-        rate_x  : _ang_vel_dem_rads.x,
-        rate_y  : _ang_vel_dem_rads.y,
-        rate_z  : _ang_vel_dem_rads.z,
-        target_x: _att_target_euler_rad.x,
-        target_y: _att_target_euler_rad.y,
-        target_z: _att_target_euler_rad.z
-    };
-    logger->WriteBlock(&pkt2, sizeof(pkt2));
+    // @LoggerMessage: GMB2
+    // @Vehicles: Copter
+    // @Description: Solo Gimbal estimation and demands
+    // @Field: TimeUS: Time since system startup
+    // @Field: es: Solo Gimbal EKF status bits
+    // @Field: ex: Solo Gimbal EKF estimate of gimbal angle, x-axis
+    // @Field: ey: Solo Gimbal EKF estimate of gimbal angle, y-axis
+    // @Field: ez: Solo Gimbal EKF estimate of gimbal angle, y-axis
+    // @Field: rx: Angular velocity demand around x-axis
+    // @Field: ry: Angular velocity demand around y-axis
+    // @Field: rz: Angular velocity demand around z-axis
+    // @Field: tx: Angular position target around x-axis
+    // @Field: ty: Angular position target around y-axis
+    // @Field: tz: Angular position target around z-axis
+    logger.Write(
+        "GMB2",
+        "TimeUS,es,ex,ey,ez,rx,ry,rz,tx,ty,tz",
+        "s-rrrEEELLL",
+        "F-000000000",
+        "QBfffffffff",
+        tstamp,
+        (uint8_t) _ekf.getStatus(),
+        eulerEst.x,
+        eulerEst.y,
+        eulerEst.z,
+        _ang_vel_dem_rads.x,
+        _ang_vel_dem_rads.y,
+        _ang_vel_dem_rads.z,
+        _att_target_euler_rad.x,
+        _att_target_euler_rad.y,
+        _att_target_euler_rad.z
+        );
 
     _log_dt = 0;
     _log_del_ang.zero();