From fcf29539de849caa55e69aa9541ccd91633f4b7d Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Thu, 28 Jul 2022 21:42:21 -0500
Subject: [PATCH] Plane: fix attitude/AOA logging and rates

---
 ArduPlane/ArduPlane.cpp | 31 ++++++++++++++++++-------------
 ArduPlane/Log.cpp       | 14 ++++----------
 ArduPlane/Plane.h       |  6 +++---
 3 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 82c820fa82..91d43b2cb9 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -101,9 +101,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #endif // CAMERA == ENABLED
     SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging,         0.2,    100, 111),
     SCHED_TASK(compass_save,          0.1,    200, 114),
-    SCHED_TASK(Log_Write_Fast,        400,    300, 117),
-    SCHED_TASK(update_logging1,        25,    300, 120),
-    SCHED_TASK(update_logging2,        25,    300, 123),
+    SCHED_TASK(Log_Write_FullRate,        400,    300, 117),
+    SCHED_TASK(update_logging10,        10,    300, 120),
+    SCHED_TASK(update_logging25,        25,    300, 123),
 #if HAL_SOARING_ENABLED
     SCHED_TASK(update_soaring,         50,    400, 126),
 #endif
@@ -226,24 +226,29 @@ void Plane::update_compass(void)
 /*
   do 10Hz logging
  */
-void Plane::update_logging1(void)
+void Plane::update_logging10(void)
 {
-    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
+    bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
         Log_Write_Attitude();
-    }
-
-    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
-        AP::ins().Write_IMU();
-
-    if (should_log(MASK_LOG_ATTITUDE_MED))
         ahrs.Write_AOA_SSA();
+    } else if (log_faster) {
+        ahrs.Write_AOA_SSA();
+    } 
 }
 
 /*
-  do 10Hz logging - part2
+  do 25Hz logging
  */
-void Plane::update_logging2(void)
+void Plane::update_logging25(void)
 {
+    // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
+    // highest rate selected wins
+    bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
+    if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
+        Log_Write_Attitude();
+    }
+
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
         AP::ins().write_notch_log_messages();
diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp
index 5b8bd3e3a7..fae1750883 100644
--- a/ArduPlane/Log.cpp
+++ b/ArduPlane/Log.cpp
@@ -49,17 +49,11 @@ void Plane::Log_Write_Attitude(void)
 }
 
 // do fast logging for plane
-void Plane::Log_Write_Fast(void)
+void Plane::Log_Write_FullRate(void)
 {
-    if (!should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
-        uint32_t now = AP_HAL::millis();
-        if (now - last_log_fast_ms < 40) {
-            // default to 25Hz
-            return;
-        }
-        last_log_fast_ms = now;
-    }
-    if (should_log(MASK_LOG_ATTITUDE_FAST | MASK_LOG_ATTITUDE_FULLRATE)) {
+    // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
+    // highest rate selected wins
+    if (should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
         Log_Write_Attitude();
     }
 }
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 85db9244ae..fafe2c1bab 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -868,7 +868,7 @@ private:
     // Log.cpp
     uint32_t last_log_fast_ms;
 
-    void Log_Write_Fast(void);
+    void Log_Write_FullRate(void);
     void Log_Write_Attitude(void);
     void Log_Write_Control_Tuning();
     void Log_Write_OFG_Guided();
@@ -1006,8 +1006,8 @@ private:
     void airspeed_ratio_update(void);
 #endif
     void compass_save(void);
-    void update_logging1(void);
-    void update_logging2(void);
+    void update_logging10(void);
+    void update_logging25(void);
     void update_control_mode(void);
     void update_fly_forward(void);
     void update_flight_stage();