From ddf0fb5fdae1084b1ba3283444ccc2c2a9c13558 Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Fri, 17 Jun 2016 02:37:41 -0700 Subject: [PATCH] Use io tags in vtx, move vtx initialization out of OSD init --- Makefile | 4 + src/main/config/config.c | 1 + src/main/config/config.h | 1 + src/main/drivers/max7456.c | 1 + src/main/drivers/vtx_soft_spi_rtc6705.c | 62 ++-- src/main/io/osd.c | 18 -- src/main/main.c | 10 + src/main/target/SIRINFPV/target.h | 12 +- src/main/target/SIRINFPV/target.mk | 2 +- src/test/unit/scheduler_unittest.cc | 407 ++++++++++++++++++++++++ 10 files changed, 448 insertions(+), 70 deletions(-) create mode 100644 src/test/unit/scheduler_unittest.cc diff --git a/Makefile b/Makefile index 0fd276063..8ef29e091 100644 --- a/Makefile +++ b/Makefile @@ -537,6 +537,10 @@ ifneq ($(filter VCP,$(FEATURES)),) TARGET_SRC += $(VCP_SRC) endif +ifneq ($(filter VTX_SOFT, $(FEATURES)),) +TARGET_SRC += $(SRC_DIR)/drivers/rtc6705_soft_spi.c +endif + ifneq ($(filter MAX_OSD, $(FEATURES)),) TARGET_SRC += $(SRC_DIR)/drivers/max7456.c \ $(SRC_DIR)/io/osd.c diff --git a/src/main/config/config.c b/src/main/config/config.c index c0dce56c9..8f94f7de6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -425,6 +425,7 @@ static void resetConf(void) #endif #ifdef USE_RTC6705 + featureSet(FEATURE_VTX); masterConfig.vtx_channel = 19; // default to Boscam E channel 4 masterConfig.vtx_power = 1; #endif diff --git a/src/main/config/config.h b/src/main/config/config.h index 219cc31be..9150540cf 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -47,6 +47,7 @@ typedef enum { FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_OSD = 1 << 24, + FEATURE_VTX = 1 << 25, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index a8065b075..f82473cba 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -114,6 +114,7 @@ void max7456_init(uint8_t video_system) { DISABLE_MAX7456; delay(100); + // display logo x = 160; for (int i = 1; i < 5; i++) { for (int j = 3; j < 27; j++) diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index b4b8ffce1..9dbd17d85 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -25,18 +25,18 @@ #include "drivers/bus_spi.h" #include "drivers/system.h" -#include "drivers/gpio.h" +#include "drivers/light_led.h" #include "vtx_soft_spi_rtc6705.h" -#define RTC6705_SPICLK_ON GPIO_SetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) -#define RTC6705_SPICLK_OFF GPIO_ResetBits(RTC6705_SPICLK_GPIO, RTC6705_SPICLK_PIN) +#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin) +#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin) -#define RTC6705_SPIDATA_ON GPIO_SetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) -#define RTC6705_SPIDATA_OFF GPIO_ResetBits(RTC6705_SPIDATA_GPIO, RTC6705_SPIDATA_PIN) +#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin) +#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin) -#define RTC6705_SPILE_ON GPIO_SetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) -#define RTC6705_SPILE_OFF GPIO_ResetBits(RTC6705_SPILE_GPIO, RTC6705_SPILE_PIN) +#define RTC6705_SPILE_ON IOHi(rtc6705LePin) +#define RTC6705_SPILE_OFF IOLo(rtc6705LePin) char *vtx_bands[] = { "BOSCAM A", @@ -57,46 +57,24 @@ uint16_t vtx_freq[] = uint16_t current_vtx_channel; +static IO_t rtc6705DataPin = IO_NONE; +static IO_t rtc6705LePin = IO_NONE; +static IO_t rtc6705ClkPin = IO_NONE; + void rtc6705_soft_spi_init(void) { - gpio_config_t gpio; -#ifdef STM32F303 - #ifdef RTC6705_SPICLK_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPILE_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPIDATA_PERIPHERAL - RCC_AHBPeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); - #endif -#endif -#ifdef STM32F10X - #ifdef RTC6705_SPICLK_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPICLK_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPILE_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPILE_PERIPHERAL, ENABLE); - #endif - #ifdef RTC6705_SPIDATA_PERIPHERAL - RCC_APB2PeriphClockCmd(RTC6705_SPIDATA_PERIPHERAL, ENABLE); - #endif -#endif + rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPIDATA_PIN)); + rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN)); + rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); - gpio.pin = RTC6705_SPICLK_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPICLK_GPIO, &gpio); + IOInit(rtc6705DataPin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); - gpio.pin = RTC6705_SPILE_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPILE_GPIO, &gpio); + IOInit(rtc6705LePin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP); - gpio.pin = RTC6705_SPIDATA_PIN; - gpio.speed = Speed_50MHz; - gpio.mode = Mode_Out_PP; - gpioInit(RTC6705_SPIDATA_GPIO, &gpio); + IOInit(rtc6705ClkPin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); } static void rtc6705_write_register(uint8_t addr, uint32_t data) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e7651c88f..f23134d89 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -347,17 +347,6 @@ void print_batt_voltage(uint16_t pos, uint8_t col) { max7456_write_string(string_buffer, pos); } -/* - TODO: add this to menu - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, - { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, - { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, - { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, - { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } }, - { "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } }, -*/ - osd_page_t menu_pages[] = { { .title = "STATUS", @@ -711,14 +700,7 @@ void updateOsd(void) void osdInit(void) { -#ifdef USE_RTC6705 - rtc6705_soft_spi_init(); - current_vtx_channel = masterConfig.vtx_channel; - rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); - rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); -#endif max7456_init(masterConfig.osdProfile.video_system); - } void resetOsdConfig(void) diff --git a/src/main/main.c b/src/main/main.c index e221da6dc..6ab9e369f 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -55,6 +55,7 @@ #include "drivers/transponder_ir.h" #include "drivers/io.h" #include "drivers/exti.h" +#include "drivers/vtx_soft_spi_rtc6705.h" #ifdef USE_BST #include "bus_bst.h" @@ -469,6 +470,15 @@ void init(void) } #endif +#ifdef USE_RTC6705 + if (feature(feature(FEATURE_VTX))) { + rtc6705_soft_spi_init(); + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); + rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); + } +#endif + #ifdef OSD if (feature(FEATURE_OSD)) { osdInit(); diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 7b98c9061..667f6d173 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -113,15 +113,9 @@ #define MAX7456_SPI_CS_PIN PA15 #define USE_RTC6705 -#define RTC6705_SPIDATA_GPIO GPIOC -#define RTC6705_SPIDATA_PIN Pin_15 -#define RTC6705_SPIDATA_PERIPHERAL RCC_AHBPeriph_GPIOC -#define RTC6705_SPILE_GPIO GPIOC -#define RTC6705_SPILE_PIN Pin_14 -#define RTC6705_SPILE_PERIPHERAL RCC_AHBPeriph_GPIOC -#define RTC6705_SPICLK_GPIO GPIOC -#define RTC6705_SPICLK_PIN Pin_13 -#define RTC6705_SPICLK_PERIPHERAL RCC_AHBPeriph_GPIOC +#define RTC6705_SPIDATA_PIN PC15 +#define RTC6705_SPILE_PIN PC14 +#define RTC6705_SPICLK_PIN PC13 #define USE_SDCARD #define USE_SDCARD_SPI2 diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 1dfe1ea32..39004bec7 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD MAX_OSD +FEATURES = VCP SDCARD MAX_OSD SOFT_VTX TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc new file mode 100644 index 000000000..d998f2f4e --- /dev/null +++ b/src/test/unit/scheduler_unittest.cc @@ -0,0 +1,407 @@ +/* + * 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 . + */ + +#include + +extern "C" { + #include "platform.h" + #include "scheduler/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 bool queueAdd(cfTask_t *task); + extern bool queueRemove(cfTask_t *task); + extern cfTask_t *queueFirst(void); + extern cfTask_t *queueNext(void); +} + +TEST(SchedulerUnittest, TestPriorites) +{ + EXPECT_EQ(14, TASK_COUNT); + // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked + 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, TestQueueInit) +{ + queueClear(); + EXPECT_EQ(0, queueSize()); + EXPECT_EQ(0, queueFirst()); + EXPECT_EQ(0, queueNext()); + for (int ii = 0; ii <= TASK_COUNT; ++ii) { + EXPECT_EQ(0, taskQueueArray[ii]); + } +} + +cfTask_t *deadBeefPtr = reinterpret_cast(0xDEADBEEF); + +TEST(SchedulerUnittest, TestQueue) +{ + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; + + queueAdd(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH + EXPECT_EQ(1, queueSize()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME + EXPECT_EQ(2, queueSize()); + 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, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + queueAdd(&cfTasks[TASK_BATTERY]); // TASK_PRIORITY_MEDIUM + EXPECT_EQ(4, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_BATTERY], 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, queueSize()); + EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); + EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); + EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + 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_BATTERY], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); +} + +TEST(SchedulerUnittest, TestQueueAddAndRemove) +{ + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; + + // fill up the queue + for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { + const bool added = queueAdd(&cfTasks[taskId]); + EXPECT_EQ(true, added); + EXPECT_EQ(taskId + 1, queueSize()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + // double check end of queue + EXPECT_EQ(TASK_COUNT, queueSize()); + EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 1]); // last item was indeed added to queue + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // null pointer at end of queue is preserved + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // there hasn't been an out by one error + + // and empty it again + for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { + const bool removed = queueRemove(&cfTasks[taskId]); + EXPECT_EQ(true, removed); + EXPECT_EQ(TASK_COUNT - taskId - 1, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - taskId]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + // double check size and end of queue + EXPECT_EQ(0, queueSize()); // queue is indeed empty + EXPECT_EQ(NULL, taskQueueArray[0]); // there is a null pointer at the end of the queueu + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); // no accidental overwrites past end of queue +} + +TEST(SchedulerUnittest, TestQueueArray) +{ + // test there are no "out by one" errors or buffer overruns when items are added and removed + queueClear(); + taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear + + for (int taskId = 0; taskId < TASK_COUNT - 1; ++taskId) { + setTaskEnabled(static_cast(taskId), true); + EXPECT_EQ(taskId + 1, queueSize()); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + } + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + EXPECT_NE(static_cast(0), taskQueueArray[TASK_COUNT - 2]); + const cfTask_t *lastTaskPrev = taskQueueArray[TASK_COUNT - 2]; + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_SYSTEM, false); + EXPECT_EQ(TASK_COUNT - 2, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); // NULL at end of queue + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + taskQueueArray[TASK_COUNT - 2] = 0; + setTaskEnabled(TASK_SYSTEM, true); + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + cfTaskInfo_t taskInfo; + getTaskInfo(static_cast(TASK_COUNT - 1), &taskInfo); + EXPECT_EQ(false, taskInfo.isEnabled); + setTaskEnabled(static_cast(TASK_COUNT - 1), true); + EXPECT_EQ(TASK_COUNT, queueSize()); + EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); // check no buffer overrun + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_SYSTEM, false); + EXPECT_EQ(TASK_COUNT - 1, queueSize()); + //EXPECT_EQ(lastTaskPrev, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_ACCEL, false); + EXPECT_EQ(TASK_COUNT - 2, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); + + setTaskEnabled(TASK_BATTERY, false); + EXPECT_EQ(TASK_COUNT - 3, queueSize()); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 3]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 2]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT - 1]); + EXPECT_EQ(NULL, taskQueueArray[TASK_COUNT]); + EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); +} + +TEST(SchedulerUnittest, TestSchedulerInit) +{ + schedulerInit(); + EXPECT_EQ(1, queueSize()); + EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); +} + +TEST(SchedulerUnittest, TestScheduleEmptyQueue) +{ + queueClear(); + simulatedTime = 4000; + // run the with an empty queue + scheduler(); + EXPECT_EQ(NULL, unittest_scheduler_selectedTask); +} + +TEST(SchedulerUnittest, TestSingleTask) +{ + schedulerInit(); + // disable all tasks except TASK_GYROPID + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(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(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(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(static_cast(0), unittest_scheduler_selectedTask); + + // 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(static_cast(0), unittest_scheduler_selectedTask); + EXPECT_EQ(0, unittest_scheduler_waitingTasks); + + // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed + simulatedTime += 500; + // TASK_GYROPID should now run + scheduler(); + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(1, unittest_scheduler_waitingTasks); + EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime); + + simulatedTime += 1000 - pidLoopCheckerTime; + scheduler(); + // TASK_GYROPID should run again + EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + + scheduler(); + EXPECT_EQ(static_cast(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 + scheduler(); + EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); +} + +TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) +{ + // disable all tasks except TASK_GYROPID and TASK_SYSTEM + for (int taskId=0; taskId < TASK_COUNT; ++taskId) { + setTaskEnabled(static_cast(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(NULL, unittest_scheduler_selectedTask); + 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(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(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask); + EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt); + + EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); +} + +// STUBS +extern "C" { +}