Minor optimisations to PIDLOOP

src/main/fc/fc_main.c

@@ -82,8 +82,6 @@ enum {
 
 #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
 
-uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
-
 int16_t magHold;
 int16_t headFreeModeHold;
 
@@ -94,15 +92,10 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 
 static float throttlePIDAttenuation;
 
-uint16_t filteredCycleTime;
 bool isRXDataNew;
 static bool armingCalibrationWasInitialised;
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 
-float getThrottlePIDAttenuation(void) {
-    return throttlePIDAttenuation;
-}
-
 float getSetpointRate(int axis) {
     return setpointRate[axis];
 }
@@ -689,21 +682,19 @@ void processRx(timeUs_t currentTimeUs)
 #endif
 }
 
-void subTaskPidController(void)
+static void subTaskPidController(void)
 {
     uint32_t startTime;
-    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
+    if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
     // PID - note this is function pointer set by setPIDController()
-    pidController(
+    pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation);
-        &currentProfile->pidProfile,
+    DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
-        &accelerometerConfig()->accelerometerTrims
-    );
-    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
 }
 
-void subTaskMainSubprocesses(void)
+static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 {
-    const uint32_t startTime = micros();
+    uint32_t startTime;
+    if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 
     // Read out gyro temperature if used for telemmetry
     if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
@@ -763,25 +754,28 @@ void subTaskMainSubprocesses(void)
 
 #ifdef BLACKBOX
     if (!cliMode && feature(FEATURE_BLACKBOX)) {
-        handleBlackbox(startTime);
+        handleBlackbox(currentTimeUs);
     }
 #endif
 
 #ifdef TRANSPONDER
-    transponderUpdate(startTime);
+    transponderUpdate(currentTimeUs);
 #endif
     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 }
 
