Added new debug mode to time pidloop subtasks.

src/main/debug.h

@@ -42,10 +42,13 @@ extern uint32_t sectionTimes[2][4];
 #endif
 
 typedef enum {
-    DEBUG_CYCLETIME = 1,
+    DEBUG_NONE,
+    DEBUG_CYCLETIME,
     DEBUG_BATTERY,
     DEBUG_GYRO,
     DEBUG_ACCELEROMETER,
     DEBUG_MIXER,
-    DEBUG_AIRMODE
+    DEBUG_AIRMODE,
+    DEBUG_PIDLOOP,
+    DEBUG_COUNT
 } debugType_e;

src/main/flight/pid.c

@@ -62,9 +62,10 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 uint8_t PIDweight[3];
 
 static int32_t errorGyroI[3], errorGyroILimit[3];
+#ifndef SKIP_PID_LUXFLOAT
 static float errorGyroIf[3], errorGyroIfLimit[3];
-static int32_t errorAngleI[2];
+#endif
-static float errorAngleIf[2];
+
 static bool lowThrottlePidReduction;
 
 static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@@ -103,22 +104,15 @@ uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) {
     return dynamicKp;
 }
 
-void pidResetErrorAngle(void)
-{
-    errorAngleI[ROLL] = 0;
-    errorAngleI[PITCH] = 0;
-
-    errorAngleIf[ROLL] = 0.0f;
-    errorAngleIf[PITCH] = 0.0f;
-}
-
 void pidResetErrorGyroState(uint8_t resetOption)
 {
     if (resetOption >= RESET_ITERM) {
         int axis;
         for (axis = 0; axis < 3; axis++) {
             errorGyroI[axis] = 0;
+#ifndef SKIP_PID_LUXFLOAT
             errorGyroIf[axis] = 0.0f;
+#endif
         }
     }
 
@@ -141,6 +135,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 static filterStatePt1_t deltaFilterState[3];
 static filterStatePt1_t yawFilterState;
 
+#ifndef SKIP_PID_LUXFLOAT
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
@@ -280,6 +275,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 #endif
     }
 }
+#endif
 
 static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
         rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@@ -426,8 +422,10 @@ void pidSetController(pidControllerType_e type)
         case PID_CONTROLLER_MWREWRITE:
             pid_controller = pidMultiWiiRewrite;
             break;
+#ifndef SKIP_PID_LUXFLOAT
         case PID_CONTROLLER_LUX_FLOAT:
             pid_controller = pidLuxFloat;
+#endif
     }
 }
 

src/main/flight/pid.h

@@ -97,7 +97,6 @@ bool antiWindupProtection;
 extern uint32_t targetPidLooptime;
 
 void pidSetController(pidControllerType_e type);
-void pidResetErrorAngle(void);
 void pidResetErrorGyroState(uint8_t resetOption);
 void setTargetPidLooptime(uint8_t pidProcessDenom);
 

src/main/io/display.c

@@ -22,6 +22,7 @@
 
 #include "platform.h"
 #include "version.h"
+#include "debug.h"
 
 #include "build_config.h"
 
@@ -58,14 +59,16 @@
 #include "flight/navigation.h"
 #endif
 
-#include "config/runtime_config.h"
-
 #include "config/config.h"
+#include "config/runtime_config.h"
+#include "config/config_profile.h"
 
 #include "display.h"
 
 #include "scheduler.h"
 
+extern profile_t *currentProfile;
+
 controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
 
 #define MICROSECONDS_IN_A_SECOND (1000 * 1000)
@@ -317,13 +320,26 @@ void showProfilePage(void)
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
+    static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
+    const pidProfile_t *pidProfile = &currentProfile->pidProfile;
+    for (int axis = 0; axis < 3; ++axis) {
+        tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d",
+            axisTitles[axis],
+            pidProfile->P8[axis],
+            pidProfile->I8[axis],
+            pidProfile->D8[axis]
+        );
+        padLineBuffer();
+        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_send_string(lineBuffer);
+    }
+
+    const uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
     tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
+    const controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
-
     tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
         controlRateConfig->rcExpo8,
         controlRateConfig->rcRate8

src/main/io/display.h

