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

Rework scheduler interaction with realtime process // Detailled task manager // Fix gyro sync (was broken) 

Fix cycletime calculations
This commit is contained in:
borisbstyle 2016-02-25 17:51:24 +01:00
parent 9e1f78075d
commit f7091f48d0
6 changed files with 86 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,7 +78,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
targetPidLooptime = targetLooptime / pidProcessDenom;
}
void pidResetErrorAngle(void)

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

@ -46,6 +46,7 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/sdcard.h"
#include "drivers/gyro_sync.h"
#include "drivers/buf_writer.h"
@ -2577,7 +2578,28 @@ static void cliTasks(char *cmdline)
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
getTaskInfo(taskId, &taskInfo);
if (taskInfo.isEnabled) {
cliPrintf("%d - %s, max = %d us, avg = %d us, total = %d ms\r\n", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskInfo.totalExecutionTime / 1000);
uint16_t taskFrequency;
uint16_t subTaskFrequency;
if (taskId == TASK_GYROPID) {
subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
} else {
taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
}
uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000;
cliPrintf("%d - (%s) max: %dus, avg: %dus, rate: %dhz, total: ", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency);
if (taskTotalTime >= 1000) {
cliPrintf("%ds", taskTotalTime / 1000);
} else {
cliPrintf("%dms", taskTotalTime);
}
if (taskId == TASK_GYROPID) cliPrintf(" (%s rate: %dhz)", taskInfo.subTaskName, subTaskFrequency);
cliPrintf("\r\n", taskTotalTime);
}
}
}

7
src/main/main.c
View File

@ -580,9 +580,14 @@ void init(void)
afatfs_init();
#endif
if (masterConfig.gyro_lpf) masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
if (masterConfig.gyro_lpf) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif

24
src/main/mw.c
View File

@ -737,9 +737,12 @@ void taskMainPidLoop(void)
void taskMainPidLoopCheck(void) {
static uint32_t previousTime;
static uint8_t pidUpdateCountdown;
static uint32_t previousPidUpdateTime, pidCycleTime;
if (!pidUpdateCountdown) pidUpdateCountdown = masterConfig.pid_process_denom;
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
cycleTime = micros() - previousTime;
previousTime = micros();
@ -747,20 +750,33 @@ void taskMainPidLoopCheck(void) {
debug[0] = cycleTime;
debug[1] = cycleTime - targetLooptime;
debug[2] = averageSystemLoadPercent;
debug[3] = pidCycleTime;
while (1) {
if (gyroSyncCheckUpdate() || ((cycleTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
imuUpdateGyro();
if (pidUpdateCountdown == 1) {
switch (pidUpdateCountdown) {
case(1):
pidCycleTime = micros() - previousPidUpdateTime;
previousPidUpdateTime = micros();
pidUpdateCountdown = masterConfig.pid_process_denom;
taskMainPidLoop();
} else {
if (masterConfig.pid_process_denom > 1) realTimeCycle = false;
break;
case(2):
realTimeCycle = true;
pidUpdateCountdown--;
break;
default:
pidUpdateCountdown--;
}
}
break;
}
}
}
void taskUpdateAccelerometer(void)
{

34
src/main/scheduler.c
View File

@ -35,12 +35,15 @@ static uint32_t totalWaitingTasks;
static uint32_t totalWaitingTasksSamples;
static uint32_t realtimeGuardInterval;
bool realTimeCycle = true;
uint32_t currentTime = 0;
uint16_t averageWaitingTasks100 = 0;
typedef struct {
/* Configuration */
const char * taskName;
const char * subTaskName; // For future use, but now just for naming convention
bool (*checkFunc)(uint32_t currentDeltaTime);
void (*taskFunc)(void);
bool isEnabled;
@ -93,7 +96,8 @@ static cfTask_t cfTasks[TASK_COUNT] = {
},
[TASK_GYROPID] = {
.taskName = "GYRO/PID",
.taskName = "PID",
.subTaskName = "GYRO",
.taskFunc = taskMainPidLoopCheck,
.desiredPeriod = 1000,
.staticPriority = TASK_PRIORITY_REALTIME,
@ -226,7 +230,8 @@ static cfTask_t cfTasks[TASK_COUNT] = {
uint16_t averageSystemLoadPercent = 0;
#define GUARD_INTERVAL 5
#define MAX_GUARD_INTERVAL 100
#define MIN_GUARD_INTERVAL 10
void taskSystem(void)
{
@ -241,13 +246,13 @@ void taskSystem(void)
/* Calculate guard interval */
uint32_t maxNonRealtimeTaskTime = 0;
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
}
}
realtimeGuardInterval = GUARD_INTERVAL;
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, MIN_GUARD_INTERVAL, MAX_GUARD_INTERVAL);
#if defined SCHEDULER_DEBUG
debug[2] = realtimeGuardInterval;
#endif
@ -257,12 +262,14 @@ void taskSystem(void)
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
{
taskInfo->taskName = cfTasks[taskId].taskName;
taskInfo->subTaskName = cfTasks[taskId].subTaskName;
taskInfo->isEnabled= cfTasks[taskId].isEnabled;
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
}
#endif
@ -299,6 +306,8 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
}
}
#define MAXT_TIME_TICKS_TO_RESET 10000
void scheduler(void)
{
uint8_t taskId;
@ -306,6 +315,15 @@ void scheduler(void)
uint8_t selectedTaskDynPrio;
uint16_t waitingTasks = 0;
uint32_t timeToNextRealtimeTask = UINT32_MAX;
uint32_t currentRealtimeGuardInterval;
static uint16_t maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
if (realTimeCycle) {
currentRealtimeGuardInterval = realtimeGuardInterval;
} else {
currentRealtimeGuardInterval = MIN_GUARD_INTERVAL;
}
/* Cache currentTime */
currentTime = micros();
@ -328,7 +346,7 @@ void scheduler(void)
}
}
bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > currentRealtimeGuardInterval);
/* Update task dynamic priorities */
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
@ -399,7 +417,13 @@ void scheduler(void)
cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
#ifndef SKIP_TASK_STATISTICS
cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
if (maxTaskCalculationReset > 0) {
cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
maxTaskCalculationReset--;
} else {
cfTasks[selectedTaskId].maxExecutionTime = taskExecutionTime;
maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
}
#endif
#if defined SCHEDULER_DEBUG
debug[3] = (micros() - currentTime) - taskExecutionTime;

3
src/main/scheduler.h
View File

@ -28,6 +28,7 @@ typedef enum {
typedef struct {
const char * taskName;
const char * subTaskName;
bool isEnabled;
uint32_t desiredPeriod;
uint8_t staticPriority;
@ -35,6 +36,7 @@ typedef struct {
uint32_t totalExecutionTime;
uint32_t lastExecutionTime;
uint32_t averageExecutionTime;
uint32_t latestDeltaTime;
} cfTaskInfo_t;
typedef enum {
@ -85,6 +87,7 @@ typedef enum {
extern uint16_t cpuLoad;
extern uint16_t averageSystemLoadPercent;
extern bool realTimeCycle;
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);