From 825475fd438b32b4ba076d2dc84577d4ef47414b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 5 May 2016 10:28:26 +0100 Subject: [PATCH] Added new debug mode to time pidloop subtasks. --- src/main/debug.h | 7 +++-- src/main/flight/pid.c | 20 ++++++-------- src/main/flight/pid.h | 1 - src/main/io/display.c | 26 ++++++++++++++---- src/main/io/display.h | 2 +- src/main/io/serial_cli.c | 6 ++-- src/main/mw.c | 14 ++++++++-- src/main/scheduler.h | 6 ++-- src/main/scheduler_tasks.c | 52 +++++++++++++++++------------------ src/main/target/CC3D/target.h | 42 ++++++++++++---------------- 10 files changed, 98 insertions(+), 78 deletions(-) diff --git a/src/main/debug.h b/src/main/debug.h index facb9982ec..4f6250b758 100644 --- a/src/main/debug.h +++ b/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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 091eda0876..0a5cf87db5 100644 --- a/src/main/flight/pid.c +++ b/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]; -static float errorAngleIf[2]; +#endif + 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 } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3738f97633..14df2b8f41 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/main/io/display.c b/src/main/io/display.c index 33fc5e5fcf..ed394d9c95 100644 --- a/src/main/io/display.c +++ b/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 = ¤tProfile->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 diff --git a/src/main/io/display.h b/src/main/io/display.h index 7d420b6563..8149a35ade 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -//#define ENABLE_DEBUG_OLED_PAGE +#define ENABLE_DEBUG_OLED_PAGE typedef enum { PAGE_WELCOME, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3f0bb134dc..6073285985 100644 --- a/src/main/io/serial_cli.c +++ b/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[] = { diff --git a/src/main/mw.c b/src/main/mw.c index ae407cf710..682ea6c181 100644 --- a/src/main/mw.c +++ b/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( ¤tProfile->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; diff --git a/src/main/scheduler.h b/src/main/scheduler.h index 34141fd820..237776c259 100755 --- a/src/main/scheduler.h +++ b/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 diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c index 223eadc03d..b9babb4b33 100644 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler_tasks.c @@ -22,14 +22,15 @@ #include #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", diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index caf6e7b880..312f692234 100644 --- a/src/main/target/CC3D/target.h +++ b/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 +