-void subTaskMotorUpdate(void)
+static void subTaskMotorUpdate(void)
 {
-    const uint32_t startTime = micros();
+    uint32_t startTime;
     if (debugMode == DEBUG_CYCLETIME) {
+        startTime = micros();
         static uint32_t previousMotorUpdateTime;
         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
         debug[2] = currentDeltaTime;
         debug[3] = currentDeltaTime - targetPidLooptime;
         previousMotorUpdateTime = startTime;
+    } else if (debugMode == DEBUG_PIDLOOP) {
+        startTime = micros();
     }
 
     mixTable(&currentProfile->pidProfile);
@@ -813,20 +807,16 @@ uint8_t setPidUpdateCountDown(void)
 // Function for loop trigger
 void taskMainPidLoop(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
-
     static bool runTaskMainSubprocesses;
     static uint8_t pidUpdateCountdown;
 
-    cycleTime = getTaskDeltaTime(TASK_SELF);
-
     if (debugMode == DEBUG_CYCLETIME) {
-        debug[0] = cycleTime;
+        debug[0] = getTaskDeltaTime(TASK_SELF);
         debug[1] = averageSystemLoadPercent;
     }
 
     if (runTaskMainSubprocesses) {
-        subTaskMainSubprocesses();
+        subTaskMainSubprocesses(currentTimeUs);
         runTaskMainSubprocesses = false;
     }
 
@@ -836,9 +826,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     // 2 - subTaskMainSubprocesses()
     // 3 - subTaskMotorUpdate()
     uint32_t startTime;
-    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
+    if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
     gyroUpdate();
-    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;}
+    DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
 
     if (pidUpdateCountdown) {
         pidUpdateCountdown--;

src/main/fc/fc_main.h

@@ -34,7 +34,6 @@ void updateLEDs(void);
 void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
-float getThrottlePIDAttenuation(void);
 float getSetpointRate(int axis);
 float getRcDeflection(int axis);
 float getRcDeflectionAbs(int axis);

src/main/fc/fc_msp.c

@@ -614,7 +614,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_STATUS_EX:
-        sbufWriteU16(dst, cycleTime);
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -638,7 +638,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_STATUS:
-        sbufWriteU16(dst, cycleTime);
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else

src/main/fc/serial_cli.c

@@ -105,8 +105,6 @@ uint8_t cliMode = 0;
 #include "telemetry/frsky.h"
 #include "telemetry/telemetry.h"
 
-extern uint16_t cycleTime; // FIXME dependency on mw.c
-
 static serialPort_t *cliPort;
 static bufWriter_t *cliWriter;
 static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
@@ -3217,7 +3215,7 @@ static void cliStatus(char *cmdline)
 #endif
     cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem());
 
-    cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
+    cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", getTaskDeltaTime(TASK_GYROPID), i2cErrorCounter, sizeof(master_t));
 
 #ifdef USE_SDCARD
     cliSdInfo(NULL);
@@ -3241,7 +3239,7 @@ static void cliTasks(char *cmdline)
             int taskFrequency;
             int subTaskFrequency;
             if (taskId == TASK_GYROPID) {
-                subTaskFrequency = (int)(1000000.0f / ((float)cycleTime));
+                subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
                 taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
                 if (pidConfig()->pid_process_denom > 1) {
                     cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);

src/main/flight/pid.c

@@ -157,7 +157,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
 }
 
-float calcHorizonLevelStrength(void) {
+static float calcHorizonLevelStrength(void) {
     float horizonLevelStrength = 0.0f;
     if (horizonTransition > 0.0f) {
         const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
@@ -167,7 +167,7 @@ float calcHorizonLevelStrength(void) {
     return horizonLevelStrength;
 }
 
-float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
+static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
     // calculate error angle and limit the angle to the max inclination
     float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
 #ifdef GPS
@@ -187,7 +187,7 @@ float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims
     return currentPidSetpoint;
 }
 
-float accelerationLimit(int axis, float currentPidSetpoint) {
+static float accelerationLimit(int axis, float currentPidSetpoint) {
     static float previousSetpoint[3];
     const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
 
@@ -200,14 +200,12 @@ float accelerationLimit(int axis, float currentPidSetpoint) {
 
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
-void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
+void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor)
 {
     static float previousRateError[2];
     static float previousSetpoint[3];
 
     // ----------PID controller----------
-    const float tpaFactor = getThrottlePIDAttenuation();
-
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
         float currentPidSetpoint = getSetpointRate(axis);
 

src/main/flight/pid.h

@@ -88,7 +88,7 @@ typedef struct pidConfig_s {
 } pidConfig_t;
 
 union rollAndPitchTrims_u;
-void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
+void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor);
 
 extern float axisPIDf[3];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -45,6 +45,8 @@
 #include "rx/rx.h"
 #include "rx/msp.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
@@ -269,7 +271,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 
 extern volatile uint8_t CRC8;
 extern volatile bool coreProReady;
-extern uint16_t cycleTime; // FIXME dependency on mw.c
 
 // this is calculated at startup based on enabled features.
 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
@@ -562,7 +563,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             break;
 
         case BST_STATUS:
-            bstWrite16(cycleTime);
+            bstWrite16(getTaskDeltaTime(TASK_GYROPID));
 #ifdef USE_I2C
             bstWrite16(i2cGetErrorCounter());
 #else
@@ -691,7 +692,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             break;
         case BST_LOOP_TIME:
             //bstWrite16(masterConfig.looptime);
-            bstWrite16(cycleTime);
+            bstWrite16(getTaskDeltaTime(TASK_GYROPID));
             break;
         case BST_RC_TUNING:
             bstWrite8(currentControlRateProfile->rcRate8);
@@ -1043,7 +1044,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             break;
         case BST_SET_LOOP_TIME:
             //masterConfig.looptime = bstRead16();
-            cycleTime = bstRead16();
+            bstRead16();
             break;
         case BST_SET_PID_CONTROLLER:
             break;