rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added new debug mode to time pidloop subtasks.

This commit is contained in:
Martin Budden 2016-05-05 10:28:26 +01:00
parent fa24d2950e
commit 825475fd43
10 changed files with 98 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/debug.h
View File

@ -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;

20
src/main/flight/pid.c
View File

@ -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
}
}

1
src/main/flight/pid.h
View File

@ -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);

26
src/main/io/display.c
View File

@ -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

2
src/main/io/display.h
View File

@ -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,

6
src/main/io/serial_cli.c
View File

@ -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[] = {

14
src/main/mw.c
View File

@ -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;

6
src/main/scheduler.h
View File

@ -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

52
src/main/scheduler_tasks.c
View File

@ -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",

42
src/main/target/CC3D/target.h
View File

@ -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