From d7d7574d2e24cdbbaabf879e3a858f2c1f721993 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 21 Apr 2022 12:41:08 +0100
Subject: [PATCH] Blimp: remove fast loop and convert to tasks

---
 Blimp/Blimp.cpp | 55 +++++++++++++++++--------------------------------
 Blimp/Blimp.h   |  1 -
 Blimp/Log.cpp   |  4 ++++
 3 files changed, 23 insertions(+), 37 deletions(-)

diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp
index 2fbde58cf0..9bab577b39 100644
--- a/Blimp/Blimp.cpp
+++ b/Blimp/Blimp.cpp
@@ -22,10 +22,10 @@
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 #define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Blimp, &blimp, func, rate_hz, max_time_micros, priority)
+#define FAST_TASK(func) FAST_TASK_CLASS(Blimp, &blimp, func)
 
 /*
-  scheduler table - all regular tasks apart from the fast_loop()
-  should be listed here.
+  scheduler table - all tasks should be listed here.
 
   All entries in this table must be ordered by priority.
 
@@ -49,6 +49,23 @@ SCHED_TASK_CLASS arguments:
 
  */
 const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
+    // update INS immediately to get current gyro data populated
+    FAST_TASK_CLASS(AP_InertialSensor, &blimp.ins, update),
+    // send outputs to the motors library immediately
+    FAST_TASK(motors_output),
+     // run EKF state estimator (expensive)
+    FAST_TASK(read_AHRS),
+    // Inertial Nav
+    FAST_TASK(read_inertia),
+    // check if ekf has reset target heading or position
+    FAST_TASK(check_ekf_reset),
+    // run the attitude controllers
+    FAST_TASK(update_flight_mode),
+    // update home from EKF if necessary
+    FAST_TASK(update_home_from_EKF),
+    // log sensor health
+    FAST_TASK(Log_Sensor_Health),
+
     SCHED_TASK(rc_loop,              100,    130,   3),
     SCHED_TASK(throttle_loop,         50,     75,   6),
     SCHED_TASK_CLASS(AP_GPS, &blimp.gps, update, 50, 200,   9),
@@ -93,40 +110,6 @@ void Blimp::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
 
 constexpr int8_t Blimp::_failsafe_priorities[4];
 
-// Main loop
-void Blimp::fast_loop()
-{
-    // update INS immediately to get current gyro data populated
-    ins.update();
-
-    // send outputs to the motors library immediately
-    motors_output();
-
-    // run EKF state estimator (expensive)
-    // --------------------
-    read_AHRS();
-
-    // Inertial Nav
-    // --------------------
-    read_inertia();
-
-    // check if ekf has reset target heading or position
-    check_ekf_reset();
-
-    // run the attitude controllers
-    update_flight_mode();
-
-    // update home from EKF if necessary
-    update_home_from_EKF();
-
-    // log sensor health
-    if (should_log(MASK_LOG_ANY)) {
-        Log_Sensor_Health();
-    }
-
-    AP_Vehicle::fast_loop(); //just does gyro fft
-}
-
 // rc_loops - reads user input from transmitter/receiver
 // called at 100hz
 void Blimp::rc_loop()
diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h
index 0325a5d2bb..0437de422c 100644
--- a/Blimp/Blimp.h
+++ b/Blimp/Blimp.h
@@ -297,7 +297,6 @@ private:
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
-    void fast_loop() override;
     void rc_loop();
     void throttle_loop();
     void update_batt_compass(void);
diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp
index a4593304e1..23e2c308fe 100644
--- a/Blimp/Log.cpp
+++ b/Blimp/Log.cpp
@@ -235,6 +235,10 @@ tuning_max     : tune_max
 // logs when baro or compass becomes unhealthy
 void Blimp::Log_Sensor_Health()
 {
+    if (!should_log(MASK_LOG_ANY)) {
+        return;
+    }
+
     // check baro
     if (sensor_health.baro != barometer.healthy()) {
         sensor_health.baro = barometer.healthy();