Merge corrections
This commit is contained in:
parent
c80b8cd718
commit
7386d88d0e
1
Makefile
1
Makefile
|
@ -284,6 +284,7 @@ COMMON_SRC = build_config.c \
|
||||||
common/encoding.c \
|
common/encoding.c \
|
||||||
common/filter.c \
|
common/filter.c \
|
||||||
scheduler.c \
|
scheduler.c \
|
||||||
|
scheduler_tasks.c \
|
||||||
main.c \
|
main.c \
|
||||||
mw.c \
|
mw.c \
|
||||||
flight/altitudehold.c \
|
flight/altitudehold.c \
|
||||||
|
|
|
@ -663,10 +663,13 @@ int main(void) {
|
||||||
init();
|
init();
|
||||||
|
|
||||||
/* Setup scheduler */
|
/* Setup scheduler */
|
||||||
|
schedulerInit();
|
||||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
rescheduleTask(TASK_GYROPID, targetLooptime);
|
||||||
|
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_MOTOR, true);
|
||||||
|
rescheduleTask(TASK_MOTOR, lrintf((1.0f / masterConfig.motor_pwm_rate) * 1000000));
|
||||||
|
|
||||||
if(sensors(SENSOR_ACC)) {
|
if(sensors(SENSOR_ACC)) {
|
||||||
setTaskEnabled(TASK_ACCEL, true);
|
setTaskEnabled(TASK_ACCEL, true);
|
||||||
switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
||||||
|
@ -713,6 +716,8 @@ int main(void) {
|
||||||
#endif
|
#endif
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
|
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
|
||||||
|
// Reschedule telemetry to 500hz for Jeti Exbus
|
||||||
|
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000);
|
||||||
#endif
|
#endif
|
||||||
#ifdef LED_STRIP
|
#ifdef LED_STRIP
|
||||||
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
|
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
|
||||||
|
@ -721,7 +726,6 @@ int main(void) {
|
||||||
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
|
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
schedulerInit();
|
|
||||||
while (1) {
|
while (1) {
|
||||||
scheduler();
|
scheduler();
|
||||||
processLoopback();
|
processLoopback();
|
||||||
|
|
|
@ -21,9 +21,6 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
#ifdef UNIT_TEST
|
|
||||||
typedef enum {TEST_IRQ = 0 } IRQn_Type;
|
|
||||||
#endif
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "scheduler.h"
|
#include "scheduler.h"
|
||||||
|
@ -36,13 +33,8 @@ typedef enum {TEST_IRQ = 0 } IRQn_Type;
|
||||||
|
|
||||||
static cfTask_t *currentTask = NULL;
|
static cfTask_t *currentTask = NULL;
|
||||||
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MIN 10
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MAX 300
|
|
||||||
#define REALTIME_GUARD_INTERVAL_MARGIN 25
|
|
||||||
|
|
||||||
static uint32_t totalWaitingTasks;
|
static uint32_t totalWaitingTasks;
|
||||||
static uint32_t totalWaitingTasksSamples;
|
static uint32_t totalWaitingTasksSamples;
|
||||||
static uint32_t realtimeGuardInterval = REALTIME_GUARD_INTERVAL_MAX;
|
|
||||||
|
|
||||||
uint32_t currentTime = 0;
|
uint32_t currentTime = 0;
|
||||||
uint16_t averageSystemLoadPercent = 0;
|
uint16_t averageSystemLoadPercent = 0;
|
||||||
|
@ -51,26 +43,17 @@ uint16_t averageSystemLoadPercent = 0;
|
||||||
static int taskQueuePos = 0;
|
static int taskQueuePos = 0;
|
||||||
static int taskQueueSize = 0;
|
static int taskQueueSize = 0;
|
||||||
// No need for a linked list for the queue, since items are only inserted at startup
|
// No need for a linked list for the queue, since items are only inserted at startup
|
||||||
#ifdef UNIT_TEST
|
|
||||||
STATIC_UNIT_TESTED cfTask_t* taskQueueArray[TASK_COUNT + 2]; // 1 extra space so test code can check for buffer overruns
|
|
||||||
#else
|
|
||||||
static cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
|
static cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
|
||||||
#endif
|
|
||||||
STATIC_UNIT_TESTED void queueClear(void)
|
void queueClear(void)
|
||||||
{
|
{
|
||||||
memset(taskQueueArray, 0, sizeof(taskQueueArray));
|
memset(taskQueueArray, 0, sizeof(taskQueueArray));
|
||||||
taskQueuePos = 0;
|
taskQueuePos = 0;
|
||||||
taskQueueSize = 0;
|
taskQueueSize = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef UNIT_TEST
|
bool queueContains(cfTask_t *task)
|
||||||
STATIC_UNIT_TESTED int queueSize(void)
|
|
||||||
{
|
|
||||||
return taskQueueSize;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
|
|
||||||
{
|
{
|
||||||
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
||||||
if (taskQueueArray[ii] == task) {
|
if (taskQueueArray[ii] == task) {
|
||||||
|
@ -80,7 +63,7 @@ STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task)
|
bool queueAdd(cfTask_t *task)
|
||||||
{
|
{
|
||||||
if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
|
if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -96,7 +79,7 @@ STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task)
|
bool queueRemove(cfTask_t *task)
|
||||||
{
|
{
|
||||||
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
for (int ii = 0; ii < taskQueueSize; ++ii) {
|
||||||
if (taskQueueArray[ii] == task) {
|
if (taskQueueArray[ii] == task) {
|
||||||
|
@ -111,7 +94,7 @@ STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task)
|
||||||
/*
|
/*
|
||||||
* Returns first item queue or NULL if queue empty
|
* Returns first item queue or NULL if queue empty
|
||||||
*/
|
*/
|
||||||
STATIC_INLINE_UNIT_TESTED cfTask_t *queueFirst(void)
|
cfTask_t *queueFirst(void)
|
||||||
{
|
{
|
||||||
taskQueuePos = 0;
|
taskQueuePos = 0;
|
||||||
return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
|
return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
|
||||||
|
@ -120,7 +103,7 @@ STATIC_INLINE_UNIT_TESTED cfTask_t *queueFirst(void)
|
||||||
/*
|
/*
|
||||||
* Returns next item in queue or NULL if at end of queue
|
* Returns next item in queue or NULL if at end of queue
|
||||||
*/
|
*/
|
||||||
STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void)
|
cfTask_t *queueNext(void)
|
||||||
{
|
{
|
||||||
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
|
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
|
||||||
}
|
}
|
||||||
|
@ -133,31 +116,20 @@ void taskSystem(void)
|
||||||
totalWaitingTasksSamples = 0;
|
totalWaitingTasksSamples = 0;
|
||||||
totalWaitingTasks = 0;
|
totalWaitingTasks = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Calculate guard interval */
|
|
||||||
uint32_t maxNonRealtimeTaskTime = 0;
|
|
||||||
for (const cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
|
|
||||||
if (task->staticPriority != TASK_PRIORITY_REALTIME) {
|
|
||||||
maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, task->averageExecutionTime);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
|
|
||||||
#if defined SCHEDULER_DEBUG
|
|
||||||
debug[2] = realtimeGuardInterval;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef SKIP_TASK_STATISTICS
|
#ifndef SKIP_TASK_STATISTICS
|
||||||
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
|
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
|
||||||
{
|
{
|
||||||
taskInfo->taskName = cfTasks[taskId].taskName;
|
taskInfo->taskName = cfTasks[taskId].taskName;
|
||||||
|
taskInfo->subTaskName = cfTasks[taskId].subTaskName;
|
||||||
taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
|
taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
|
||||||
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
|
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
|
||||||
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
|
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
|
||||||
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
|
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
|
||||||
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
|
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
|
||||||
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
|
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
|
||||||
|
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -213,7 +185,7 @@ void scheduler(void)
|
||||||
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
|
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
|
const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > 0);
|
||||||
|
|
||||||
// The task to be invoked
|
// The task to be invoked
|
||||||
cfTask_t *selectedTask = NULL;
|
cfTask_t *selectedTask = NULL;
|
||||||
|
@ -286,5 +258,4 @@ void scheduler(void)
|
||||||
debug[3] = (micros() - currentTime);
|
debug[3] = (micros() - currentTime);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
GET_SCHEDULER_LOCALS();
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -30,18 +30,21 @@ typedef enum {
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
const char * taskName;
|
const char * taskName;
|
||||||
|
const char * subTaskName;
|
||||||
bool isEnabled;
|
bool isEnabled;
|
||||||
uint32_t desiredPeriod;
|
uint32_t desiredPeriod;
|
||||||
uint8_t staticPriority;
|
uint8_t staticPriority;
|
||||||
uint32_t maxExecutionTime;
|
uint32_t maxExecutionTime;
|
||||||
uint32_t totalExecutionTime;
|
uint32_t totalExecutionTime;
|
||||||
uint32_t averageExecutionTime;
|
uint32_t averageExecutionTime;
|
||||||
|
uint32_t latestDeltaTime;
|
||||||
} cfTaskInfo_t;
|
} cfTaskInfo_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
/* Actual tasks */
|
/* Actual tasks */
|
||||||
TASK_SYSTEM = 0,
|
TASK_SYSTEM = 0,
|
||||||
TASK_GYROPID,
|
TASK_GYROPID,
|
||||||
|
TASK_MOTOR,
|
||||||
TASK_ACCEL,
|
TASK_ACCEL,
|
||||||
TASK_SERIAL,
|
TASK_SERIAL,
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
|
@ -88,6 +91,7 @@ typedef enum {
|
||||||
typedef struct {
|
typedef struct {
|
||||||
/* Configuration */
|
/* Configuration */
|
||||||
const char * taskName;
|
const char * taskName;
|
||||||
|
const char * subTaskName;
|
||||||
bool (*checkFunc)(uint32_t currentDeltaTime);
|
bool (*checkFunc)(uint32_t currentDeltaTime);
|
||||||
void (*taskFunc)(void);
|
void (*taskFunc)(void);
|
||||||
uint32_t desiredPeriod; // target period of execution
|
uint32_t desiredPeriod; // target period of execution
|
||||||
|
|
|
@ -22,7 +22,8 @@
|
||||||
#include <platform.h>
|
#include <platform.h>
|
||||||
#include "scheduler.h"
|
#include "scheduler.h"
|
||||||
|
|
||||||
void taskMainPidLoopChecker(void);
|
void taskMainPidLoopCheck(void);
|
||||||
|
void taskMotorUpdate(void);
|
||||||
void taskUpdateAccelerometer(void);
|
void taskUpdateAccelerometer(void);
|
||||||
void taskHandleSerial(void);
|
void taskHandleSerial(void);
|
||||||
void taskUpdateBeeper(void);
|
void taskUpdateBeeper(void);
|
||||||
|
@ -49,8 +50,16 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
||||||
},
|
},
|
||||||
|
|
||||||
[TASK_GYROPID] = {
|
[TASK_GYROPID] = {
|
||||||
.taskName = "GYRO/PID",
|
.taskName = "PID",
|
||||||
.taskFunc = taskMainPidLoopChecker,
|
.subTaskName = "GYRO",
|
||||||
|
.taskFunc = taskMainPidLoopCheck,
|
||||||
|
.desiredPeriod = 1000,
|
||||||
|
.staticPriority = TASK_PRIORITY_HIGH,
|
||||||
|
},
|
||||||
|
|
||||||
|
[TASK_MOTOR] = {
|
||||||
|
.taskName = "MOTOR",
|
||||||
|
.taskFunc = taskMotorUpdate,
|
||||||
.desiredPeriod = 1000,
|
.desiredPeriod = 1000,
|
||||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||||
},
|
},
|
||||||
|
|
|
@ -405,4 +405,3 @@ TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun)
|
||||||
// STUBS
|
// STUBS
|
||||||
extern "C" {
|
extern "C" {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue