From d1d0ab41d685436e80cfce3efc891a2039319ccd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 9 Aug 2021 16:21:18 +1000
Subject: [PATCH] Plane: added option to log PIDs at full rate

useful for tuning quadplanes
---
 ArduPlane/ArduPlane.cpp  |  2 +-
 ArduPlane/Log.cpp        | 10 +++++++++-
 ArduPlane/Parameters.cpp |  2 +-
 ArduPlane/Plane.h        |  2 ++
 ArduPlane/defines.h      |  1 +
 5 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 43f4b00a20..b5c96a1619 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -83,7 +83,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #endif // CAMERA == ENABLED
     SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging,         0.2,    100),
     SCHED_TASK(compass_save,          0.1,    200),
-    SCHED_TASK(Log_Write_Fast,         25,    300),
+    SCHED_TASK(Log_Write_Fast,        400,    300),
     SCHED_TASK(update_logging1,        25,    300),
     SCHED_TASK(update_logging2,        25,    300),
 #if HAL_SOARING_ENABLED
diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp
index f5701f2a98..f12233eeac 100644
--- a/ArduPlane/Log.cpp
+++ b/ArduPlane/Log.cpp
@@ -47,7 +47,15 @@ void Plane::Log_Write_Attitude(void)
 // do fast logging for plane
 void Plane::Log_Write_Fast(void)
 {
-    if (should_log(MASK_LOG_ATTITUDE_FAST)) {
+    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)) {
         Log_Write_Attitude();
     }
 }
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index bd29c62604..1c10ba64af 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -637,7 +637,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: LOG_BITMASK
     // @DisplayName: Log bitmask
     // @Description: Bitmap of what on-board log types to enable. This value is made up of the sum of each of the log types you want to be saved. It is usually best just to enable all log types by setting this to 65535. The individual bits are ATTITUDE_FAST=1, ATTITUDE_MEDIUM=2, GPS=4, PerformanceMonitoring=8, ControlTuning=16, NavigationTuning=32, Mode=64, IMU=128, Commands=256, Battery=512, Compass=1024, TECS=2048, Camera=4096, RCandServo=8192, Sonar=16384, Arming=32768, FullLogs=65535
-    // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:MODE,7:IMU,8:CMD,9:CURRENT,10:COMPASS,11:TECS,12:CAMERA,13:RC,14:SONAR,15:ARM/DISARM,19:IMU_RAW
+    // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:MODE,7:IMU,8:CMD,9:CURRENT,10:COMPASS,11:TECS,12:CAMERA,13:RC,14:SONAR,15:ARM/DISARM,19:IMU_RAW,20:ATTITUDE_FULLRATE
     // @User: Advanced
     GSCALAR(log_bitmask,            "LOG_BITMASK",    DEFAULT_LOG_BITMASK),
 
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index a4e19a1230..06fde5c7fa 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -856,6 +856,8 @@ private:
     void calc_nav_yaw_ground(void);
 
     // Log.cpp
+    uint32_t last_log_fast_ms;
+
     void Log_Write_Fast(void);
     void Log_Write_Attitude(void);
     void Log_Write_Startup(uint8_t type);
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 5e2ac140af..1bd9ade168 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -116,6 +116,7 @@ enum log_messages {
 #define MASK_LOG_SONAR                  (1<<14)
 // #define MASK_LOG_ARM_DISARM             (1<<15)
 #define MASK_LOG_IMU_RAW                (1UL<<19)
+#define MASK_LOG_ATTITUDE_FULLRATE      (1U<<20)
 
 // altitude control algorithms
 enum {