From 8c7c72c5dc4cd69c24546121b8f20e5a469019a4 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 4 May 2016 10:12:11 +0100
Subject: [PATCH] Tidy of main pid loop.

---
 src/main/flight/imu.c |  5 -----
 src/main/flight/imu.h |  2 --
 src/main/mw.c         | 50 ++++++++++++++++---------------------------
 3 files changed, 19 insertions(+), 38 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index d72331ddd..0906c4ac4 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -419,11 +419,6 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
     }
 }
 
-void imuUpdateGyro(void)
-{
-    gyroUpdate();
-}
-
 void imuUpdateAttitude(void)
 {
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 3f64b5c50..6dc0a1f6b 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -79,7 +79,6 @@ void imuConfigure(
 float getCosTiltAngle(void);
 void calculateEstimatedAltitude(uint32_t currentTime);
 void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
-void imuUpdateGyro(void);
 void imuUpdateAttitude(void);
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
@@ -88,5 +87,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
 int16_t imuCalculateHeading(t_fp_vector *vec);
 
 void imuResetAccelerationSum(void);
-void imuUpdateGyro(void);
 void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims);
diff --git a/src/main/mw.c b/src/main/mw.c
index 7e8dfcd38..ae407cf71 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -653,13 +653,8 @@ void processRx(void)
 
 }
 
-#if defined(BARO) || defined(SONAR)
-static bool haveProcessedAnnexCodeOnce = false;
-#endif
-
-void taskMainPidLoop(void)
+void subTaskPidController(void)
 {
-
     // PID - note this is function pointer set by setPIDController()
     pid_controller(
         &currentProfile->pidProfile,
@@ -668,20 +663,14 @@ void taskMainPidLoop(void)
         &masterConfig.accelerometerTrims,
         &masterConfig.rxConfig
     );
-
-    mixTable();
 }
 
-void subTasksMainPidLoop(void) {
+void subTaskMainSubprocesses(void) {
 
     if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
         filterRc();
     }
 
-    #if defined(BARO) || defined(SONAR)
-        haveProcessedAnnexCodeOnce = true;
-    #endif
-
     #ifdef MAG
             if (sensors(SENSOR_MAG)) {
                 updateMagHold();
@@ -743,7 +732,8 @@ void subTasksMainPidLoop(void) {
     #endif
 }
 
-void taskMotorUpdate(void) {
+void subTaskMotorUpdate(void)
+{
     if (debugMode == DEBUG_CYCLETIME) {
         static uint32_t previousMotorUpdateTime;
         uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
@@ -752,6 +742,8 @@ void taskMotorUpdate(void) {
         previousMotorUpdateTime = micros();
     }
 
+    mixTable();
+
 #ifdef USE_SERVOS
     filterServos();
     writeServos();
@@ -771,11 +763,12 @@ uint8_t setPidUpdateCountDown(void) {
 }
 
 // Function for loop trigger
-void taskMainPidLoopCheck(void) {
+void taskMainPidLoopCheck(void)
+{
     static uint32_t previousTime;
     static bool runTaskMainSubprocesses;
 
-    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+    const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 
     cycleTime = micros() - previousTime;
     previousTime = micros();
@@ -790,18 +783,18 @@ void taskMainPidLoopCheck(void) {
             static uint8_t pidUpdateCountdown;
 
             if (runTaskMainSubprocesses) {
-                subTasksMainPidLoop();
+                subTaskMainSubprocesses();
                 runTaskMainSubprocesses = false;
             }
 
-            imuUpdateGyro();
+            gyroUpdate();
 
             if (pidUpdateCountdown) {
                 pidUpdateCountdown--;
             } else {
                 pidUpdateCountdown = setPidUpdateCountDown();
-                taskMainPidLoop();
-                taskMotorUpdate();
+                subTaskPidController();
+                subTaskMotorUpdate();
                 runTaskMainSubprocesses = true;
             }
 
@@ -862,24 +855,19 @@ void taskUpdateRxMain(void)
     processRx();
     isRXDataNew = true;
 
+    // the 'annexCode' initialses rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
+    annexCode();
 #ifdef BARO
-    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
-    if (haveProcessedAnnexCodeOnce) {
-        if (sensors(SENSOR_BARO)) {
-            updateAltHoldState();
-        }
+    if (sensors(SENSOR_BARO)) {
+        updateAltHoldState();
     }
 #endif
 
 #ifdef SONAR
-    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
-    if (haveProcessedAnnexCodeOnce) {
-        if (sensors(SENSOR_SONAR)) {
-            updateSonarAltHoldState();
-        }
+    if (sensors(SENSOR_SONAR)) {
+        updateSonarAltHoldState();
     }
 #endif
-    annexCode();
 }
 
 #ifdef GPS