Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Gyro native rate sampling, filtering, and scheduler restructuring

Browse source
This commit is contained in:
Bruce Luckcuck 2020-01-06 07:52:35 -05:00
parent b11565efbe
commit f584780944
42 changed files with 718 additions and 421 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/test/unit/arming_prevention_unittest.cc
View file

@ -74,6 +74,7 @@ extern "C" {
int16_t GPS_directionToHome = 0;
acc_t acc = {};
bool mockIsUpright = false;
uint8_t activePidLoopDenom = 1;
}
uint32_t simulationFeatureFlags = 0;
@ -1049,7 +1050,7 @@ extern "C" {
void writeServos(void) {};
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
bool isMixerUsingServos(void) { return false; }
void gyroUpdate(timeUs_t) {}
void gyroUpdate(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; }
@ -1099,4 +1100,5 @@ extern "C" {
bool compassIsCalibrationComplete(void) { return true; }
bool isUpright(void) { return mockIsUpright; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
}

8
src/test/unit/blackbox_unittest.cc
View file

@ -75,14 +75,6 @@ TEST(BlackboxTest, TestInitIntervals)
EXPECT_EQ(0, blackboxPInterval);
EXPECT_EQ(4096, blackboxSInterval);
// 1kHz PIDloop
gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1);
targetPidLooptime = gyro.targetLooptime * 1;
blackboxInit();
EXPECT_EQ(32, blackboxIInterval);
EXPECT_EQ(1, blackboxPInterval);
EXPECT_EQ(8192, blackboxSInterval);
// 2kHz PIDloop
targetPidLooptime = 500;
blackboxInit();

2
src/test/unit/pid_unittest.cc
View file

@ -144,7 +144,7 @@ void setDefaultTestSettings(void) {
pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
pidProfile->launchControlGain = 40,
gyro.targetLooptime = 4000;
gyro.targetLooptime = 8000;
}
timeUs_t currentTestTime(void) {

341
src/test/unit/scheduler_unittest.cc
View file

@ -25,8 +25,12 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
const int TEST_PID_LOOP_TIME = 650;
const int TEST_UPDATE_ACCEL_TIME = 192;
const int TEST_GYRO_SAMPLE_HZ = 8000;
const int TEST_GYRO_SAMPLE_TIME = 10;
const int TEST_FILTERING_TIME = 40;
const int TEST_PID_LOOP_TIME = 58;
const int TEST_UPDATE_ACCEL_TIME = 32;
const int TEST_UPDATE_ATTITUDE_TIME = 28;
const int TEST_HANDLE_SERIAL_TIME = 30;
const int TEST_UPDATE_BATTERY_TIME = 1;
const int TEST_UPDATE_RX_CHECK_TIME = 34;
@ -41,13 +45,23 @@ extern "C" {
cfTask_t * unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynPrio;
uint16_t unittest_scheduler_waitingTasks;
timeDelta_t unittest_scheduler_taskRequiredTimeUs;
bool taskGyroRan = false;
bool taskFilterRan = false;
bool taskPidRan = false;
bool taskFilterReady = false;
bool taskPidReady = false;
// 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 taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; }
bool gyroFilterReady(void) { return taskFilterReady; }
bool pidLoopReady(void) { return taskPidReady; }
void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }
void taskUpdateAccelerometer(timeUs_t) { simulatedTime += TEST_UPDATE_ACCEL_TIME; }
void taskHandleSerial(timeUs_t) { simulatedTime += TEST_HANDLE_SERIAL_TIME; }
void taskUpdateBatteryVoltage(timeUs_t) { simulatedTime += TEST_UPDATE_BATTERY_TIME; }
@ -56,6 +70,14 @@ extern "C" {
void imuUpdateAttitude(timeUs_t) { simulatedTime += TEST_IMU_UPDATE_TIME; }
void dispatchProcess(timeUs_t) { simulatedTime += TEST_DISPATCH_TIME; }
void resetGyroTaskTestFlags(void) {
taskGyroRan = false;
taskFilterRan = false;
taskPidRan = false;
taskFilterReady = false;
taskPidReady = false;
}
extern int taskQueueSize;
extern cfTask_t* taskQueueArray[];
@ -73,17 +95,28 @@ extern "C" {
.desiredPeriod = TASK_PERIOD_HZ(10),
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
},
[TASK_GYROPID] = {
[TASK_GYRO] = {
.taskName = "GYRO",
.taskFunc = taskGyroSample,
.desiredPeriod = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_FILTER] = {
.taskName = "FILTER",
.taskFunc = taskFiltering,
.desiredPeriod = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_PID] = {
.taskName = "PID",
.subTaskName = "GYRO",
.taskFunc = taskMainPidLoop,
.desiredPeriod = 1000,
.desiredPeriod = TASK_PERIOD_HZ(4000),
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_ACCEL] = {
.taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer,
.desiredPeriod = 10000,
.desiredPeriod = TASK_PERIOD_HZ(1000),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_ATTITUDE] = {
@ -123,7 +156,7 @@ extern "C" {
TEST(SchedulerUnittest, TestPriorites)
{
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYRO].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_VOLTAGE].staticPriority);
@ -152,34 +185,24 @@ TEST(SchedulerUnittest, TestQueue)
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME
EXPECT_EQ(2, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
EXPECT_EQ(3, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(2, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM
EXPECT_EQ(4, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(3, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH
EXPECT_EQ(5, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
EXPECT_EQ(4, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
@ -187,9 +210,8 @@ TEST(SchedulerUnittest, TestQueue)
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
EXPECT_EQ(4, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
EXPECT_EQ(3, taskQueueSize);
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
EXPECT_EQ(NULL, queueNext());
@ -320,47 +342,47 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue)
TEST(SchedulerUnittest, TestSingleTask)
{
schedulerInit();
// 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(TEST_PID_LOOP_TIME, 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
// disable all tasks except 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);
cfTasks[TASK_ACCEL].lastExecutedAt = 1000;
simulatedTime = 2050;
// run the scheduler and check the task has executed
scheduler();
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(1050, cfTasks[TASK_ACCEL].taskLatestDeltaTime);
EXPECT_EQ(2050, cfTasks[TASK_ACCEL].lastExecutedAt);
EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, cfTasks[TASK_ACCEL].totalExecutionTime);
// task has run, so its dynamic priority should have been set to zero
EXPECT_EQ(0, cfTasks[TASK_GYRO].dynamicPriority);
}
// set it up so that TASK_ACCEL ran just before TASK_GYROPID
TEST(SchedulerUnittest, TestTwoTasks)
{
// disable all tasks except TASK_ACCEL and TASK_ATTITUDE
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_ACCEL, true);
setTaskEnabled(TASK_ATTITUDE, true);
// set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
static const uint32_t startTime = 4000;
simulatedTime = startTime;
cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - TEST_UPDATE_ACCEL_TIME;
EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles);
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime;
cfTasks[TASK_ATTITUDE].lastExecutedAt = cfTasks[TASK_ACCEL].lastExecutedAt - TEST_UPDATE_ATTITUDE_TIME;
EXPECT_EQ(0, cfTasks[TASK_ATTITUDE].taskAgeCycles);
// run the scheduler
scheduler();
// no tasks should have run, since neither task's desired time has elapsed
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
// NOTE:
// TASK_GYROPID desiredPeriod is 1000 microseconds
// TASK_ACCEL desiredPeriod is 10000 microseconds
// TASK_ACCEL desiredPeriod is 1000 microseconds
// TASK_ATTITUDE desiredPeriod is 10000 microseconds
// 500 microseconds later
simulatedTime += 500;
// no tasks should run, since neither task's desired time has elapsed
@ -368,28 +390,213 @@ TEST(SchedulerUnittest, TestTwoTasks)
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
// 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed
// 500 microseconds later, TASK_ACCEL desiredPeriod has elapsed
simulatedTime += 500;
// TASK_GYROPID should now run
// TASK_ACCEL should now run
scheduler();
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
EXPECT_EQ(1, unittest_scheduler_waitingTasks);
EXPECT_EQ(5000 + TEST_PID_LOOP_TIME, simulatedTime);
EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
simulatedTime += 1000 - TEST_PID_LOOP_TIME;
simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
scheduler();
// TASK_GYROPID should run again
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
// TASK_ACCEL should run again
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
scheduler();
// No task should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
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(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
// and finally TASK_ACCEL should now run
simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriods have elapsed
// of the two TASK_ACCEL should run first
scheduler();
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
// and finally TASK_ATTITUDE should now run
scheduler();
EXPECT_EQ(&cfTasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
}
TEST(SchedulerUnittest, TestGyroTask)
{
static const uint32_t startTime = 4000;
// enable the gyro
schedulerEnableGyro();
// disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_GYRO, true);
setTaskEnabled(TASK_FILTER, true);
setTaskEnabled(TASK_PID, true);
// First set it up so TASK_GYRO just ran
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// no tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
// also the gyro, filter and PID task indicators should be false
EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
/* Test the gyro task running but not triggering the filtering or PID */
// set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false
EXPECT_TRUE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// expect that no other tasks other than TASK_GYRO should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
/* Test the gyro task running and triggering the filtering task */
// set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags
resetGyroTaskTestFlags();
taskFilterReady = true;
// run the scheduler
scheduler();
// the gyro and filter task indicators should be true and TASK_PID indicator should be false
EXPECT_TRUE(taskGyroRan);
EXPECT_TRUE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// expect that no other tasks other tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
/* Test the gyro task running and triggering the PID task */
// set the TASK_GYRO last executed time to be one period earlier
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
// reset the flags
resetGyroTaskTestFlags();
taskPidReady = true;
// run the scheduler
scheduler();
// the gyro and PID task indicators should be true and TASK_FILTER indicator should be false
EXPECT_TRUE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_TRUE(taskPidRan);
// expect that no other tasks other tasks should have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
}
// Test the scheduling logic that prevents other tasks from running if they
// might interfere with the timing of the next gyro task.
TEST(SchedulerUnittest, TestGyroLookahead)
{
static const uint32_t startTime = 4000;
// enable task statistics
schedulerSetCalulateTaskStatistics(true);
// disable scheduler optimize rate
schedulerOptimizeRate(false);
// enable the gyro
schedulerEnableGyro();
// disable all tasks except TASK_GYRO, TASK_ACCEL
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
}
setTaskEnabled(TASK_GYRO, true);
setTaskEnabled(TASK_ACCEL, true);
#if defined(USE_TASK_STATISTICS)
// set the average run time for TASK_ACCEL
cfTasks[TASK_ACCEL].movingSumExecutionTime = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT;
#endif
/* Test that another task will run if there's plenty of time till the next gyro sample time */
// set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// the gyro, filter and PID task indicators should be false
EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should have run
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
/* Test that another task won't run if the time till the gyro task is less than the guard interval */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// the gyro, filter and PID task indicators should be false
EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should not have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
/* Test that another task won't run if the time till the gyro task is less than the average task interval */
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
// run the scheduler
scheduler();
// the gyro, filter and PID task indicators should be false
EXPECT_FALSE(taskGyroRan);
EXPECT_FALSE(taskFilterRan);
EXPECT_FALSE(taskPidRan);
// TASK_ACCEL should not have run
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
/* Test that another task will run if the gyro task gets executed */
// set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
simulatedTime = startTime;
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
// reset the flags
resetGyroTaskTestFlags();
// make the TASK_FILTER and TASK_PID ready to run
taskFilterReady = true;
taskPidReady = true;
// run the scheduler
scheduler();
// TASK_GYRO, TASK_FILTER, and TASK_PID should all run
EXPECT_TRUE(taskGyroRan);
EXPECT_TRUE(taskFilterRan);
EXPECT_TRUE(taskPidRan);
// TASK_ACCEL should have run
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
}

17
src/test/unit/sensor_gyro_unittest.cc
View file

@ -85,6 +85,7 @@ TEST(SensorGyro, Calibrate)
{
pgResetAll();
gyroInit();
gyroSetTargetLooptime(1);
fakeGyroSet(gyroDevPtr, 5, 6, 7);
const bool read = gyroDevPtr->readFn(gyroDevPtr);
EXPECT_TRUE(read);
@ -119,16 +120,16 @@ TEST(SensorGyro, Update)
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
gyroInit();
gyroSetTargetLooptime(1);
gyroDevPtr->readFn = fakeGyroRead;
gyroStartCalibration(false);
EXPECT_FALSE(gyroIsCalibrationComplete());
timeUs_t currentTimeUs = 0;
fakeGyroSet(gyroDevPtr, 5, 6, 7);
gyroUpdate(currentTimeUs);
gyroUpdate();
while (!gyroIsCalibrationComplete()) {
fakeGyroSet(gyroDevPtr, 5, 6, 7);
gyroUpdate(currentTimeUs);
gyroUpdate();
}
EXPECT_TRUE(gyroIsCalibrationComplete());
EXPECT_EQ(5, gyroDevPtr->gyroZero[X]);
@ -137,16 +138,16 @@ TEST(SensorGyro, Update)
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
gyroUpdate(currentTimeUs);
gyroUpdate();
// expect zero values since gyro is calibrated
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
fakeGyroSet(gyroDevPtr, 15, 26, 97);
gyroUpdate(currentTimeUs);
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADCf[X], 1e-3); // gyroADCf values are scaled
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADCf[Y], 1e-3);
EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADCf[Z], 1e-3);
gyroUpdate();
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADC[Z], 1e-3);
}
// STUBS

4
src/test/unit/vtx_unittest.cc
View file

@ -113,6 +113,7 @@ TEST(VtxTest, PitMode)
// STUBS
extern "C" {
uint8_t activePidLoopDenom = 1;
uint32_t micros(void) { return simulationTime; }
uint32_t millis(void) { return micros() / 1000; }
bool rxIsReceivingSignal(void) { return simulationHaveRx; }
@ -141,7 +142,7 @@ extern "C" {
void writeServos(void) {};
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
bool isMixerUsingServos(void) { return false; }
void gyroUpdate(timeUs_t) {}
void gyroUpdate() {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; }
@ -184,4 +185,5 @@ extern "C" {
bool compassIsCalibrationComplete(void) { return true; }
bool isUpright(void) { return true; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
}