diff --git a/Makefile b/Makefile index cbe5ad123d..88f600479d 100755 --- a/Makefile +++ b/Makefile @@ -66,7 +66,7 @@ VPATH := $(SRC_DIR):$(SRC_DIR)/startup USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) -CSOURCES := $(shell find $(SRC_DIR) -name '*.c') +CSOURCES := $(shell find $(SRC_DIR) -name '*.c') ifeq ($(TARGET),$(filter $(TARGET),ALIENWIIF3 CHEBUZZF3 COLIBRI_RACE LUX_RACE MOTOLAB NAZE32PRO RMDO SPARKY SPRACINGF3 STM32F3DISCOVERY)) @@ -208,7 +208,7 @@ TARGET_DIR = $(ROOT)/src/main/target/NAZE endif INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(TARGET_DIR) + $(TARGET_DIR) VPATH := $(VPATH):$(TARGET_DIR) @@ -223,8 +223,8 @@ COMMON_SRC = build_config.c \ common/typeconversion.c \ common/encoding.c \ common/filter.c \ - scheduler.c \ - scheduler_tasks.c \ + scheduler/scheduler.c \ + scheduler/scheduler_tasks.c \ main.c \ mw.c \ flight/failsafe.c \ @@ -236,7 +236,7 @@ COMMON_SRC = build_config.c \ drivers/serial.c \ drivers/sound_beeper.c \ drivers/system.c \ - drivers/gps_i2cnav.c \ + drivers/gps_i2cnav.c \ drivers/gyro_sync.c \ drivers/buf_writer.c \ io/beeper.c \ @@ -267,18 +267,18 @@ COMMON_SRC = build_config.c \ HIGHEND_SRC = \ flight/gtune.c \ - flight/navigation_rewrite.c \ - flight/navigation_rewrite_multicopter.c \ - flight/navigation_rewrite_fixedwing.c \ - flight/navigation_rewrite_pos_estimator.c \ - flight/navigation_rewrite_geo.c \ + flight/navigation_rewrite.c \ + flight/navigation_rewrite_multicopter.c \ + flight/navigation_rewrite_fixedwing.c \ + flight/navigation_rewrite_pos_estimator.c \ + flight/navigation_rewrite_geo.c \ flight/gps_conversion.c \ common/colorconversion.c \ io/gps.c \ io/gps_ublox.c \ - io/gps_nmea.c \ - io/gps_naza.c \ - io/gps_i2cnav.c \ + io/gps_nmea.c \ + io/gps_naza.c \ + io/gps_i2cnav.c \ io/ledstrip.c \ io/display.c \ telemetry/telemetry.c \ @@ -319,8 +319,8 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/bus_spi.c \ drivers/bus_i2c_stm32f10x.c \ drivers/compass_hmc5883l.c \ - drivers/compass_mag3110.c \ - drivers/compass_ak8975.c \ + drivers/compass_mag3110.c \ + drivers/compass_ak8975.c \ drivers/display_ug2864hsweg01.h \ drivers/flash_m25p16.c \ drivers/gpio_stm32f10x.c \ @@ -363,7 +363,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \ drivers/bus_i2c_stm32f10x.c \ drivers/bus_spi.c \ drivers/compass_ak8975.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.c \ drivers/flash_m25p16.c \ @@ -398,8 +398,8 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ drivers/bus_i2c_stm32f10x.c \ drivers/bus_spi.c \ drivers/compass_hmc5883l.c \ - drivers/compass_mag3110.c \ - drivers/compass_ak8975.c \ + drivers/compass_mag3110.c \ + drivers/compass_ak8975.c \ drivers/gpio_stm32f10x.c \ drivers/light_led_stm32f10x.c \ drivers/light_ws2811strip.c \ @@ -425,9 +425,9 @@ CJMCU_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/bus_i2c_stm32f10x.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_hmc5883l.c \ - drivers/compass_ak8975.c \ + drivers/compass_ak8975.c \ drivers/gpio_stm32f10x.c \ drivers/light_led_stm32f10x.c \ drivers/pwm_mapping.c \ @@ -456,9 +456,9 @@ CC3D_SRC = \ drivers/barometer_bmp280.c \ drivers/bus_spi.c \ drivers/bus_i2c_stm32f10x.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_hmc5883l.c \ - drivers/compass_ak8975.c \ + drivers/compass_ak8975.c \ drivers/display_ug2864hsweg01.c \ drivers/flash_m25p16.c \ drivers/gpio_stm32f10x.c \ @@ -513,9 +513,9 @@ STM32F3DISCOVERY_COMMON_SRC = \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3gd20.c \ drivers/accgyro_lsm303dlhc.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_hmc5883l.c \ - drivers/compass_ak8975.c \ + drivers/compass_ak8975.c \ $(VCP_SRC) STM32F3DISCOVERY_SRC = \ @@ -528,8 +528,8 @@ STM32F3DISCOVERY_SRC = \ drivers/accgyro_mpu6050.c \ drivers/accgyro_l3g4200d.c \ drivers/barometer_ms5611.c \ - drivers/compass_mag3110.c \ - drivers/compass_hmc5883l.c \ + drivers/compass_mag3110.c \ + drivers/compass_hmc5883l.c \ drivers/compass_ak8975.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) @@ -547,7 +547,7 @@ COLIBRI_RACE_SRC = \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ drivers/barometer_ms5611.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/serial_usb_vcp.c \ @@ -576,7 +576,7 @@ SPARKY_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_ms5611.c \ drivers/barometer_bmp280.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/serial_usb_vcp.c \ @@ -594,7 +594,7 @@ RMDO_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_ms5611.c \ drivers/barometer_bmp280.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.h \ @@ -612,7 +612,7 @@ SPRACINGF3_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_ms5611.c \ drivers/barometer_bmp280.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.h \ @@ -628,7 +628,7 @@ MOTOLAB_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ - drivers/compass_mag3110.c \ + drivers/compass_mag3110.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ drivers/display_ug2864hsweg01.c \ @@ -699,14 +699,14 @@ LDFLAGS = -lm \ -static \ -Wl,-gc-sections,-Map,$(TARGET_MAP) \ -Wl,-L$(LINKER_DIR) \ - -Wl,--cref \ + -Wl,--cref \ -T$(LD_SCRIPT) ############################################################################### # No user-serviceable parts below ############################################################################### -CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ +CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ --std=c99 --inline-suppr --quiet --force \ $(addprefix -I,$(INCLUDE_DIRS)) \ -I/usr/include -I/usr/include/linux @@ -726,19 +726,19 @@ TARGET_DEPS = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $( TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map ## Default make goal: -## hex : Make filetype hex only +## hex : Make filetype hex only .DEFAULT_GOAL := hex ## Optional make goals: -## all : Make all filetypes, binary and hex +## all : Make all filetypes, binary and hex all: hex bin -## binary : Make binary filetype -## bin : Alias of 'binary' -## hex : Make hex filetype -bin: $(TARGET_BIN) +## binary : Make binary filetype +## bin : Alias of 'binary' +## hex : Make hex filetype +bin: $(TARGET_BIN) binary: $(TARGET_BIN) -hex: $(TARGET_HEX) +hex: $(TARGET_HEX) # rule to reinvoke make with TARGET= parameter # rules that should be handled in toplevel Makefile, not dependent on TARGET @@ -752,7 +752,7 @@ $(VALID_TARGETS): .PHONY: all_targets all_targets : $(VALID_TARGETS) -## clean : clean up all temporary / machine-generated files +## clean : clean up all temporary / machine-generated files clean: rm -f $(TARGET_BIN) $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) rm -rf $(OBJECT_DIR)/$(TARGET) @@ -763,13 +763,13 @@ flash_$(TARGET): $(TARGET_HEX) echo -n 'R' >$(SERIAL_DEVICE) stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) -## flash : flash firmware (.hex) onto flight controller +## flash : flash firmware (.hex) onto flight controller flash: flash_$(TARGET) st-flash_$(TARGET): $(TARGET_BIN) st-flash --reset write $< 0x08000000 -## st-flash : flash firmware (.bin) onto flight controller +## st-flash : flash firmware (.bin) onto flight controller st-flash: st-flash_$(TARGET) binary: $(TARGET_BIN) @@ -778,29 +778,29 @@ unbrick_$(TARGET): $(TARGET_HEX) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) -## unbrick : unbrick flight controller +## unbrick : unbrick flight controller unbrick: unbrick_$(TARGET) -## cppcheck : run static analysis on C source code +## cppcheck : run static analysis on C source code cppcheck: $(CSOURCES) $(CPPCHECK) cppcheck-result.xml: $(CSOURCES) $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml -## help : print this help message and exit +## help : print this help message and exit help: Makefile @echo "" @echo "Makefile for the $(FORKNAME) firmware" @echo "" @echo "Usage:" - @echo " make [TARGET=] [OPTIONS=\"\"]" + @echo " make [TARGET=] [OPTIONS=\"\"]" @echo "" @echo "Valid TARGET values are: $(VALID_TARGETS)" @echo "" @sed -n 's/^## //p' $< -## test : run the cleanflight test suite +## test : run the cleanflight test suite test: cd src/test && $(MAKE) test || true diff --git a/src/main/build_config.h b/src/main/build_config.h index 1810347241..e396983f9f 100644 --- a/src/main/build_config.h +++ b/src/main/build_config.h @@ -21,10 +21,15 @@ #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)])) #ifdef UNIT_TEST -#define STATIC_UNIT_TESTED // make visible to unit test +// make these visible to unit test +#define STATIC_UNIT_TESTED +#define STATIC_INLINE_UNIT_TESTED +#define INLINE_UNIT_TESTED #define UNIT_TESTED #else #define STATIC_UNIT_TESTED static +#define STATIC_INLINE_UNIT_TESTED static inline +#define INLINE_UNIT_TESTED inline #define UNIT_TESTED #endif diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h new file mode 100644 index 0000000000..eee4b83109 --- /dev/null +++ b/src/main/config/config_unittest.h @@ -0,0 +1,100 @@ +/* + * 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 . + */ + +#pragma once + +#ifdef SRC_MAIN_SCHEDULER_C_ +#ifdef UNIT_TEST + +cfTask_t *unittest_scheduler_selectedTask; +uint8_t unittest_scheduler_selectedTaskDynamicPriority; +uint16_t unittest_scheduler_waitingTasks; +uint32_t unittest_scheduler_timeToNextRealtimeTask; +bool unittest_outsideRealtimeGuardInterval; + +#define GET_SCHEDULER_LOCALS() \ + { \ + unittest_scheduler_selectedTask = selectedTask; \ + unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \ + 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_ + diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index cd2e1d2ad2..34957a63fd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -24,9 +24,10 @@ #include #include "platform.h" -#include "scheduler.h" #include "version.h" +#include "scheduler/scheduler.h" + #include "build_config.h" #include "common/axis.h" @@ -2442,11 +2443,13 @@ static void cliTasks(char *cmdline) cfTaskId_e taskId; cfTaskInfo_t taskInfo; - cliPrintf("Task list:\r\n"); + cliPrintf("Task list max/us avg/us rate/hz total/ms\r\n"); 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); + const uint16_t taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); + cliPrintf("%2d - %12s, %6d, %5d, %5d, %8d\r\n", + taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, taskInfo.totalExecutionTime / 1000); } } } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0c8fea4c08..b18459f766 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -24,7 +24,8 @@ #include "build_config.h" #include "debug.h" #include "platform.h" -#include "scheduler.h" + +#include "scheduler/scheduler.h" #include "common/axis.h" #include "common/color.h" diff --git a/src/main/main.c b/src/main/main.c index 97a1c9617e..4dc0a4ecf9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -21,7 +21,8 @@ #include #include "platform.h" -#include "scheduler.h" + +#include "scheduler/scheduler.h" #include "common/axis.h" #include "common/color.h" @@ -542,16 +543,15 @@ int main(void) { init(); /* Setup scheduler */ - if (masterConfig.gyroSync) { - rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME); - } - else { - rescheduleTask(TASK_GYROPID, targetLooptime); - } + schedulerInit(); + rescheduleTask(TASK_GYROPID, targetLooptime); setTaskEnabled(TASK_GYROPID, true); + setTaskEnabled(TASK_SERIAL, true); +#ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); +#endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS diff --git a/src/main/mw.c b/src/main/mw.c index 3f4bce1891..6c8404321f 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -21,9 +21,10 @@ #include #include "platform.h" -#include "scheduler.h" #include "debug.h" +#include "scheduler/scheduler.h" + #include "common/maths.h" #include "common/axis.h" #include "common/color.h" diff --git a/src/main/scheduler.c b/src/main/scheduler.c deleted file mode 100755 index 818f5face1..0000000000 --- a/src/main/scheduler.c +++ /dev/null @@ -1,226 +0,0 @@ -/* - * 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 -#include -#include - -#include "platform.h" -#include "scheduler.h" -#include "debug.h" - -#include "common/maths.h" - -#include "drivers/system.h" - -cfTaskId_e currentTaskId = TASK_NONE; - -static uint32_t totalWaitingTasks; -static uint32_t totalWaitingTasksSamples; -static uint32_t realtimeGuardInterval; - -uint32_t currentTime = 0; -uint16_t averageSystemLoadPercent = 0; - -#define REALTIME_GUARD_INTERVAL_MIN 10 -#define REALTIME_GUARD_INTERVAL_MAX 300 -#define REALTIME_GUARD_INTERVAL_MARGIN 25 - -void taskSystem(void) -{ - uint8_t taskId; - - /* Calculate system load */ - if (totalWaitingTasksSamples > 0) { - averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples; - totalWaitingTasksSamples = 0; - totalWaitingTasks = 0; - } - - /* 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 = 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 -void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) -{ - taskInfo->taskName = cfTasks[taskId].taskName; - 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; -} -#endif - -void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros) -{ - if (taskId == TASK_SELF) - taskId = currentTaskId; - - if (taskId < TASK_COUNT) { - cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging - } -} - -void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState) -{ - if (taskId == TASK_SELF) - taskId = currentTaskId; - - if (taskId < TASK_COUNT) { - cfTasks[taskId].isEnabled = newEnabledState; - } -} - -uint32_t getTaskDeltaTime(cfTaskId_e taskId) -{ - if (taskId == TASK_SELF) - taskId = currentTaskId; - - if (taskId < TASK_COUNT) { - return cfTasks[taskId].taskLatestDeltaTime; - } - else { - return 0; - } -} - -void scheduler(void) -{ - uint8_t taskId; - uint8_t selectedTaskId; - uint8_t selectedTaskDynPrio; - uint16_t waitingTasks = 0; - uint32_t timeToNextRealtimeTask = UINT32_MAX; - - /* Cache currentTime */ - currentTime = micros(); - - /* The task to be invoked */ - selectedTaskId = TASK_NONE; - selectedTaskDynPrio = 0; - - /* Check for realtime tasks */ - for (taskId = 0; taskId < TASK_COUNT; taskId++) { - if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) { - uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod; - if ((int32_t)(currentTime - nextExecuteAt) >= 0) { - timeToNextRealtimeTask = 0; - } - else { - uint32_t newTimeInterval = nextExecuteAt - currentTime; - timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval); - } - } - } - - bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval); - - /* Update task dynamic priorities */ - for (taskId = 0; taskId < TASK_COUNT; taskId++) { - if (cfTasks[taskId].isEnabled) { - /* Task has checkFunc - event driven */ - if (cfTasks[taskId].checkFunc != NULL) { - /* Increase priority for event driven tasks */ - if (cfTasks[taskId].dynamicPriority > 0) { - cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod); - cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles; - waitingTasks++; - } - else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) { - cfTasks[taskId].lastSignaledAt = currentTime; - cfTasks[taskId].taskAgeCycles = 1; - cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority; - waitingTasks++; - } - else { - cfTasks[taskId].taskAgeCycles = 0; - } - } - /* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */ - else { - // Task age is calculated from last execution - cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod); - if (cfTasks[taskId].taskAgeCycles > 0) { - cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles; - waitingTasks++; - } - } - - /* limit new priority to avoid overflow of uint8_t */ - cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);; - - bool taskCanBeChosenForScheduling = - (outsideRealtimeGuardInterval) || - (cfTasks[taskId].taskAgeCycles > 1) || - (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME); - - if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) { - selectedTaskDynPrio = cfTasks[taskId].dynamicPriority; - selectedTaskId = taskId; - } - } - } - - totalWaitingTasksSamples += 1; - totalWaitingTasks += waitingTasks; - - /* Found a task that should be run */ - if (selectedTaskId != TASK_NONE) { - cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt; - cfTasks[selectedTaskId].lastExecutedAt = currentTime; - cfTasks[selectedTaskId].dynamicPriority = 0; - - currentTaskId = selectedTaskId; - - uint32_t currentTimeBeforeTaskCall = micros(); - - /* Execute task */ - if (cfTasks[selectedTaskId].taskFunc != NULL) { - cfTasks[selectedTaskId].taskFunc(); - } - - uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; - - cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32; -#ifndef SKIP_TASK_STATISTICS - cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task - cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime); -#endif -#if defined SCHEDULER_DEBUG - debug[3] = (micros() - currentTime) - taskExecutionTime; -#endif - } - else { - currentTaskId = TASK_NONE; -#if defined SCHEDULER_DEBUG - debug[3] = (micros() - currentTime); -#endif - } -} diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c new file mode 100755 index 0000000000..13f665800f --- /dev/null +++ b/src/main/scheduler/scheduler.c @@ -0,0 +1,286 @@ +/* + * 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 +#include +#include + +#ifdef UNIT_TEST +typedef enum {TEST_IRQ = 0 } IRQn_Type; +#endif +#include "platform.h" + +#include "scheduler.h" +#include "debug.h" +#include "build_config.h" + +#include "common/maths.h" + +#include "drivers/system.h" + +static cfTask_t *currentTask = NULL; + +static uint32_t totalWaitingTasks; +static uint32_t totalWaitingTasksSamples; +static uint32_t realtimeGuardInterval; + +uint32_t currentTime = 0; +uint16_t averageSystemLoadPercent = 0; + +#define REALTIME_GUARD_INTERVAL_MIN 10 +#define REALTIME_GUARD_INTERVAL_MAX 300 +#define REALTIME_GUARD_INTERVAL_MARGIN 25 + +static int taskQueuePos = 0; +static int taskQueueSize = 0; +// 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 +#endif +STATIC_UNIT_TESTED void queueClear(void) +{ + memset(taskQueueArray, 0, 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 < taskQueueSize; ++ii) { + if (taskQueueArray[ii] == task) { + return true; + } + } + return false; +} + +STATIC_UNIT_TESTED bool queueAdd(cfTask_t *task) +{ + if ((taskQueueSize >= TASK_COUNT) || queueContains(task)) { + return false; + } + 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)); + taskQueueArray[ii] = task; + ++taskQueueSize; + return true; + } + } + return false; +} + +STATIC_UNIT_TESTED bool queueRemove(cfTask_t *task) +{ + for (int ii = 0; ii < taskQueueSize; ++ii) { + if (taskQueueArray[ii] == task) { + memmove(&taskQueueArray[ii], &taskQueueArray[ii+1], sizeof(task) * (taskQueueSize - ii)); + --taskQueueSize; + return true; + } + } + return false; +} + +/* + * Returns first item queue or NULL if queue empty + */ +STATIC_INLINE_UNIT_TESTED cfTask_t *queueFirst(void) +{ + taskQueuePos = 0; + return taskQueueArray[0]; // guaranteed to be NULL if queue is empty +} + +/* + * Returns next item in queue or NULL if at end of queue + */ +STATIC_INLINE_UNIT_TESTED cfTask_t *queueNext(void) +{ + return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue +} + +void taskSystem(void) +{ + /* Calculate system load */ + if (totalWaitingTasksSamples > 0) { + averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples; + totalWaitingTasksSamples = 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 +void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) +{ + taskInfo->taskName = cfTasks[taskId].taskName; + taskInfo->isEnabled = queueContains(&cfTasks[taskId]); + 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 + +void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros) +{ + if (taskId == TASK_SELF || taskId < TASK_COUNT) { + cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId]; + task->desiredPeriod = MAX((uint32_t)100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging + } +} + +void setTaskEnabled(cfTaskId_e taskId, bool enabled) +{ + if (taskId == TASK_SELF || taskId < TASK_COUNT) { + cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId]; + if (enabled && task->taskFunc) { + queueAdd(task); + } else { + queueRemove(task); + } + } +} + +uint32_t getTaskDeltaTime(cfTaskId_e taskId) +{ + if (taskId == TASK_SELF || taskId < TASK_COUNT) { + cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId]; + return task->taskLatestDeltaTime; + } else { + return 0; + } +} + +void schedulerInit(void) +{ + queueClear(); + queueAdd(&cfTasks[TASK_SYSTEM]); +} + +void scheduler(void) +{ + // Cache currentTime + currentTime = micros(); + + uint32_t timeToNextRealtimeTask = UINT32_MAX; + for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) { + const uint32_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod; + if ((int32_t)(currentTime - nextExecuteAt) >= 0) { + timeToNextRealtimeTask = 0; + } else { + const uint32_t newTimeInterval = nextExecuteAt - currentTime; + timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval); + } + } + const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval); + + // The task to be invoked + cfTask_t *selectedTask = NULL; + uint16_t selectedTaskDynamicPriority = 0; + + // Update task dynamic priorities + uint16_t waitingTasks = 0; + for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { + // Task has checkFunc - event driven + if (task->checkFunc != NULL) { + // Increase priority for event driven tasks + if (task->dynamicPriority > 0) { + task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod); + task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; + waitingTasks++; + } else if (task->checkFunc(currentTime - task->lastExecutedAt)) { + task->lastSignaledAt = currentTime; + task->taskAgeCycles = 1; + task->dynamicPriority = 1 + task->staticPriority; + waitingTasks++; + } else { + task->taskAgeCycles = 0; + } + } else { + // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) + // Task age is calculated from last execution + task->taskAgeCycles = ((currentTime - task->lastExecutedAt) / task->desiredPeriod); + if (task->taskAgeCycles > 0) { + task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; + waitingTasks++; + } + } + + if (task->dynamicPriority > selectedTaskDynamicPriority) { + const bool taskCanBeChosenForScheduling = + (outsideRealtimeGuardInterval) || + (task->taskAgeCycles > 1) || + (task->staticPriority == TASK_PRIORITY_REALTIME); + if (taskCanBeChosenForScheduling) { + selectedTaskDynamicPriority = task->dynamicPriority; + selectedTask = task; + } + } + } + + totalWaitingTasksSamples++; + totalWaitingTasks += waitingTasks; + + currentTask = selectedTask; + + if (selectedTask != NULL) { + // Found a task that should be run + selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt; + selectedTask->lastExecutedAt = currentTime; + selectedTask->dynamicPriority = 0; + + // Execute task + const uint32_t currentTimeBeforeTaskCall = micros(); + selectedTask->taskFunc(); + const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; + + selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32; +#ifndef SKIP_TASK_STATISTICS + selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task + selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); +#endif +#if defined SCHEDULER_DEBUG + debug[3] = (micros() - currentTime) - taskExecutionTime; + } else { + debug[3] = (micros() - currentTime); +#endif + } +} diff --git a/src/main/scheduler.h b/src/main/scheduler/scheduler.h similarity index 82% rename from src/main/scheduler.h rename to src/main/scheduler/scheduler.h index d734b095b9..2bd7b47e2e 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -36,6 +36,7 @@ typedef struct { uint32_t maxExecutionTime; uint32_t totalExecutionTime; uint32_t averageExecutionTime; + uint32_t latestDeltaTime; } cfTaskInfo_t; typedef enum { @@ -81,18 +82,17 @@ typedef struct { const char * taskName; bool (*checkFunc)(uint32_t currentDeltaTime); void (*taskFunc)(void); - bool isEnabled; - uint32_t desiredPeriod; // target period of execution - uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero + uint32_t desiredPeriod; // target period of execution + const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero /* Scheduling */ - uint8_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation - uint32_t lastExecutedAt; // last time of invocation - uint32_t lastSignaledAt; // time of invocation event for event-driven tasks + uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation uint16_t taskAgeCycles; + uint32_t lastExecutedAt; // last time of invocation + uint32_t lastSignaledAt; // time of invocation event for event-driven tasks /* Statistics */ - uint32_t averageExecutionTime; // Moving averate over 6 samples, used to calculate guard interval + uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval uint32_t taskLatestDeltaTime; // #ifndef SKIP_TASK_STATISTICS uint32_t maxExecutionTime; @@ -109,6 +109,7 @@ void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros); void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); uint32_t getTaskDeltaTime(cfTaskId_e taskId); +void schedulerInit(void); void scheduler(void); #define isSystemOverloaded() (averageSystemLoadPercent >= 100) diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c similarity index 99% rename from src/main/scheduler_tasks.c rename to src/main/scheduler/scheduler_tasks.c index ba7ac94cfd..78784a0039 100755 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -39,7 +39,6 @@ void taskSystem(void); cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { - .isEnabled = true, .taskName = "SYSTEM", .taskFunc = taskSystem, .desiredPeriod = 1000000 / 10, // run every 100 ms diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a8b5b4622a..d40bfbca5f 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "scheduler.h" + +#include "scheduler/scheduler.h" #include "common/maths.h" diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc new file mode 100644 index 0000000000..4313963977 --- /dev/null +++ b/src/test/unit/scheduler_unittest.cc @@ -0,0 +1,405 @@ +/* + * 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.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_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()); + 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_BEEPER], 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_BEEPER], queueNext()); + EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); + EXPECT_EQ(NULL, queueNext()); +} + +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; + + for (int taskId=0; taskId < TASK_COUNT - 1; ++taskId) { + setTaskEnabled(static_cast(taskId), true); + } + 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" { +} +