From d398e567a36be41ec9940f28599f992e667b9ca1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Oct 2022 12:49:02 +1100
Subject: [PATCH] AP_AHRS: added ATSC logging

log scale factors for angle P scaling when not == 1.0
---
 libraries/AP_AHRS/AP_AHRS_Logging.cpp | 18 +++++++++++++++++-
 libraries/AP_AHRS/LogStructure.h      | 19 ++++++++++++++++++-
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp
index 3c5316a991..cef4f838f7 100644
--- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp
@@ -145,9 +145,10 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
 {
     const Vector3f &rate_targets = attitude_control.rate_bf_targets();
     const Vector3f &accel_target = pos_control.get_accel_target_cmss();
+    const auto timeus = AP_HAL::micros64();
     const struct log_Rate pkt_rate{
         LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
-        time_us         : AP_HAL::micros64(),
+        time_us         : timeus,
         control_roll    : degrees(rate_targets.x),
         roll            : degrees(get_gyro().x),
         roll_out        : motors.get_roll()+motors.get_roll_ff(),
@@ -162,4 +163,19 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
         accel_out       : motors.get_throttle()
     };
     AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
+
+    /*
+      log P gain scale if not == 1.0
+     */
+    const Vector3f &scale = attitude_control.get_angle_P_scale_logging();
+    if (!is_equal(scale.x,1.0f) || !is_equal(scale.y,1.0f) || !is_equal(scale.z,1.0f)) {
+        const struct log_ATSC pkt_ATSC {
+            LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
+            time_us  : timeus,
+            scaleP_x : scale.x,
+            scaleP_y : scale.y,
+            scaleP_z : scale.z,
+        };
+        AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
+    }
 }
diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h
index c8eb04f2a4..9434ebfe7b 100644
--- a/libraries/AP_AHRS/LogStructure.h
+++ b/libraries/AP_AHRS/LogStructure.h
@@ -8,7 +8,8 @@
     LOG_ATTITUDE_MSG, \
     LOG_ORGN_MSG, \
     LOG_POS_MSG, \
-    LOG_RATE_MSG
+    LOG_RATE_MSG, \
+    LOG_ATSC_MSG
 
 // @LoggerMessage: AHR2
 // @Description: Backup AHRS data
@@ -168,6 +169,20 @@ struct PACKED log_Video_Stabilisation {
     float Q4;
 };
 
+// @LoggerMessage: ATSC
+// @Description: Scale factors for attitude controller
+// @Field: TimeUS: Time since system startup
+// @Field: AngPScX: Angle P scale X
+// @Field: AngPScY: Angle P scale Y
+// @Field: AngPScZ: Angle P scale Z
+struct PACKED log_ATSC {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float scaleP_x;
+    float scaleP_y;
+    float scaleP_z;
+};
+
 
 #define LOG_STRUCTURE_FROM_AHRS \
     { LOG_AHR2_MSG, sizeof(log_AHRS), \
@@ -182,6 +197,8 @@ struct PACKED log_Video_Stabilisation {
         "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_ATSC_MSG, sizeof(log_ATSC), \
+        "ATSC", "Qfff",  "TimeUS,AngPScX,AngPScY,AngPScZ", "s---", "F000" , true }, \
     { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \
         "VSTB", "Qffffffffff",  "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" },