Unittests

This commit is contained in:
Martin Budden 2016-01-19 19:03:11 +00:00 committed by borisbstyle
parent e627751076
commit b22610b9e5
3 changed files with 480 additions and 21 deletions

View File

@ -0,0 +1,136 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef SRC_MAIN_SCHEDULER_C_
#ifdef UNIT_TEST
uint8_t unittest_scheduler_taskId;
uint8_t unittest_scheduler_selectedTaskId;
uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks;
uint32_t unittest_scheduler_timeToNextRealtimeTask;
bool unittest_outsideRealtimeGuardInterval;
#define SET_SCHEDULER_LOCALS() \
{ \
}
#define GET_SCHEDULER_LOCALS() \
{ \
unittest_scheduler_taskId = taskId; \
unittest_scheduler_selectedTaskId = selectedTaskId; \
unittest_scheduler_selectedTaskDynPrio = selectedTaskDynPrio; \
unittest_scheduler_waitingTasks = waitingTasks; \
unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
unittest_outsideRealtimeGuardInterval = outsideRealtimeGuardInterval; \
}
#else
#define SET_SCHEDULER_LOCALS() {}
#define GET_SCHEDULER_LOCALS() {}
#endif
#endif
#ifdef SRC_MAIN_SCHEDULER_C_
#ifdef UNIT_TEST
cfTask_t *unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks;
uint32_t unittest_scheduler_timeToNextRealtimeTask;
#define SET_SCHEDULER_LOCALS() \
{ \
}
#define GET_SCHEDULER_LOCALS() \
{ \
unittest_scheduler_selectedTask = selectedTask; \
unittest_scheduler_selectedTaskDynPrio = selectedTaskDynPrio; \
unittest_scheduler_waitingTasks = waitingTasks; \
unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
}
#else
#define SET_SCHEDULER_LOCALS() {}
#define GET_SCHEDULER_LOCALS() {}
#endif
#endif
#ifdef SRC_MAIN_FLIGHT_PID_C_
#ifdef UNIT_TEST
float unittest_pidLuxFloat_lastErrorForDelta[3];
float unittest_pidLuxFloat_delta1[3];
float unittest_pidLuxFloat_delta2[3];
float unittest_pidLuxFloat_PTerm[3];
float unittest_pidLuxFloat_ITerm[3];
float unittest_pidLuxFloat_DTerm[3];
#define SET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidLuxFloat_lastErrorForDelta[axis]; \
delta1[axis] = unittest_pidLuxFloat_delta1[axis]; \
delta2[axis] = unittest_pidLuxFloat_delta2[axis]; \
}
#define GET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \
unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \
unittest_pidLuxFloat_PTerm[axis] = PTerm; \
unittest_pidLuxFloat_ITerm[axis] = ITerm; \
unittest_pidLuxFloat_DTerm[axis] = DTerm; \
}
int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3];
int32_t unittest_pidMultiWiiRewrite_PTerm[3];
int32_t unittest_pidMultiWiiRewrite_ITerm[3];
int32_t unittest_pidMultiWiiRewrite_DTerm[3];
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidMultiWiiRewrite_lastErrorForDelta[axis]; \
}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \
unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \
unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \
}
#else
#define SET_PID_LUX_FLOAT_LOCALS(axis) {}
#define GET_PID_LUX_FLOAT_LOCALS(axis) {}
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#endif // UNIT_TEST
#endif // SRC_MAIN_FLIGHT_PID_C_

View File

