Use io tags in vtx, move vtx initialization out of OSD init
This commit is contained in:
Evgeny Sychov
parent
7a4d996318
commit
ddf0fb5fda
4
Makefile
4
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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++)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
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<cfTask_t*>(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<cfTask_t*>(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<cfTaskId_e>(taskId), true);
|
||||
EXPECT_EQ(taskId + 1, queueSize());
|
||||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
}
|
||||
EXPECT_EQ(TASK_COUNT - 1, queueSize());
|
||||
EXPECT_NE(static_cast<cfTask_t*>(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<cfTaskId_e>(TASK_COUNT - 1), &taskInfo);
|
||||
EXPECT_EQ(false, taskInfo.isEnabled);
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(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<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = 1000;
|
||||
simulatedTime = 4000;
|
||||
// run the scheduler and check the task has executed
|
||||
scheduler();
|
||||
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime);
|
||||
EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||
EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime);
|
||||
// task has run, so its dynamic priority should have been set to zero
|
||||
EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority);
|
||||
}
|
||||
|
||||
TEST(SchedulerUnittest, TestTwoTasks)
|
||||
{
|
||||
// disable all tasks except TASK_GYROPID and TASK_ACCEL
|
||||
for (int taskId=0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
|
||||
// set it up so that TASK_ACCEL ran just before TASK_GYROPID
|
||||
static const uint32_t startTime = 4000;
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime;
|
||||
EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles);
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// no tasks should have run, since neither task's desired time has elapsed
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(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<cfTask_t*>(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<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
|
||||
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<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
|
||||
simulatedTime = 200700;
|
||||
|
||||
setTaskEnabled(TASK_SYSTEM, true);
|
||||
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
|
||||
|
||||
scheduler();
|
||||
|
||||
EXPECT_EQ(false, unittest_outsideRealtimeGuardInterval);
|
||||
EXPECT_EQ(300, unittest_scheduler_timeToNextRealtimeTask);
|
||||
|
||||
// Nothing should be scheduled in guard period
|
||||
EXPECT_EQ(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<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = 200000;
|
||||
simulatedTime = 200699;
|
||||
|
||||
setTaskEnabled(TASK_SYSTEM, true);
|
||||
cfTasks[TASK_SYSTEM].lastExecutedAt = 100000;
|
||||
|
||||
scheduler();
|
||||
|
||||
EXPECT_EQ(true, unittest_outsideRealtimeGuardInterval);
|
||||
EXPECT_EQ(301, unittest_scheduler_timeToNextRealtimeTask);
|
||||
|
||||
// System should be scheduled as not in guard period
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt);
|
||||
|
||||
EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
extern "C" {
|
||||
}
|
||||
Loading…
Reference in New Issue