@@ -15,7 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//#define ENABLE_DEBUG_OLED_PAGE
+#define ENABLE_DEBUG_OLED_PAGE
 
 typedef enum {
     PAGE_WELCOME,

src/main/io/serial_cli.c

@@ -26,6 +26,7 @@
 #include "platform.h"
 #include "scheduler.h"
 #include "version.h"
+#include "debug.h"
 
 #include "build_config.h"
 
@@ -426,14 +427,15 @@ static const char * const lookupTableMagHardware[] = {
     "AK8963"
 };
 
-static const char * const lookupTableDebug[] = {
+static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "NONE",
     "CYCLETIME",
     "BATTERY",
     "GYRO",
     "ACCELEROMETER",
     "MIXER",
-    "AIRMODE"
+    "AIRMODE",
+    "PIDLOOP",
 };
 
 static const char * const lookupTableSuperExpoYaw[] = {

src/main/mw.c

@@ -504,7 +504,6 @@ void processRx(void)
             } else {
                 pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
             }
-            pidResetErrorAngle();
         }
     } else {
         pidResetErrorGyroState(RESET_DISABLE);
@@ -655,6 +654,7 @@ void processRx(void)
 
 void subTaskPidController(void)
 {
+    const uint32_t startTime = micros();
     // PID - note this is function pointer set by setPIDController()
     pid_controller(
         &currentProfile->pidProfile,
@@ -663,10 +663,13 @@ void subTaskPidController(void)
         &masterConfig.accelerometerTrims,
         &masterConfig.rxConfig
     );
+    if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
 }
 
 void subTaskMainSubprocesses(void) {
 
+    const uint32_t startTime = micros();
+
     if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
         filterRc();
     }
@@ -730,16 +733,18 @@ void subTaskMainSubprocesses(void) {
     #ifdef TRANSPONDER
         updateTransponder();
     #endif
+    if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
 }
 
 void subTaskMotorUpdate(void)
 {
+    const uint32_t startTime = micros();
     if (debugMode == DEBUG_CYCLETIME) {
         static uint32_t previousMotorUpdateTime;
-        uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
+        const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
         debug[2] = currentDeltaTime;
         debug[3] = currentDeltaTime - targetPidLooptime;
-        previousMotorUpdateTime = micros();
+        previousMotorUpdateTime = startTime;
     }
 
     mixTable();
@@ -752,6 +757,7 @@ void subTaskMotorUpdate(void)
     if (motorControlEnable) {
         writeMotors();
     }
+    if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
 }
 
 uint8_t setPidUpdateCountDown(void) {
@@ -778,10 +784,12 @@ void taskMainPidLoopCheck(void)
         debug[1] = averageSystemLoadPercent;
     }
 
+    const uint32_t startTime = micros();
     while (true) {
         if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
             static uint8_t pidUpdateCountdown;
 
+            if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
             if (runTaskMainSubprocesses) {
                 subTaskMainSubprocesses();
                 runTaskMainSubprocesses = false;

src/main/scheduler.h

@@ -44,14 +44,14 @@ typedef enum {
     /* Actual tasks */
     TASK_SYSTEM = 0,
     TASK_GYROPID,
-    TASK_ATTITUDE,
     TASK_ACCEL,
+    TASK_ATTITUDE,
+    TASK_RX,
     TASK_SERIAL,
+    TASK_BATTERY,
 #ifdef BEEPER
     TASK_BEEPER,
 #endif
-    TASK_BATTERY,
-    TASK_RX,
 #ifdef GPS
     TASK_GPS,
 #endif

src/main/scheduler_tasks.c

@@ -22,14 +22,15 @@
 #include <platform.h>
 #include "scheduler.h"
 
+void taskSystem(void);
 void taskMainPidLoopCheck(void);
 void taskUpdateAccelerometer(void);
-void taskHandleSerial(void);
 void taskUpdateAttitude(void);
-void taskUpdateBeeper(void);
-void taskUpdateBattery(void);
 bool taskUpdateRxCheck(uint32_t currentDeltaTime);
 void taskUpdateRxMain(void);
+void taskHandleSerial(void);
+void taskUpdateBattery(void);
+void taskUpdateBeeper(void);
 void taskProcessGPS(void);
 void taskUpdateCompass(void);
 void taskUpdateBaro(void);
@@ -39,7 +40,6 @@ void taskUpdateDisplay(void);
 void taskTelemetry(void);
 void taskLedStrip(void);
 void taskTransponder(void);
-void taskSystem(void);
 #ifdef USE_BST
 void taskBstReadWrite(void);
 void taskBstMasterProcess(void);
@@ -61,13 +61,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_REALTIME,
     },
 
-    [TASK_ATTITUDE] = {
-        .taskName = "ATTITUDE",
-        .taskFunc = taskUpdateAttitude,
-        .desiredPeriod = 1000000 / 100,
-        .staticPriority = TASK_PRIORITY_MEDIUM,
-    },
-
     [TASK_ACCEL] = {
         .taskName = "ACCEL",
         .taskFunc = taskUpdateAccelerometer,
@@ -75,6 +68,21 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 
+    [TASK_ATTITUDE] = {
+        .taskName = "ATTITUDE",
+        .taskFunc = taskUpdateAttitude,
+        .desiredPeriod = 1000000 / 100,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+
+    [TASK_RX] = {
+        .taskName = "RX",
+        .checkFunc = taskUpdateRxCheck,
+        .taskFunc = taskUpdateRxMain,
+        .desiredPeriod = 1000000 / 50,      // If event-based scheduling doesn't work, fallback to periodic scheduling
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
     [TASK_SERIAL] = {
         .taskName = "SERIAL",
         .taskFunc = taskHandleSerial,
@@ -82,6 +90,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_LOW,
     },
 
+    [TASK_BATTERY] = {
+        .taskName = "BATTERY",
+        .taskFunc = taskUpdateBattery,
+        .desiredPeriod = 1000000 / 50,      // 50 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+
 #ifdef BEEPER
     [TASK_BEEPER] = {
         .taskName = "BEEPER",
@@ -91,21 +106,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-    [TASK_BATTERY] = {
-        .taskName = "BATTERY",
-        .taskFunc = taskUpdateBattery,
-        .desiredPeriod = 1000000 / 50,      // 50 Hz
-        .staticPriority = TASK_PRIORITY_LOW,
-    },
-
-    [TASK_RX] = {
-        .taskName = "RX",
-        .checkFunc = taskUpdateRxCheck,
-        .taskFunc = taskUpdateRxMain,
-        .desiredPeriod = 1000000 / 50,      // If event-based scheduling doesn't work, fallback to periodic scheduling
-        .staticPriority = TASK_PRIORITY_HIGH,
-    },
-
 #ifdef GPS
     [TASK_GPS] = {
         .taskName = "GPS",

src/main/target/CC3D/target.h

@@ -71,10 +71,6 @@
 #define MAG
 #define USE_MAG_HMC5883
 
-#define INVERTER
-#define BEEPER
-#define DISPLAY
-
 #define USE_VCP
 #define USE_USART1
 #define USE_USART3
@@ -117,28 +113,26 @@
 #define WS2811_DMA_TC_FLAG           DMA1_FLAG_TC6
 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
 
-#define BLACKBOX
-#define TELEMETRY
-#define SERIAL_RX
-#define SONAR
-#define USE_SERVOS
-#define USE_CLI
-
-#define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-
-#undef DISPLAY
-#undef SONAR
-//#if defined(OPBL) && defined(USE_SERIAL_1WIRE)
-#undef BARO
-//#undef BLACKBOX
-#undef GPS
-//#endif
-#define SKIP_CLI_COMMAND_HELP
-
 #define SPEKTRUM_BIND
 // USART3, PB11 (Flexport)
 #define BIND_PORT  GPIOB
 #define BIND_PIN   Pin_11
 
-#define USE_QUATERNION
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define INVERTER
+#define BEEPER
+#define DISPLAY
+#define BLACKBOX
+#define TELEMETRY
+#define SERIAL_RX
+#define USE_SERVOS
+#define USE_CLI
+//#define SONAR
+//#define GPS
+
+#undef BARO
+
+#define SKIP_CLI_COMMAND_HELP
+#define SKIP_PID_LUXFLOAT
+