@ -46,22 +46,29 @@ uint32_t currentTime = 0;
uint16_t averageSystemLoadPercent = 0;
static int queuePos = 0;
static int queueSize = 0;
static int taskQueuePos = 0;
static int taskQueueSize = 0;
// No need for a linked list for the queue, since items are only inserted at startup
static cfTask_t* queueArray[TASK_COUNT];
STATIC_UNIT_TESTED cfTask_t* taskQueueArray[TASK_COUNT + 1]; // 1 extra space so test code can check for buffer overruns
STATIC_UNIT_TESTED void queueClear(void)
{
memset(queueArray, 0, sizeof(queueArray));
queuePos = 0;
queueSize = 0;
memset(taskQueueArray, 0, (int)(sizeof(taskQueueArray)));
taskQueuePos = 0;
taskQueueSize = 0;
}
#ifdef UNIT_TEST
STATIC_UNIT_TESTED int queueSize(void)
{
return taskQueueSize;
}
#endif
STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
{
for (int ii = 0; ii < queueSize; ++ii) {
if (queueArray[ii] == task) {
for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) {
return true;
}
}
@ -70,14 +77,17 @@ STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
STATIC_UNIT_TESTED void queueAdd(cfTask_t *task)
{
if (taskQueueSize >= TASK_COUNT -1) {
return;
}
if (queueContains(task)) {
return;
}
++queueSize;
for (int ii = 0; ii < queueSize; ++ii) {
if (queueArray[ii] == NULL || queueArray[ii]->staticPriority < task->staticPriority) {
memmove(&queueArray[ii+1], &queueArray[ii], sizeof(task) * (queueSize - ii - 1));
queueArray[ii] = task;
++taskQueueSize;
for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->staticPriority < task->staticPriority) {
memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii - 1));
taskQueueArray[ii] = task;
return;
}
}
@ -85,9 +95,10 @@ STATIC_UNIT_TESTED void queueAdd(cfTask_t *task)
STATIC_UNIT_TESTED void queueRemove(cfTask_t *task)
{
for (int ii = 0; ii < queueSize; ++ii) {
if (queueArray[ii] == task) {
memmove(&queueArray[ii], &queueArray[ii+1], sizeof(task) * (queueSize - ii - 1));
for (int ii = 0; ii < taskQueueSize; ++ii) {
if (taskQueueArray[ii] == task) {
--taskQueueSize;
memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii));
return;
}
}
@ -95,13 +106,14 @@ STATIC_UNIT_TESTED void queueRemove(cfTask_t *task)
STATIC_UNIT_TESTED cfTask_t *queueFirst(void)
{
queuePos = 0;
return queueSize > 0 ? queueArray[0] : NULL;
taskQueuePos = 0;
return taskQueueSize > 0 ? taskQueueArray[0] : NULL;
}
STATIC_UNIT_TESTED cfTask_t *queueNext(void) {
++queuePos;
return queuePos < queueSize ? queueArray[queuePos] : NULL;
STATIC_UNIT_TESTED cfTask_t *queueNext(void)
{
++taskQueuePos;
return taskQueuePos < taskQueueSize ? taskQueueArray[taskQueuePos] : NULL;
}
void taskSystem(void)
@ -178,6 +190,7 @@ void schedulerInit(void)
void scheduler(void)
{
SET_SCHEDULER_LOCALS();
/* Cache currentTime */
currentTime = micros();

View File

@ -0,0 +1,310 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
extern "C" {
#include <platform.h>
#include "scheduler.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
enum {
systemTime = 10,
pidLoopCheckerTime = 650,
updateAccelerometerTime = 192,
handleSerialTime = 30,
updateBeeperTime = 1,
updateBatteryTime = 1,
updateRxCheckTime = 34,
updateRxMainTime = 10,
processGPSTime = 10,
updateCompassTime = 195,
updateBaroTime = 201,
updateSonarTime = 10,
calculateAltitudeTime = 154,
updateDisplayTime = 10,
telemetryTime = 10,
ledStripTime = 10,
transponderTime = 10
};
extern "C" {
cfTask_t * unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks;
uint32_t unittest_scheduler_timeToNextRealtimeTask;
bool unittest_outsideRealtimeGuardInterval;
// set up micros() to simulate time
uint32_t simulatedTime = 0;
uint32_t micros(void) {return simulatedTime;}
// set up tasks to take a simulated representative time to execute
void taskMainPidLoopChecker(void) {simulatedTime+=pidLoopCheckerTime;}
void taskUpdateAccelerometer(void) {simulatedTime+=updateAccelerometerTime;}
void taskHandleSerial(void) {simulatedTime+=handleSerialTime;}
void taskUpdateBeeper(void) {simulatedTime+=updateBeeperTime;}
void taskUpdateBattery(void) {simulatedTime+=updateBatteryTime;}
bool taskUpdateRxCheck(uint32_t currentDeltaTime) {UNUSED(currentDeltaTime);simulatedTime+=updateRxCheckTime;return false;}
void taskUpdateRxMain(void) {simulatedTime+=updateRxMainTime;}
void taskProcessGPS(void) {simulatedTime+=processGPSTime;}
void taskUpdateCompass(void) {simulatedTime+=updateCompassTime;}
void taskUpdateBaro(void) {simulatedTime+=updateBaroTime;}
void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;}
void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;}
void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;}
void taskTelemetry(void) {simulatedTime+=telemetryTime;}
void taskLedStrip(void) {simulatedTime+=ledStripTime;}
void taskTransponder(void) {simulatedTime+=transponderTime;}
extern cfTask_t* taskQueueArray[];
extern void queueClear(void);
extern int queueSize();
extern bool queueContains(cfTask_t *task);
extern void queueAdd(cfTask_t *task);
extern void queueRemove(cfTask_t *task);
extern cfTask_t *queueFirst(void);
extern cfTask_t *queueNext(void);
}
TEST(SchedulerUnittest, TestPriorites)
{
// check that the #defines used by scheduler.c and scheduler_unittest.cc are in sync
EXPECT_EQ(14, TASK_COUNT);
EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority);
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority);
}
TEST(SchedulerUnittest, TestQueue)
{
queueClear();
EXPECT_EQ(0, queueSize());
queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
EXPECT_EQ(1, queueSize());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME
EXPECT_EQ(2, queueSize());
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
EXPECT_EQ(3, queueSize());
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
queueAdd(&cfTasks[TASK_BEEPER]); // TASK_PRIORITY_MEDIUM
EXPECT_EQ(4, queueSize());
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH
EXPECT_EQ(5, queueSize());
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
EXPECT_EQ(4, queueSize());
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
}
TEST(SchedulerUnittest, TestQueueArray)
{
// test there are no "out by one" errors or buffer overruns when items are added and removed
queueClear();
for (int taskId=0; taskId < TASK_COUNT - 1; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), true);
}
EXPECT_EQ(TASK_COUNT - 1, queueSize());
EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 2]);
cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2];
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 1]);
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT]);
setTaskEnabled(TASK_SYSTEM, false);
EXPECT_EQ(TASK_COUNT - 2, queueSize());
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]);
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]); // this won't have been moved
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 1]);
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT]);
taskQueueArray[TASK_COUNT - 2] = 0;
setTaskEnabled(TASK_SYSTEM, true);
EXPECT_EQ(TASK_COUNT - 1, queueSize());
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]);
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 1]);
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT]);
// now there are TASK_COUNT items in the array
setTaskEnabled(static_cast<cfTaskId_e>(TASK_COUNT - 1), true);
EXPECT_EQ(TASK_COUNT, queueSize());
EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]);
EXPECT_EQ(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT]); // check no buffer overrun
}
TEST(SchedulerUnittest, TestInit)
{
schedulerInit();
EXPECT_EQ(1, queueSize());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
}
TEST(SchedulerUnittest, TestSingleTask)
{
// disable all tasks except TASK_GYROPID
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_GYROPID, true);
cfTasks[TASK_GYROPID].lastExecutedAt = 1000;
simulatedTime = 4000;
// run the scheduler and check the task has executed
scheduler();
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime);
EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt);
EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime);
// task has run, so its dynamic priority should have been set to zero
EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority);
}
TEST(SchedulerUnittest, TestTwoTasks)
{
// disable all tasks except TASK_GYROPID and TASK_ACCEL
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_ACCEL, true);
setTaskEnabled(TASK_GYROPID, true);
// set it up so that TASK_ACCEL ran just before TASK_GYROPID
static const uint32_t startTime = 4000;
simulatedTime = startTime;
cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime;
EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles);
// run the scheduler
scheduler();
// no tasks should have run, since neither task's desired time has elapsed
EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId);
// NOTE:
// TASK_GYROPID desiredPeriod is 1000 microseconds
// TASK_ACCEL desiredPeriod is 10000 microseconds
// 500 microseconds later
simulatedTime += 500;
// no tasks should run, since neither task's desired time has elapsed
scheduler();
EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId);
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
// 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed
simulatedTime += 500;
// TASK_GYROPID should now run
scheduler();
EXPECT_EQ(TASK_GYROPID, unittest_scheduler_selectedTaskId);
EXPECT_EQ(1, unittest_scheduler_waitingTasks);
EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime);
simulatedTime += 1000 - pidLoopCheckerTime;
scheduler();
// TASK_GYROPID should run again
EXPECT_EQ(TASK_GYROPID, unittest_scheduler_selectedTaskId);
scheduler();
EXPECT_EQ(TASK_NONE, unittest_scheduler_selectedTaskId);
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed
// of the two TASK_GYROPID should run first
scheduler();
EXPECT_EQ(TASK_GYROPID, unittest_scheduler_selectedTaskId);
// and finally TASK_ACCEL should now run
scheduler();
EXPECT_EQ(TASK_ACCEL, unittest_scheduler_selectedTaskId);
}
TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun)
{
// disable all tasks except TASK_GYROPID and TASK_SYSTEM
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_GYROPID, true);
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
simulatedTime = 200700;
setTaskEnabled(TASK_SYSTEM, true);
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
scheduler();
EXPECT_EQ(false, unittest_outsideRealtimeGuardInterval);
EXPECT_EQ(300, unittest_scheduler_timeToNextRealtimeTask);
// Nothing should be scheduled in guard period
EXPECT_EQ((uint8_t)TASK_NONE, unittest_scheduler_selectedTaskId);
EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt);
EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt);
}
TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun)
{
// disable all tasks except TASK_GYROPID and TASK_SYSTEM
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_GYROPID, true);
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
simulatedTime = 200699;
setTaskEnabled(TASK_SYSTEM, true);
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
scheduler();
EXPECT_EQ(true, unittest_outsideRealtimeGuardInterval);
EXPECT_EQ(301, unittest_scheduler_timeToNextRealtimeTask);
// System should be scheduled as not in guard period
EXPECT_EQ((uint8_t)TASK_SYSTEM, unittest_scheduler_selectedTaskId);
EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt);
EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt);
}
// STUBS
extern "C" {
}