Gyro native rate sampling, filtering, and scheduler restructuring
This commit is contained in:
Bruce Luckcuck
parent
b11565efbe
commit
f584780944
|
|
@ -1270,9 +1270,9 @@ static bool blackboxWriteSysinfo(void)
|
|||
}
|
||||
);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.sampleLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", 1);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", activePidLoopDenom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
|
||||
|
|
|
|||
|
|
@ -94,4 +94,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"FF_LIMIT",
|
||||
"FF_INTERPOLATED",
|
||||
"BLACKBOX_OUTPUT",
|
||||
"GYRO_SAMPLE",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -110,6 +110,7 @@ typedef enum {
|
|||
DEBUG_FF_LIMIT,
|
||||
DEBUG_FF_INTERPOLATED,
|
||||
DEBUG_BLACKBOX_OUTPUT,
|
||||
DEBUG_GYRO_SAMPLE,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@ bool cliMode = false;
|
|||
#include "common/typeconversion.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
|
|
@ -94,7 +95,6 @@ bool cliMode = false;
|
|||
#include "drivers/vtx_table.h"
|
||||
|
||||
#include "fc/board_info.h"
|
||||
#include "config/config.h"
|
||||
#include "fc/controlrate_profile.h"
|
||||
#include "fc/core.h"
|
||||
#include "fc/rc.h"
|
||||
|
|
@ -4680,11 +4680,11 @@ static void cliStatus(char *cmdline)
|
|||
|
||||
// Run status
|
||||
|
||||
const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID)));
|
||||
const int gyroRate = getTaskDeltaTime(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYRO)));
|
||||
const int rxRate = currentRxRefreshRate == 0 ? 0 : (int)(1000000.0f / ((float)currentRxRefreshRate));
|
||||
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
|
||||
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
|
||||
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
|
||||
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate);
|
||||
|
||||
// Battery meter
|
||||
|
||||
|
|
@ -4731,21 +4731,8 @@ static void cliTasks(char *cmdline)
|
|||
cfTaskInfo_t taskInfo;
|
||||
getTaskInfo(taskId, &taskInfo);
|
||||
if (taskInfo.isEnabled) {
|
||||
int taskFrequency;
|
||||
int subTaskFrequency = 0;
|
||||
if (taskId == TASK_GYROPID) {
|
||||
subTaskFrequency = taskInfo.movingAverageCycleTime == 0.0f ? 0.0f : (int)(1000000.0f / (taskInfo.movingAverageCycleTime));
|
||||
taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
|
||||
if (pidConfig()->pid_process_denom > 1) {
|
||||
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
|
||||
} else {
|
||||
taskFrequency = subTaskFrequency;
|
||||
cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
|
||||
}
|
||||
} else {
|
||||
taskFrequency = taskInfo.averageDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.averageDeltaTime));
|
||||
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
|
||||
}
|
||||
int taskFrequency = taskInfo.averageDeltaTime == 0 ? 0 : lrintf(1e6f / taskInfo.averageDeltaTime);
|
||||
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
|
||||
const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
|
||||
const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
|
||||
if (taskId != TASK_SERIAL) {
|
||||
|
|
@ -4759,9 +4746,6 @@ static void cliTasks(char *cmdline)
|
|||
} else {
|
||||
cliPrintLinef("%6d", taskFrequency);
|
||||
}
|
||||
if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) {
|
||||
cliPrintLinef(" - (%15s) %6d", taskInfo.subTaskName, subTaskFrequency);
|
||||
}
|
||||
|
||||
schedulerResetTaskMaxExecutionTime(taskId);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -258,7 +258,6 @@ static const char * const lookupTableRxSpi[] = {
|
|||
|
||||
static const char * const lookupTableGyroHardwareLpf[] = {
|
||||
"NORMAL",
|
||||
"1KHZ_SAMPLING",
|
||||
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
|
||||
"EXPERIMENTAL"
|
||||
#endif
|
||||
|
|
@ -621,7 +620,6 @@ const clivalue_t valueTable[] = {
|
|||
#if defined(USE_GYRO_SPI_ICM20649)
|
||||
{ "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
|
||||
#endif
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
|
||||
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
|
||||
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
|||
.debug_mode = DEBUG_MODE,
|
||||
.task_statistics = true,
|
||||
.rateProfile6PosSwitch = false,
|
||||
.cpu_overclock = 0,
|
||||
.cpu_overclock = DEFAULT_CPU_OVERCLOCK,
|
||||
.powerOnArmingGraceTime = 5,
|
||||
.boardIdentifier = TARGET_BOARD_IDENTIFIER,
|
||||
.hseMhz = SYSTEM_HSE_VALUE, // Not used for non-F4 targets
|
||||
|
|
@ -559,13 +559,6 @@ static void validateAndFixConfig(void)
|
|||
|
||||
void validateAndFixGyroConfig(void)
|
||||
{
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
// Disable dynamic filter if gyro loop is less than 2KHz
|
||||
if (gyro.targetLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
|
||||
featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Fix gyro filter settings to handle cases where an older configurator was used that
|
||||
// allowed higher cutoff limits from previous firmware versions.
|
||||
adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass_hz, FILTER_FREQUENCY_MAX);
|
||||
|
|
@ -589,15 +582,6 @@ void validateAndFixGyroConfig(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
||||
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
|
||||
gyroConfigMutable()->gyro_sync_denom = 1;
|
||||
}
|
||||
|
||||
#if defined(STM32F1)
|
||||
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
|
||||
#endif
|
||||
|
||||
float samplingTime;
|
||||
switch (gyroMpuDetectionResult()->sensor) {
|
||||
case ICM_20649_SPI:
|
||||
|
|
@ -610,16 +594,6 @@ void validateAndFixGyroConfig(void)
|
|||
samplingTime = 0.000125f;
|
||||
break;
|
||||
}
|
||||
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
||||
switch (gyroMpuDetectionResult()->sensor) {
|
||||
case ICM_20649_SPI:
|
||||
samplingTime = 1.0f / 1100.0f;
|
||||
break;
|
||||
default:
|
||||
samplingTime = 0.001f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// check for looptime restrictions based on motor protocol. Motor times have safety margin
|
||||
|
|
@ -654,13 +628,21 @@ void validateAndFixGyroConfig(void)
|
|||
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
|
||||
}
|
||||
} else {
|
||||
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
|
||||
const float pidLooptime = samplingTime * pidConfig()->pid_process_denom;
|
||||
if (pidLooptime < motorUpdateRestriction) {
|
||||
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
|
||||
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / samplingTime, 1, MAX_PID_PROCESS_DENOM);
|
||||
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
// Disable dynamic filter if gyro loop is less than 2KHz
|
||||
const uint32_t configuredLooptime = (gyro.sampleRateHz > 0) ? (pidConfig()->pid_process_denom * 1e6 / gyro.sampleRateHz) : 0;
|
||||
if (configuredLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
|
||||
featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
#ifndef USE_FLASHFS
|
||||
if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
|
||||
|
|
|
|||
|
|
@ -26,14 +26,12 @@
|
|||
cfTask_t *unittest_scheduler_selectedTask;
|
||||
uint8_t unittest_scheduler_selectedTaskDynamicPriority;
|
||||
uint16_t unittest_scheduler_waitingTasks;
|
||||
bool unittest_outsideRealtimeGuardInterval;
|
||||
|
||||
#define GET_SCHEDULER_LOCALS() \
|
||||
{ \
|
||||
unittest_scheduler_selectedTask = selectedTask; \
|
||||
unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \
|
||||
unittest_scheduler_waitingTasks = waitingTasks; \
|
||||
unittest_outsideRealtimeGuardInterval = outsideRealtimeGuardInterval; \
|
||||
}
|
||||
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -59,17 +59,11 @@ typedef enum {
|
|||
|
||||
typedef enum {
|
||||
GYRO_HARDWARE_LPF_NORMAL,
|
||||
GYRO_HARDWARE_LPF_1KHZ_SAMPLE,
|
||||
#ifdef USE_GYRO_DLPF_EXPERIMENTAL
|
||||
GYRO_HARDWARE_LPF_EXPERIMENTAL
|
||||
#endif
|
||||
} gyroHardwareLpf_e;
|
||||
|
||||
typedef enum {
|
||||
GYRO_32KHZ_HARDWARE_LPF_NORMAL,
|
||||
GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL
|
||||
} gyro32khzHardwareLpf;
|
||||
|
||||
typedef enum {
|
||||
GYRO_RATE_1_kHz,
|
||||
GYRO_RATE_1100_Hz,
|
||||
|
|
@ -106,6 +100,8 @@ typedef struct gyroDev_s {
|
|||
uint8_t gyroHasOverflowProtection;
|
||||
gyroHardware_e gyroHardware;
|
||||
fp_rotationMatrix_t rotationMatrix;
|
||||
uint16_t gyroSampleRateHz;
|
||||
uint16_t accSampleRateHz;
|
||||
} gyroDev_t;
|
||||
|
||||
typedef struct accDev_s {
|
||||
|
|
|
|||
|
|
@ -350,10 +350,6 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
|
|||
break;
|
||||
#endif
|
||||
|
||||
case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
|
||||
ret = 1;
|
||||
break;
|
||||
|
||||
case GYRO_HARDWARE_LPF_NORMAL:
|
||||
default:
|
||||
ret = 0;
|
||||
|
|
|
|||
|
|
@ -53,17 +53,6 @@
|
|||
#define MPU3050_USER_RESET 0x01
|
||||
#define MPU3050_CLK_SEL_PLL_GX 0x01
|
||||
|
||||
static uint8_t mpu3050GetDLPF(uint8_t lpf)
|
||||
{
|
||||
uint8_t ret;
|
||||
if (lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
||||
ret = MPU3050_DLPF_188HZ;
|
||||
} else {
|
||||
ret = MPU3050_DLPF_256HZ;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void mpu3050Init(gyroDev_t *gyro)
|
||||
{
|
||||
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
||||
|
|
@ -73,7 +62,7 @@ static void mpu3050Init(gyroDev_t *gyro)
|
|||
failureMode(FAILURE_ACC_INIT);
|
||||
}
|
||||
|
||||
busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpu3050GetDLPF(gyro->hardware_lpf));
|
||||
busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | MPU3050_DLPF_256HZ);
|
||||
busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0);
|
||||
busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
|
|
|
|||
|
|
@ -47,41 +47,30 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
|
|||
return ret;
|
||||
}
|
||||
|
||||
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator)
|
||||
uint16_t gyroSetSampleRate(gyroDev_t *gyro)
|
||||
{
|
||||
float gyroSamplePeriod;
|
||||
uint16_t gyroSampleRateHz;
|
||||
uint16_t accSampleRateHz;
|
||||
|
||||
if (lpf != GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
||||
switch (gyro->mpuDetectionResult.sensor) {
|
||||
case BMI_160_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
|
||||
gyroSamplePeriod = 312.0f;
|
||||
break;
|
||||
case ICM_20649_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_9_kHz;
|
||||
gyroSamplePeriod = 1000000.0f / 9000.0f;
|
||||
break;
|
||||
default:
|
||||
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
|
||||
gyroSamplePeriod = 125.0f;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
switch (gyro->mpuDetectionResult.sensor) {
|
||||
switch (gyro->mpuDetectionResult.sensor) {
|
||||
case BMI_160_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
|
||||
gyroSampleRateHz = 3200;
|
||||
accSampleRateHz = 800;
|
||||
break;
|
||||
case ICM_20649_SPI:
|
||||
gyro->gyroRateKHz = GYRO_RATE_1100_Hz;
|
||||
gyroSamplePeriod = 1000000.0f / 1100.0f;
|
||||
gyro->gyroRateKHz = GYRO_RATE_9_kHz;
|
||||
gyroSampleRateHz = 9000;
|
||||
accSampleRateHz = 1125;
|
||||
break;
|
||||
default:
|
||||
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
|
||||
gyroSamplePeriod = 1000.0f;
|
||||
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
|
||||
gyroSampleRateHz = 8000;
|
||||
accSampleRateHz = 1000;
|
||||
break;
|
||||
}
|
||||
gyroSyncDenominator = 1; // Always full Sampling 1khz
|
||||
}
|
||||
|
||||
// calculate gyro divider and targetLooptime (expected cycleTime)
|
||||
gyro->mpuDividerDrops = gyroSyncDenominator - 1;
|
||||
const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
|
||||
return targetLooptime;
|
||||
gyro->mpuDividerDrops = 0; // we no longer use the gyro's sample divider
|
||||
gyro->accSampleRateHz = accSampleRateHz;
|
||||
return gyroSampleRateHz;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,4 +30,4 @@
|
|||
#include "drivers/accgyro/accgyro.h"
|
||||
|
||||
bool gyroSyncCheckUpdate(gyroDev_t *gyro);
|
||||
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator);
|
||||
uint16_t gyroSetSampleRate(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@
|
|||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/stats.h"
|
||||
#include "fc/tasks.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
|
|
@ -142,6 +143,8 @@ enum {
|
|||
int16_t magHold;
|
||||
#endif
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint8_t pidUpdateCounter;
|
||||
|
||||
static bool flipOverAfterCrashActive = false;
|
||||
|
||||
static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
||||
|
|
@ -1214,10 +1217,42 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
|
|||
processRcCommand();
|
||||
}
|
||||
|
||||
FAST_CODE void taskGyroSample(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
gyroUpdate();
|
||||
if (pidUpdateCounter % activePidLoopDenom == 0) {
|
||||
pidUpdateCounter = 0;
|
||||
}
|
||||
pidUpdateCounter++;
|
||||
}
|
||||
|
||||
FAST_CODE bool gyroFilterReady(void)
|
||||
{
|
||||
if (pidUpdateCounter % activePidLoopDenom == 0) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
FAST_CODE bool pidLoopReady(void)
|
||||
{
|
||||
if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
|
||||
{
|
||||
gyroFiltering(currentTimeUs);
|
||||
|
||||
}
|
||||
|
||||
// Function for loop trigger
|
||||
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||
{
|
||||
static uint32_t pidUpdateCounter = 0;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
|
||||
if (lockMainPID() != 0) return;
|
||||
|
|
@ -1228,15 +1263,12 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
// 1 - subTaskPidController()
|
||||
// 2 - subTaskMotorUpdate()
|
||||
// 3 - subTaskPidSubprocesses()
|
||||
gyroUpdate(currentTimeUs);
|
||||
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
|
||||
|
||||
if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
|
||||
subTaskRcCommand(currentTimeUs);
|
||||
subTaskPidController(currentTimeUs);
|
||||
subTaskMotorUpdate(currentTimeUs);
|
||||
subTaskPidSubprocesses(currentTimeUs);
|
||||
}
|
||||
subTaskRcCommand(currentTimeUs);
|
||||
subTaskPidController(currentTimeUs);
|
||||
subTaskMotorUpdate(currentTimeUs);
|
||||
subTaskPidSubprocesses(currentTimeUs);
|
||||
|
||||
if (debugMode == DEBUG_CYCLETIME) {
|
||||
debug[0] = getTaskDeltaTime(TASK_SELF);
|
||||
|
|
|
|||
|
|
@ -80,6 +80,10 @@ void tryArm(void);
|
|||
bool processRx(timeUs_t currentTimeUs);
|
||||
void updateArmingStatus(void);
|
||||
|
||||
void taskGyroSample(timeUs_t currentTimeUs);
|
||||
bool gyroFilterReady(void);
|
||||
bool pidLoopReady(void);
|
||||
void taskFiltering(timeUs_t currentTimeUs);
|
||||
void taskMainPidLoop(timeUs_t currentTimeUs);
|
||||
|
||||
bool isFlipOverAfterCrashActive(void);
|
||||
|
|
|
|||
|
|
@ -724,13 +724,19 @@ void init(void)
|
|||
|
||||
systemState |= SYSTEM_STATE_SENSORS_READY;
|
||||
|
||||
// gyro.targetLooptime set in sensorsAutodetect(),
|
||||
// so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
|
||||
// Set the targetLooptime based on the detected gyro sampleRateHz and pid_process_denom
|
||||
gyroSetTargetLooptime(pidConfig()->pid_process_denom);
|
||||
|
||||
// Validate and correct the gyro config or PID loop time if needed
|
||||
validateAndFixGyroConfig();
|
||||
|
||||
// Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
|
||||
gyroSetTargetLooptime(pidConfig()->pid_process_denom);
|
||||
|
||||
// Finally initialize the gyro filtering
|
||||
gyroInitFilters();
|
||||
|
||||
pidInit(currentPidProfile);
|
||||
#ifdef USE_ACC
|
||||
accInitFilters();
|
||||
#endif
|
||||
|
||||
#ifdef USE_PID_AUDIO
|
||||
pidAudioInit();
|
||||
|
|
|
|||
|
|
@ -249,14 +249,19 @@ void tasksInit(void)
|
|||
#endif
|
||||
|
||||
if (sensors(SENSOR_GYRO)) {
|
||||
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
rescheduleTask(TASK_GYRO, gyro.sampleLooptime);
|
||||
rescheduleTask(TASK_FILTER, gyro.targetLooptime);
|
||||
rescheduleTask(TASK_PID, gyro.targetLooptime);
|
||||
setTaskEnabled(TASK_GYRO, true);
|
||||
setTaskEnabled(TASK_FILTER, true);
|
||||
setTaskEnabled(TASK_PID, true);
|
||||
schedulerEnableGyro();
|
||||
}
|
||||
|
||||
#if defined(USE_ACC)
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
if (sensors(SENSOR_ACC) && acc.sampleRateHz) {
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
|
||||
rescheduleTask(TASK_ACCEL, TASK_PERIOD_HZ(acc.sampleRateHz));
|
||||
setTaskEnabled(TASK_ATTITUDE, true);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -394,7 +399,9 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
|
||||
#endif
|
||||
|
||||
[TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
|
||||
[TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
|
||||
[TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
|
||||
[TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
|
||||
#ifdef USE_ACC
|
||||
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
|
||||
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
|
||||
|
|
|
|||
|
|
@ -20,6 +20,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
|
||||
|
||||
void tasksInit(void);
|
||||
|
|
|
|||
|
|
@ -755,7 +755,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
|
||||
void pidInit(const pidProfile_t *pidProfile)
|
||||
{
|
||||
pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
|
||||
pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
|
||||
pidInitFilters(pidProfile);
|
||||
pidInitConfig(pidProfile);
|
||||
#ifdef USE_RPM_FILTER
|
||||
|
|
@ -857,7 +857,10 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
|
|||
return constrainf(horizonLevelStrength, 0, 1);
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
|
||||
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
|
||||
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
|
||||
// processing power that it should be a non-issue.
|
||||
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
|
||||
// calculate error angle and limit the angle to the max inclination
|
||||
// rcDeflection is in range [-1.0, 1.0]
|
||||
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
|
||||
|
|
@ -1215,7 +1218,10 @@ float pidGetAirmodeThrottleOffset()
|
|||
#define LAUNCH_CONTROL_MIN_RATE 5.0f
|
||||
#define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
|
||||
|
||||
static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
|
||||
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
|
||||
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
|
||||
// processing power that it should be a non-issue.
|
||||
static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
|
||||
|
|
|
|||
|
|
@ -124,7 +124,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
|
|||
return;
|
||||
}
|
||||
|
||||
pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
|
||||
pidLooptime = gyro.targetLooptime;
|
||||
if (config->gyro_rpm_notch_harmonics) {
|
||||
gyroFilter = &filters[numberRpmNotchFilters++];
|
||||
rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
|
||||
|
|
|
|||
|
|
@ -655,6 +655,9 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
|
|||
// Added in API version 1.42
|
||||
sbufWriteU8(dst, systemConfig()->configurationState);
|
||||
|
||||
//Added in API version 1.43
|
||||
sbufWriteU16(dst, gyro.sampleRateHz); // informational so the configurator can display the correct gyro/pid frequencies in the drop-down
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -971,7 +974,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
boxBitmask_t flightModeFlags;
|
||||
const int flagBits = packFlightModeFlags(&flightModeFlags);
|
||||
|
||||
sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
|
||||
sbufWriteU16(dst, getTaskDeltaTime(TASK_PID));
|
||||
#ifdef USE_I2C
|
||||
sbufWriteU16(dst, i2cGetErrorCounter());
|
||||
#else
|
||||
|
|
@ -1634,7 +1637,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
break;
|
||||
}
|
||||
case MSP_ADVANCED_CONFIG:
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
|
||||
sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43
|
||||
sbufWriteU8(dst, pidConfig()->pid_process_denom);
|
||||
sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
|
||||
sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
|
||||
|
|
@ -2424,7 +2427,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
}
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG:
|
||||
gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
|
||||
sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
|
||||
pidConfigMutable()->pid_process_denom = sbufReadU8(src);
|
||||
motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
|
||||
#ifdef USE_DSHOT
|
||||
|
|
|
|||
|
|
@ -39,6 +39,11 @@
|
|||
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "fc/core.h"
|
||||
|
||||
#define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined
|
||||
#define TASK_AVERAGE_EXECUTE_PADDING_US 5 // Add a little padding to the average execution time
|
||||
|
||||
// DEBUG_SCHEDULER, timings for:
|
||||
// 0 - gyroUpdate()
|
||||
// 1 - pidController()
|
||||
|
|
@ -57,6 +62,7 @@ static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
|
|||
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
|
||||
|
||||
static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt);
|
||||
static FAST_RAM_ZERO_INIT bool gyroEnabled;
|
||||
|
||||
// No need for a linked list for the queue, since items are only inserted at startup
|
||||
|
||||
|
|
@ -140,7 +146,6 @@ void taskSystemLoad(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
#define MOVING_SUM_COUNT 32
|
||||
timeUs_t checkFuncMaxExecutionTime;
|
||||
timeUs_t checkFuncTotalExecutionTime;
|
||||
timeUs_t checkFuncMovingSumExecutionTime;
|
||||
|
|
@ -150,8 +155,8 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
|
|||
{
|
||||
checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
|
||||
checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
|
||||
checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
|
||||
checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
|
||||
checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -165,8 +170,8 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
|
|||
taskInfo->subTaskName = cfTasks[taskId].subTaskName;
|
||||
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
|
||||
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
|
||||
taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT;
|
||||
taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / MOVING_SUM_COUNT;
|
||||
taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
|
||||
taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime;
|
||||
#endif
|
||||
|
|
@ -271,89 +276,12 @@ inline static timeUs_t getPeriodCalculationBasis(const cfTask_t* task)
|
|||
}
|
||||
}
|
||||
|
||||
FAST_CODE void scheduler(void)
|
||||
FAST_CODE timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs)
|
||||
{
|
||||
// Cache currentTime
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
// Check for realtime tasks
|
||||
bool outsideRealtimeGuardInterval = true;
|
||||
for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
|
||||
const timeUs_t nextExecuteAt = getPeriodCalculationBasis(task) + task->desiredPeriod;
|
||||
if ((timeDelta_t)(currentTimeUs - nextExecuteAt) >= 0) {
|
||||
outsideRealtimeGuardInterval = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 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) {
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
const timeUs_t currentTimeBeforeCheckFuncCall = micros();
|
||||
#else
|
||||
const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
|
||||
#endif
|
||||
// Increase priority for event driven tasks
|
||||
if (task->dynamicPriority > 0) {
|
||||
task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
|
||||
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||
waitingTasks++;
|
||||
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
|
||||
#endif
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (calculateTaskStatistics) {
|
||||
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
|
||||
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
|
||||
checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
|
||||
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
|
||||
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
|
||||
}
|
||||
#endif
|
||||
task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
|
||||
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 = ((currentTimeUs - getPeriodCalculationBasis(task)) / 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;
|
||||
timeUs_t taskExecutionTime = 0;
|
||||
|
||||
if (selectedTask) {
|
||||
// Found a task that should be run
|
||||
currentTask = selectedTask;
|
||||
selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
float period = currentTimeUs - selectedTask->lastExecutedAt;
|
||||
|
|
@ -367,9 +295,9 @@ FAST_CODE void scheduler(void)
|
|||
if (calculateTaskStatistics) {
|
||||
const timeUs_t currentTimeBeforeTaskCall = micros();
|
||||
selectedTask->taskFunc(currentTimeBeforeTaskCall);
|
||||
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
|
||||
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
|
||||
selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / MOVING_SUM_COUNT;
|
||||
taskExecutionTime = micros() - currentTimeBeforeTaskCall;
|
||||
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
|
||||
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
|
||||
selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime);
|
||||
|
|
@ -378,13 +306,128 @@ FAST_CODE void scheduler(void)
|
|||
{
|
||||
selectedTask->taskFunc(currentTimeUs);
|
||||
}
|
||||
}
|
||||
|
||||
return taskExecutionTime;
|
||||
}
|
||||
|
||||
FAST_CODE void scheduler(void)
|
||||
{
|
||||
// Cache currentTime
|
||||
const timeUs_t schedulerStartTimeUs = micros();
|
||||
timeUs_t currentTimeUs = schedulerStartTimeUs;
|
||||
timeUs_t taskExecutionTime = 0;
|
||||
cfTask_t *selectedTask = NULL;
|
||||
uint16_t selectedTaskDynamicPriority = 0;
|
||||
uint16_t waitingTasks = 0;
|
||||
bool realtimeTaskRan = false;
|
||||
timeDelta_t gyroTaskDelayUs;
|
||||
|
||||
if (gyroEnabled) {
|
||||
// Realtime gyro/filtering/PID tasks get complete priority
|
||||
cfTask_t *gyroTask = &cfTasks[TASK_GYRO];
|
||||
const timeUs_t gyroExecuteTime = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriod;
|
||||
gyroTaskDelayUs = cmpTimeUs(gyroExecuteTime, currentTimeUs); // time until the next expected gyro sample
|
||||
if (cmpTimeUs(currentTimeUs, gyroExecuteTime) >= 0) {
|
||||
taskExecutionTime = schedulerExecuteTask(gyroTask, currentTimeUs);
|
||||
if (gyroFilterReady()) {
|
||||
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_FILTER], currentTimeUs);
|
||||
}
|
||||
if (pidLoopReady()) {
|
||||
taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_PID], currentTimeUs);
|
||||
}
|
||||
currentTimeUs = micros();
|
||||
realtimeTaskRan = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!gyroEnabled || realtimeTaskRan || (gyroTaskDelayUs > GYRO_TASK_GUARD_INTERVAL_US)) {
|
||||
// The task to be invoked
|
||||
|
||||
// Update task dynamic priorities
|
||||
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
|
||||
if (task->staticPriority != TASK_PRIORITY_REALTIME) {
|
||||
// Task has checkFunc - event driven
|
||||
if (task->checkFunc) {
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
const timeUs_t currentTimeBeforeCheckFuncCall = micros();
|
||||
#else
|
||||
const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
|
||||
#endif
|
||||
// Increase priority for event driven tasks
|
||||
if (task->dynamicPriority > 0) {
|
||||
task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
|
||||
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||
waitingTasks++;
|
||||
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
|
||||
#endif
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (calculateTaskStatistics) {
|
||||
const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
|
||||
checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
|
||||
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
|
||||
}
|
||||
#endif
|
||||
task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
|
||||
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 = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
|
||||
if (task->taskAgeCycles > 0) {
|
||||
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||
waitingTasks++;
|
||||
}
|
||||
}
|
||||
|
||||
if (task->dynamicPriority > selectedTaskDynamicPriority) {
|
||||
selectedTaskDynamicPriority = task->dynamicPriority;
|
||||
selectedTask = task;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
totalWaitingTasksSamples++;
|
||||
totalWaitingTasks += waitingTasks;
|
||||
|
||||
if (selectedTask) {
|
||||
timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US; // default average time if task statistics are not available
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (calculateTaskStatistics) {
|
||||
taskRequiredTimeUs = selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US;
|
||||
}
|
||||
#endif
|
||||
// Add in the time spent so far in check functions and the scheduler logic
|
||||
taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
|
||||
if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
|
||||
taskExecutionTime += schedulerExecuteTask(selectedTask, currentTimeUs);
|
||||
} else {
|
||||
selectedTask = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
|
||||
} else {
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTime); // time spent in scheduler
|
||||
#else
|
||||
UNUSED(taskExecutionTime);
|
||||
#endif
|
||||
}
|
||||
|
||||
GET_SCHEDULER_LOCALS();
|
||||
}
|
||||
|
||||
void schedulerEnableGyro(void)
|
||||
{
|
||||
gyroEnabled = true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,14 +27,19 @@
|
|||
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
|
||||
#define TASK_PERIOD_US(us) (us)
|
||||
|
||||
#define GYRO_TASK_GUARD_INTERVAL_US 10 // Don't run any other tasks if gyro task will be run soon
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
#define TASK_STATS_MOVING_SUM_COUNT 32
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle
|
||||
TASK_PRIORITY_REALTIME = -1, // Task will be run outside the scheduler logic
|
||||
TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle
|
||||
TASK_PRIORITY_LOW = 1,
|
||||
TASK_PRIORITY_MEDIUM = 3,
|
||||
TASK_PRIORITY_MEDIUM_HIGH = 4,
|
||||
TASK_PRIORITY_HIGH = 5,
|
||||
TASK_PRIORITY_REALTIME = 6,
|
||||
TASK_PRIORITY_MAX = 255
|
||||
} cfTaskPriority_e;
|
||||
|
||||
|
|
@ -49,7 +54,7 @@ typedef struct {
|
|||
const char * taskName;
|
||||
const char * subTaskName;
|
||||
bool isEnabled;
|
||||
uint8_t staticPriority;
|
||||
int8_t staticPriority;
|
||||
timeDelta_t desiredPeriod;
|
||||
timeDelta_t latestDeltaTime;
|
||||
timeUs_t maxExecutionTime;
|
||||
|
|
@ -63,7 +68,9 @@ typedef enum {
|
|||
/* Actual tasks */
|
||||
TASK_SYSTEM = 0,
|
||||
TASK_MAIN,
|
||||
TASK_GYROPID,
|
||||
TASK_GYRO,
|
||||
TASK_FILTER,
|
||||
TASK_PID,
|
||||
TASK_ACCEL,
|
||||
TASK_ATTITUDE,
|
||||
TASK_RX,
|
||||
|
|
@ -153,7 +160,7 @@ typedef struct {
|
|||
bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
|
||||
void (*taskFunc)(timeUs_t currentTimeUs);
|
||||
timeDelta_t desiredPeriod; // target period of execution
|
||||
const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero
|
||||
const int8_t staticPriority; // dynamicPriority grows in steps of this size
|
||||
|
||||
// Scheduling
|
||||
uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation
|
||||
|
|
@ -188,8 +195,10 @@ void schedulerResetCheckFunctionMaxExecutionTime(void);
|
|||
|
||||
void schedulerInit(void);
|
||||
void scheduler(void);
|
||||
timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs);
|
||||
void taskSystemLoad(timeUs_t currentTime);
|
||||
void schedulerOptimizeRate(bool optimizeRate);
|
||||
void schedulerEnableGyro(void);
|
||||
|
||||
#define LOAD_PERCENTAGE_ONE 100
|
||||
|
||||
|
|
|
|||
|
|
@ -323,7 +323,20 @@ retry:
|
|||
return true;
|
||||
}
|
||||
|
||||
bool accInit(uint32_t gyroSamplingInverval)
|
||||
void accInitFilters(void)
|
||||
{
|
||||
// Only set the lowpass cutoff if the ACC sample rate is detected otherwise
|
||||
// the filter initialization is not defined (sample rate = 0)
|
||||
accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
|
||||
if (accLpfCutHz) {
|
||||
const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSampleTimeUs);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool accInit(uint16_t accSampleRateHz)
|
||||
{
|
||||
memset(&acc, 0, sizeof(acc));
|
||||
// copy over the common gyro mpu settings
|
||||
|
|
@ -353,23 +366,9 @@ bool accInit(uint32_t gyroSamplingInverval)
|
|||
acc.dev.acc_1G = 256; // set default
|
||||
acc.dev.initFn(&acc.dev); // driver initialisation
|
||||
acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
|
||||
// set the acc sampling interval according to the gyro sampling interval
|
||||
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
|
||||
case 500:
|
||||
case 375:
|
||||
case 250:
|
||||
case 125:
|
||||
acc.accSamplingInterval = 1000;
|
||||
break;
|
||||
case 1000:
|
||||
default:
|
||||
acc.accSamplingInterval = 1000;
|
||||
}
|
||||
if (accLpfCutHz) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
|
||||
}
|
||||
}
|
||||
|
||||
acc.sampleRateHz = accSampleRateHz;
|
||||
accInitFilters();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -552,16 +551,6 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
|
|||
accelerationTrims = accelerationTrimsToUse;
|
||||
}
|
||||
|
||||
void accInitFilters(void)
|
||||
{
|
||||
accLpfCutHz = accelerometerConfig()->acc_lpf_hz;
|
||||
if (acc.accSamplingInterval) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
|
||||
{
|
||||
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ typedef enum {
|
|||
|
||||
typedef struct acc_s {
|
||||
accDev_t dev;
|
||||
uint32_t accSamplingInterval;
|
||||
uint16_t sampleRateHz;
|
||||
float accADC[XYZ_AXIS_COUNT];
|
||||
bool isAccelUpdatedAtLeastOnce;
|
||||
} acc_t;
|
||||
|
|
@ -78,7 +78,7 @@ typedef struct accelerometerConfig_s {
|
|||
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
|
||||
#endif
|
||||
|
||||
bool accInit(uint32_t gyroTargetLooptime);
|
||||
bool accInit(uint16_t accSampleRateHz);
|
||||
bool accIsCalibrationComplete(void);
|
||||
bool accHasBeenCalibrated(void);
|
||||
void accStartCalibration(void);
|
||||
|
|
|
|||
|
|
@ -114,6 +114,7 @@ static bool gyroHasOverflowProtection = true;
|
|||
|
||||
static FAST_RAM_ZERO_INIT bool useDualGyroDebugging;
|
||||
static FAST_RAM_ZERO_INIT flight_dynamics_index_t gyroDebugAxis;
|
||||
FAST_RAM uint8_t activePidLoopDenom = 1;
|
||||
|
||||
typedef struct gyroCalibration_s {
|
||||
float sum[XYZ_AXIS_COUNT];
|
||||
|
|
@ -141,19 +142,10 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
|
|||
#endif
|
||||
|
||||
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
|
||||
static void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz);
|
||||
static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime);
|
||||
|
||||
#define DEBUG_GYRO_CALIBRATION 3
|
||||
|
||||
#ifdef STM32F10X
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 8
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
|
||||
|| defined(USE_GYRO_SPI_ICM20689)
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 1
|
||||
#else
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 3
|
||||
#endif
|
||||
|
||||
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
|
||||
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
|
||||
|
||||
|
|
@ -172,7 +164,6 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
{
|
||||
gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds
|
||||
gyroConfig->gyroMovementCalibrationThreshold = 48;
|
||||
gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
|
||||
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
|
||||
gyroConfig->gyro_lowpass_type = FILTER_PT1;
|
||||
gyroConfig->gyro_lowpass_hz = 200; // NOTE: dynamic lpf is enabled by default so this setting is actually
|
||||
|
|
@ -421,8 +412,8 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
|
|||
buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
|
||||
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
|
||||
|
||||
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
|
||||
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
|
||||
// The targetLooptime gets set later based on the active sensor's gyroSampleRateHz and pid_process_denom
|
||||
gyroSensor->gyroDev.gyroSampleRateHz = gyroSetSampleRate(&gyroSensor->gyroDev);
|
||||
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
|
||||
gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
|
||||
|
||||
|
|
@ -490,6 +481,7 @@ bool gyroInit(void)
|
|||
case DEBUG_GYRO_SCALED:
|
||||
case DEBUG_GYRO_FILTERED:
|
||||
case DEBUG_DYN_LPF:
|
||||
case DEBUG_GYRO_SAMPLE:
|
||||
gyroDebugMode = debugMode;
|
||||
break;
|
||||
case DEBUG_DUAL_GYRO_DIFF:
|
||||
|
|
@ -567,7 +559,14 @@ bool gyroInit(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
gyroInitFilters();
|
||||
if (gyro.rawSensorDev) {
|
||||
gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
|
||||
gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
|
||||
} else {
|
||||
gyro.sampleRateHz = 0;
|
||||
gyro.accSampleRateHz = 0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -603,7 +602,7 @@ static void dynLpfFilterInit()
|
|||
}
|
||||
#endif
|
||||
|
||||
void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
|
||||
bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
|
||||
{
|
||||
filterApplyFnPtr *lowpassFilterApplyFn;
|
||||
gyroLowpassFilter_t *lowpassFilter = NULL;
|
||||
|
|
@ -620,12 +619,14 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
|
|||
break;
|
||||
|
||||
default:
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = false;
|
||||
|
||||
// Establish some common constants
|
||||
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
|
||||
const float gyroDt = gyro.targetLooptime * 1e-6f;
|
||||
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / looptime;
|
||||
const float gyroDt = looptime * 1e-6f;
|
||||
|
||||
// Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
|
||||
const float gain = pt1FilterGain(lpfHz, gyroDt);
|
||||
|
|
@ -642,6 +643,7 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
|
|||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
|
||||
}
|
||||
ret = true;
|
||||
break;
|
||||
case FILTER_BIQUAD:
|
||||
#ifdef USE_DYN_LPF
|
||||
|
|
@ -650,11 +652,13 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
|
|||
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
|
||||
#endif
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
|
||||
}
|
||||
ret = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
|
||||
|
|
@ -744,6 +748,18 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
|
|||
#endif
|
||||
}
|
||||
|
||||
void gyroSetTargetLooptime(uint8_t pidDenom)
|
||||
{
|
||||
activePidLoopDenom = pidDenom;
|
||||
if (gyro.sampleRateHz) {
|
||||
gyro.sampleLooptime = 1e6 / gyro.sampleRateHz;
|
||||
gyro.targetLooptime = activePidLoopDenom * 1e6 / gyro.sampleRateHz;
|
||||
} else {
|
||||
gyro.sampleLooptime = 0;
|
||||
gyro.targetLooptime = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void gyroInitFilters(void)
|
||||
{
|
||||
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
|
||||
|
|
@ -757,13 +773,15 @@ void gyroInitFilters(void)
|
|||
gyroInitLowpassFilterLpf(
|
||||
FILTER_LOWPASS,
|
||||
gyroConfig()->gyro_lowpass_type,
|
||||
gyro_lowpass_hz
|
||||
gyro_lowpass_hz,
|
||||
gyro.targetLooptime
|
||||
);
|
||||
|
||||
gyroInitLowpassFilterLpf(
|
||||
gyro.downsampleFilterEnabled = gyroInitLowpassFilterLpf(
|
||||
FILTER_LOWPASS2,
|
||||
gyroConfig()->gyro_lowpass2_type,
|
||||
gyroConfig()->gyro_lowpass2_hz
|
||||
gyroConfig()->gyro_lowpass2_hz,
|
||||
gyro.sampleLooptime
|
||||
);
|
||||
|
||||
gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
|
|
@ -809,7 +827,7 @@ static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati
|
|||
|
||||
static int32_t gyroCalculateCalibratingCycles(void)
|
||||
{
|
||||
return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.targetLooptime;
|
||||
return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.sampleLooptime;
|
||||
}
|
||||
|
||||
static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
|
||||
|
|
@ -1046,21 +1064,8 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
|
|||
}
|
||||
}
|
||||
|
||||
#define GYRO_FILTER_FUNCTION_NAME filterGyro
|
||||
#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
|
||||
#include "gyro_filter_impl.c"
|
||||
#undef GYRO_FILTER_FUNCTION_NAME
|
||||
#undef GYRO_FILTER_DEBUG_SET
|
||||
|
||||
#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
|
||||
#define GYRO_FILTER_DEBUG_SET DEBUG_SET
|
||||
#include "gyro_filter_impl.c"
|
||||
#undef GYRO_FILTER_FUNCTION_NAME
|
||||
#undef GYRO_FILTER_DEBUG_SET
|
||||
|
||||
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
||||
FAST_CODE void gyroUpdate(void)
|
||||
{
|
||||
|
||||
switch (gyroToUse) {
|
||||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
gyroUpdateSensor(&gyroSensor1);
|
||||
|
|
@ -1091,6 +1096,38 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
}
|
||||
|
||||
if (gyro.downsampleFilterEnabled) {
|
||||
// using gyro lowpass 2 filter for downsampling
|
||||
gyro.sampleSum[X] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[X], gyro.gyroADC[X]);
|
||||
gyro.sampleSum[Y] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Y], gyro.gyroADC[Y]);
|
||||
gyro.sampleSum[Z] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Z], gyro.gyroADC[Z]);
|
||||
} else {
|
||||
// using simple averaging for downsampling
|
||||
gyro.sampleSum[X] += gyro.gyroADC[X];
|
||||
gyro.sampleSum[Y] += gyro.gyroADC[Y];
|
||||
gyro.sampleSum[Z] += gyro.gyroADC[Z];
|
||||
gyro.sampleCount++;
|
||||
}
|
||||
}
|
||||
|
||||
#define GYRO_FILTER_FUNCTION_NAME filterGyro
|
||||
#define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
|
||||
#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do { UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
|
||||
#include "gyro_filter_impl.c"
|
||||
#undef GYRO_FILTER_FUNCTION_NAME
|
||||
#undef GYRO_FILTER_DEBUG_SET
|
||||
#undef GYRO_FILTER_AXIS_DEBUG_SET
|
||||
|
||||
#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
|
||||
#define GYRO_FILTER_DEBUG_SET DEBUG_SET
|
||||
#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyroDebugAxis) DEBUG_SET(mode, index, value)
|
||||
#include "gyro_filter_impl.c"
|
||||
#undef GYRO_FILTER_FUNCTION_NAME
|
||||
#undef GYRO_FILTER_DEBUG_SET
|
||||
#undef GYRO_FILTER_AXIS_DEBUG_SET
|
||||
|
||||
FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
|
||||
{
|
||||
if (gyroDebugMode == DEBUG_NONE) {
|
||||
filterGyro();
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@
|
|||
#include "flight/gyroanalyse.h"
|
||||
#endif
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
|
||||
|
|
@ -42,10 +44,15 @@ typedef union gyroLowpassFilter_u {
|
|||
} gyroLowpassFilter_t;
|
||||
|
||||
typedef struct gyro_s {
|
||||
uint16_t sampleRateHz;
|
||||
uint32_t targetLooptime;
|
||||
uint32_t sampleLooptime;
|
||||
float scale;
|
||||
float gyroADC[XYZ_AXIS_COUNT]; // aligned, calibrated, scaled, but unfiltered data from the sensor(s)
|
||||
float gyroADCf[XYZ_AXIS_COUNT]; // filtered gyro data
|
||||
uint8_t sampleCount; // gyro sensor sample counter
|
||||
float sampleSum[XYZ_AXIS_COUNT]; // summed samples used for downsampling
|
||||
bool downsampleFilterEnabled; // if true then downsample using gyro lowpass 2, otherwise use averaging
|
||||
|
||||
gyroDev_t *rawSensorDev; // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
|
||||
|
||||
|
|
@ -72,9 +79,12 @@ typedef struct gyro_s {
|
|||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroAnalyseState_t gyroAnalyseState;
|
||||
#endif
|
||||
|
||||
uint16_t accSampleRateHz;
|
||||
} gyro_t;
|
||||
|
||||
extern gyro_t gyro;
|
||||
extern uint8_t activePidLoopDenom;
|
||||
|
||||
enum {
|
||||
GYRO_OVERFLOW_CHECK_NONE = 0,
|
||||
|
|
@ -109,7 +119,6 @@ typedef enum gyroDetectionFlags_e {
|
|||
|
||||
typedef struct gyroConfig_s {
|
||||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||
uint8_t gyro_sync_denom; // Gyro sample divider
|
||||
uint8_t gyro_hardware_lpf; // gyro DLPF setting
|
||||
|
||||
uint8_t gyro_high_fsr;
|
||||
|
|
@ -149,9 +158,10 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
|
|||
|
||||
void gyroPreInit(void);
|
||||
bool gyroInit(void);
|
||||
|
||||
void gyroSetTargetLooptime(uint8_t pidDenom);
|
||||
void gyroInitFilters(void);
|
||||
void gyroUpdate(timeUs_t currentTimeUs);
|
||||
void gyroUpdate(void);
|
||||
void gyroFiltering(timeUs_t currentTimeUs);
|
||||
bool gyroGetAccumulationAverage(float *accumulation);
|
||||
const busDevice_t *gyroSensorBus(void);
|
||||
struct mpuDetectionResult_s;
|
||||
|
|
|
|||
|
|
@ -23,11 +23,31 @@
|
|||
static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
||||
{
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// DEBUG_GYRO_RAW records the raw value read from the sensor (not zero offset, not scaled)
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyro.rawSensorDev->gyroADCRaw[axis]);
|
||||
// scale gyro output to degrees per second
|
||||
float gyroADCf = gyro.gyroADC[axis];
|
||||
|
||||
// DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
|
||||
// If downsampling than the last value in the sample group will be output
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyro.gyroADC[axis]));
|
||||
|
||||
// DEBUG_GYRO_SAMPLE(0) Record the pre-downsample value for the selected debug axis (same as DEBUG_GYRO_SCALED)
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 0, lrintf(gyro.gyroADC[axis]));
|
||||
|
||||
// downsample the individual gyro samples
|
||||
float gyroADCf = 0;
|
||||
if (gyro.downsampleFilterEnabled) {
|
||||
// using gyro lowpass 2 filter for downsampling
|
||||
gyroADCf = gyro.sampleSum[axis];
|
||||
} else {
|
||||
// using simple average for downsampling
|
||||
if (gyro.sampleCount) {
|
||||
gyroADCf = gyro.sampleSum[axis] / gyro.sampleCount;
|
||||
}
|
||||
gyro.sampleSum[axis] = 0;
|
||||
}
|
||||
|
||||
// DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
|
|
@ -43,12 +63,16 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
gyroADCf = rpmFilterGyro(axis, gyroADCf);
|
||||
#endif
|
||||
|
||||
// DEBUG_GYRO_SAMPLE(2) Record the post-RPM Filter value for the selected debug axis
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 2, lrintf(gyroADCf));
|
||||
|
||||
// apply static notch filters and software lowpass filters
|
||||
gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf);
|
||||
gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf);
|
||||
gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf);
|
||||
gyroADCf = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[axis], gyroADCf);
|
||||
|
||||
// DEBUG_GYRO_SAMPLE(3) Record the post-static notch and lowpass filter value for the selected debug axis
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf));
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
|
|
@ -68,4 +92,5 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
gyro.sampleCount = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,6 +33,8 @@
|
|||
#include "config/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/adcinternal.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
|
@ -68,7 +70,7 @@ bool sensorsAutodetect(void)
|
|||
|
||||
#ifdef USE_ACC
|
||||
if (gyroDetected) {
|
||||
accInit(gyro.targetLooptime);
|
||||
accInit(gyro.accSampleRateHz);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -90,8 +90,7 @@ void targetConfiguration(void)
|
|||
statusLedConfigMutable()->ioTags[2] = IO_TAG(LED2_A);
|
||||
#endif
|
||||
} else {
|
||||
gyroConfigMutable()->gyro_sync_denom = 2;
|
||||
pidConfigMutable()->pid_process_denom = 2;
|
||||
pidConfigMutable()->pid_process_denom = 4;
|
||||
}
|
||||
|
||||
if (!haveFrSkyRX) {
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ void targetConfiguration(void)
|
|||
{
|
||||
pinioBoxConfigMutable()->permanentId[0] = 40;
|
||||
motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
|
||||
gyroConfigMutable()->gyro_sync_denom = 1; // 8kHz gyro
|
||||
pidConfigMutable()->pid_process_denom = 1; // 8kHz PID
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -279,7 +279,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
}
|
||||
break;
|
||||
case BST_STATUS:
|
||||
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
||||
bstWrite16(getTaskDeltaTime(TASK_PID));
|
||||
#ifdef USE_I2C
|
||||
bstWrite16(i2cGetErrorCounter());
|
||||
#else
|
||||
|
|
@ -314,7 +314,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(getCurrentPidProfileIndex());
|
||||
break;
|
||||
case BST_LOOP_TIME:
|
||||
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
||||
bstWrite16(getTaskDeltaTime(TASK_PID));
|
||||
break;
|
||||
case BST_RC_TUNING:
|
||||
bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]);
|
||||
|
|
@ -420,14 +420,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(rcControlsConfig()->yaw_deadband);
|
||||
break;
|
||||
case BST_FC_FILTERS:
|
||||
switch (gyroConfig()->gyro_hardware_lpf) { // Extra safety to prevent OSD setting corrupt values
|
||||
case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
|
||||
bstWrite16(1);
|
||||
break;
|
||||
default:
|
||||
bstWrite16(0);
|
||||
break;
|
||||
}
|
||||
bstWrite16(0);
|
||||
break;
|
||||
default:
|
||||
// we do not know how to handle the (valid) message, indicate error BST
|
||||
|
|
@ -628,14 +621,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
rcControlsConfigMutable()->yaw_deadband = bstRead8();
|
||||
break;
|
||||
case BST_SET_FC_FILTERS:
|
||||
switch (bstRead16()) {
|
||||
case 1:
|
||||
gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
|
||||
break;
|
||||
default:
|
||||
gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
|
||||
break;
|
||||
}
|
||||
bstRead16(); // 1KHz sampling no longer supported
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@
|
|||
// Motolab target supports 2 different type of boards Tornado / Cyclone.
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
gyroConfigMutable()->gyro_sync_denom = 4;
|
||||
pidConfigMutable()->pid_process_denom = 1;
|
||||
pidConfigMutable()->pid_process_denom = 4;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -81,8 +81,7 @@ void targetConfiguration(void)
|
|||
|
||||
motorConfigMutable()->dev.motorPwmRate = 17000;
|
||||
|
||||
gyroConfigMutable()->gyro_sync_denom = 4;
|
||||
pidConfigMutable()->pid_process_denom = 1;
|
||||
pidConfigMutable()->pid_process_denom = 4;
|
||||
|
||||
for (uint8_t pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
||||
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@ void targetConfiguration(void)
|
|||
|
||||
gyroDeviceConfigMutable()->extiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
|
||||
gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
|
||||
gyroConfigMutable()->gyro_soft_lpf_hz = 100;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
||||
|
|
|
|||
|
|
@ -75,7 +75,6 @@ void targetConfiguration(void)
|
|||
ledStripStatusModeConfigMutable()->ledConfigs[0] = DEFINE_LED(0, 0, 0, 0, LF(COLOR), LO(VTX), 0);
|
||||
targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction));
|
||||
motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
|
||||
gyroConfigMutable()->gyro_sync_denom = 1; // 8kHz gyro
|
||||
pidConfigMutable()->pid_process_denom = 1; // 8kHz PID
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -137,6 +137,14 @@
|
|||
#define DEFAULT_AUX_CHANNEL_COUNT 6
|
||||
#endif
|
||||
|
||||
// Set the default cpu_overclock to the first level (108MHz) for F411
|
||||
// Helps with looptime stability as the CPU is borderline when running native gyro sampling
|
||||
#if defined(USE_OVERCLOCK) && defined(STM32F411xE)
|
||||
#define DEFAULT_CPU_OVERCLOCK 1
|
||||
#else
|
||||
#define DEFAULT_CPU_OVERCLOCK 0
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef USE_ITCM_RAM
|
||||
#define FAST_CODE __attribute__((section(".tcm_code")))
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ extern "C" {
|
|||
int16_t GPS_directionToHome = 0;
|
||||
acc_t acc = {};
|
||||
bool mockIsUpright = false;
|
||||
uint8_t activePidLoopDenom = 1;
|
||||
}
|
||||
|
||||
uint32_t simulationFeatureFlags = 0;
|
||||
|
|
@ -1049,7 +1050,7 @@ extern "C" {
|
|||
void writeServos(void) {};
|
||||
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
|
||||
bool isMixerUsingServos(void) { return false; }
|
||||
void gyroUpdate(timeUs_t) {}
|
||||
void gyroUpdate(void) {}
|
||||
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
|
||||
void updateRSSI(timeUs_t) {}
|
||||
bool failsafeIsMonitoring(void) { return false; }
|
||||
|
|
@ -1099,4 +1100,5 @@ extern "C" {
|
|||
bool compassIsCalibrationComplete(void) { return true; }
|
||||
bool isUpright(void) { return mockIsUpright; }
|
||||
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
|
||||
void gyroFiltering(timeUs_t) {};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,14 +75,6 @@ TEST(BlackboxTest, TestInitIntervals)
|
|||
EXPECT_EQ(0, blackboxPInterval);
|
||||
EXPECT_EQ(4096, blackboxSInterval);
|
||||
|
||||
// 1kHz PIDloop
|
||||
gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1);
|
||||
targetPidLooptime = gyro.targetLooptime * 1;
|
||||
blackboxInit();
|
||||
EXPECT_EQ(32, blackboxIInterval);
|
||||
EXPECT_EQ(1, blackboxPInterval);
|
||||
EXPECT_EQ(8192, blackboxSInterval);
|
||||
|
||||
// 2kHz PIDloop
|
||||
targetPidLooptime = 500;
|
||||
blackboxInit();
|
||||
|
|
|
|||
|
|
@ -144,7 +144,7 @@ void setDefaultTestSettings(void) {
|
|||
pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
|
||||
pidProfile->launchControlGain = 40,
|
||||
|
||||
gyro.targetLooptime = 4000;
|
||||
gyro.targetLooptime = 8000;
|
||||
}
|
||||
|
||||
timeUs_t currentTestTime(void) {
|
||||
|
|
|
|||
|
|
@ -25,8 +25,12 @@ extern "C" {
|
|||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
const int TEST_PID_LOOP_TIME = 650;
|
||||
const int TEST_UPDATE_ACCEL_TIME = 192;
|
||||
const int TEST_GYRO_SAMPLE_HZ = 8000;
|
||||
const int TEST_GYRO_SAMPLE_TIME = 10;
|
||||
const int TEST_FILTERING_TIME = 40;
|
||||
const int TEST_PID_LOOP_TIME = 58;
|
||||
const int TEST_UPDATE_ACCEL_TIME = 32;
|
||||
const int TEST_UPDATE_ATTITUDE_TIME = 28;
|
||||
const int TEST_HANDLE_SERIAL_TIME = 30;
|
||||
const int TEST_UPDATE_BATTERY_TIME = 1;
|
||||
const int TEST_UPDATE_RX_CHECK_TIME = 34;
|
||||
|
|
@ -41,13 +45,23 @@ extern "C" {
|
|||
cfTask_t * unittest_scheduler_selectedTask;
|
||||
uint8_t unittest_scheduler_selectedTaskDynPrio;
|
||||
uint16_t unittest_scheduler_waitingTasks;
|
||||
timeDelta_t unittest_scheduler_taskRequiredTimeUs;
|
||||
bool taskGyroRan = false;
|
||||
bool taskFilterRan = false;
|
||||
bool taskPidRan = false;
|
||||
bool taskFilterReady = false;
|
||||
bool taskPidReady = false;
|
||||
|
||||
// set up micros() to simulate time
|
||||
uint32_t simulatedTime = 0;
|
||||
uint32_t micros(void) { return simulatedTime; }
|
||||
|
||||
// set up tasks to take a simulated representative time to execute
|
||||
void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; }
|
||||
bool gyroFilterReady(void) { return taskFilterReady; }
|
||||
bool pidLoopReady(void) { return taskPidReady; }
|
||||
void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
|
||||
void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
|
||||
void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }
|
||||
void taskUpdateAccelerometer(timeUs_t) { simulatedTime += TEST_UPDATE_ACCEL_TIME; }
|
||||
void taskHandleSerial(timeUs_t) { simulatedTime += TEST_HANDLE_SERIAL_TIME; }
|
||||
void taskUpdateBatteryVoltage(timeUs_t) { simulatedTime += TEST_UPDATE_BATTERY_TIME; }
|
||||
|
|
@ -56,6 +70,14 @@ extern "C" {
|
|||
void imuUpdateAttitude(timeUs_t) { simulatedTime += TEST_IMU_UPDATE_TIME; }
|
||||
void dispatchProcess(timeUs_t) { simulatedTime += TEST_DISPATCH_TIME; }
|
||||
|
||||
void resetGyroTaskTestFlags(void) {
|
||||
taskGyroRan = false;
|
||||
taskFilterRan = false;
|
||||
taskPidRan = false;
|
||||
taskFilterReady = false;
|
||||
taskPidReady = false;
|
||||
}
|
||||
|
||||
extern int taskQueueSize;
|
||||
extern cfTask_t* taskQueueArray[];
|
||||
|
||||
|
|
@ -73,17 +95,28 @@ extern "C" {
|
|||
.desiredPeriod = TASK_PERIOD_HZ(10),
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
|
||||
},
|
||||
[TASK_GYROPID] = {
|
||||
[TASK_GYRO] = {
|
||||
.taskName = "GYRO",
|
||||
.taskFunc = taskGyroSample,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
[TASK_FILTER] = {
|
||||
.taskName = "FILTER",
|
||||
.taskFunc = taskFiltering,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(4000),
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
[TASK_PID] = {
|
||||
.taskName = "PID",
|
||||
.subTaskName = "GYRO",
|
||||
.taskFunc = taskMainPidLoop,
|
||||
.desiredPeriod = 1000,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(4000),
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
[TASK_ACCEL] = {
|
||||
.taskName = "ACCEL",
|
||||
.taskFunc = taskUpdateAccelerometer,
|
||||
.desiredPeriod = 10000,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(1000),
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
[TASK_ATTITUDE] = {
|
||||
|
|
@ -123,7 +156,7 @@ extern "C" {
|
|||
TEST(SchedulerUnittest, TestPriorites)
|
||||
{
|
||||
EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
|
||||
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
|
||||
EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYRO].staticPriority);
|
||||
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
|
||||
EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority);
|
||||
EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY_VOLTAGE].staticPriority);
|
||||
|
|
@ -152,34 +185,24 @@ TEST(SchedulerUnittest, TestQueue)
|
|||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
|
||||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
|
||||
queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME
|
||||
EXPECT_EQ(2, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||
EXPECT_EQ(NULL, queueNext());
|
||||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
|
||||
queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
|
||||
EXPECT_EQ(3, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||
EXPECT_EQ(2, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||
EXPECT_EQ(NULL, queueNext());
|
||||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
|
||||
queueAdd(&cfTasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM
|
||||
EXPECT_EQ(4, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||
EXPECT_EQ(3, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
|
||||
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||
EXPECT_EQ(NULL, queueNext());
|
||||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
|
||||
queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH
|
||||
EXPECT_EQ(5, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
|
||||
EXPECT_EQ(4, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
|
||||
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
|
||||
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||
|
|
@ -187,9 +210,8 @@ TEST(SchedulerUnittest, TestQueue)
|
|||
EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
|
||||
|
||||
queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
|
||||
EXPECT_EQ(4, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
|
||||
EXPECT_EQ(3, taskQueueSize);
|
||||
EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
|
||||
EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
|
||||
EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
|
||||
EXPECT_EQ(NULL, queueNext());
|
||||
|
|
@ -320,47 +342,47 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue)
|
|||
TEST(SchedulerUnittest, TestSingleTask)
|
||||
{
|
||||
schedulerInit();
|
||||
// disable all tasks except TASK_GYROPID
|
||||
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = 1000;
|
||||
simulatedTime = 4000;
|
||||
// run the scheduler and check the task has executed
|
||||
scheduler();
|
||||
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime);
|
||||
EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt);
|
||||
EXPECT_EQ(TEST_PID_LOOP_TIME, cfTasks[TASK_GYROPID].totalExecutionTime);
|
||||
// task has run, so its dynamic priority should have been set to zero
|
||||
EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority);
|
||||
}
|
||||
|
||||
TEST(SchedulerUnittest, TestTwoTasks)
|
||||
{
|
||||
// disable all tasks except TASK_GYROPID and TASK_ACCEL
|
||||
// disable all tasks except TASK_ACCEL
|
||||
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = 1000;
|
||||
simulatedTime = 2050;
|
||||
// run the scheduler and check the task has executed
|
||||
scheduler();
|
||||
EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(1050, cfTasks[TASK_ACCEL].taskLatestDeltaTime);
|
||||
EXPECT_EQ(2050, cfTasks[TASK_ACCEL].lastExecutedAt);
|
||||
EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, cfTasks[TASK_ACCEL].totalExecutionTime);
|
||||
// task has run, so its dynamic priority should have been set to zero
|
||||
EXPECT_EQ(0, cfTasks[TASK_GYRO].dynamicPriority);
|
||||
}
|
||||
|
||||
// set it up so that TASK_ACCEL ran just before TASK_GYROPID
|
||||
TEST(SchedulerUnittest, TestTwoTasks)
|
||||
{
|
||||
// disable all tasks except TASK_ACCEL and TASK_ATTITUDE
|
||||
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
setTaskEnabled(TASK_ATTITUDE, true);
|
||||
|
||||
// set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
|
||||
static const uint32_t startTime = 4000;
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - TEST_UPDATE_ACCEL_TIME;
|
||||
EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles);
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime;
|
||||
cfTasks[TASK_ATTITUDE].lastExecutedAt = cfTasks[TASK_ACCEL].lastExecutedAt - TEST_UPDATE_ATTITUDE_TIME;
|
||||
EXPECT_EQ(0, cfTasks[TASK_ATTITUDE].taskAgeCycles);
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// no tasks should have run, since neither task's desired time has elapsed
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
|
||||
// NOTE:
|
||||
// TASK_GYROPID desiredPeriod is 1000 microseconds
|
||||
// TASK_ACCEL desiredPeriod is 10000 microseconds
|
||||
// TASK_ACCEL desiredPeriod is 1000 microseconds
|
||||
// TASK_ATTITUDE desiredPeriod is 10000 microseconds
|
||||
// 500 microseconds later
|
||||
simulatedTime += 500;
|
||||
// no tasks should run, since neither task's desired time has elapsed
|
||||
|
|
@ -368,28 +390,213 @@ TEST(SchedulerUnittest, TestTwoTasks)
|
|||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
|
||||
|
||||
// 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed
|
||||
// 500 microseconds later, TASK_ACCEL desiredPeriod has elapsed
|
||||
simulatedTime += 500;
|
||||
// TASK_GYROPID should now run
|
||||
// TASK_ACCEL should now run
|
||||
scheduler();
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(1, unittest_scheduler_waitingTasks);
|
||||
EXPECT_EQ(5000 + TEST_PID_LOOP_TIME, simulatedTime);
|
||||
EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
|
||||
|
||||
simulatedTime += 1000 - TEST_PID_LOOP_TIME;
|
||||
simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
|
||||
scheduler();
|
||||
// TASK_GYROPID should run again
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||
// TASK_ACCEL should run again
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
|
||||
scheduler();
|
||||
// No task should have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
|
||||
|
||||
simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed
|
||||
// of the two TASK_GYROPID should run first
|
||||
scheduler();
|
||||
EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
|
||||
// and finally TASK_ACCEL should now run
|
||||
simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriods have elapsed
|
||||
// of the two TASK_ACCEL should run first
|
||||
scheduler();
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
// and finally TASK_ATTITUDE should now run
|
||||
scheduler();
|
||||
EXPECT_EQ(&cfTasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
|
||||
}
|
||||
|
||||
TEST(SchedulerUnittest, TestGyroTask)
|
||||
{
|
||||
static const uint32_t startTime = 4000;
|
||||
|
||||
// enable the gyro
|
||||
schedulerEnableGyro();
|
||||
|
||||
// disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
|
||||
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYRO, true);
|
||||
setTaskEnabled(TASK_FILTER, true);
|
||||
setTaskEnabled(TASK_PID, true);
|
||||
|
||||
// First set it up so TASK_GYRO just ran
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// no tasks should have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
// also the gyro, filter and PID task indicators should be false
|
||||
EXPECT_FALSE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
|
||||
/* Test the gyro task running but not triggering the filtering or PID */
|
||||
// set the TASK_GYRO last executed time to be one period earlier
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
|
||||
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false
|
||||
EXPECT_TRUE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
// expect that no other tasks other than TASK_GYRO should have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
|
||||
/* Test the gyro task running and triggering the filtering task */
|
||||
// set the TASK_GYRO last executed time to be one period earlier
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
|
||||
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
taskFilterReady = true;
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro and filter task indicators should be true and TASK_PID indicator should be false
|
||||
EXPECT_TRUE(taskGyroRan);
|
||||
EXPECT_TRUE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
// expect that no other tasks other tasks should have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
|
||||
/* Test the gyro task running and triggering the PID task */
|
||||
// set the TASK_GYRO last executed time to be one period earlier
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
|
||||
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
taskPidReady = true;
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro and PID task indicators should be true and TASK_FILTER indicator should be false
|
||||
EXPECT_TRUE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_TRUE(taskPidRan);
|
||||
// expect that no other tasks other tasks should have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
}
|
||||
|
||||
// Test the scheduling logic that prevents other tasks from running if they
|
||||
// might interfere with the timing of the next gyro task.
|
||||
TEST(SchedulerUnittest, TestGyroLookahead)
|
||||
{
|
||||
static const uint32_t startTime = 4000;
|
||||
|
||||
// enable task statistics
|
||||
schedulerSetCalulateTaskStatistics(true);
|
||||
|
||||
// disable scheduler optimize rate
|
||||
schedulerOptimizeRate(false);
|
||||
|
||||
// enable the gyro
|
||||
schedulerEnableGyro();
|
||||
|
||||
// disable all tasks except TASK_GYRO, TASK_ACCEL
|
||||
for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
||||
setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYRO, true);
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
// set the average run time for TASK_ACCEL
|
||||
cfTasks[TASK_ACCEL].movingSumExecutionTime = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT;
|
||||
#endif
|
||||
|
||||
/* Test that another task will run if there's plenty of time till the next gyro sample time */
|
||||
// set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro, filter and PID task indicators should be false
|
||||
EXPECT_FALSE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
// TASK_ACCEL should have run
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
|
||||
/* Test that another task won't run if the time till the gyro task is less than the guard interval */
|
||||
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro, filter and PID task indicators should be false
|
||||
EXPECT_FALSE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
// TASK_ACCEL should not have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
|
||||
/* Test that another task won't run if the time till the gyro task is less than the average task interval */
|
||||
// set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// the gyro, filter and PID task indicators should be false
|
||||
EXPECT_FALSE(taskGyroRan);
|
||||
EXPECT_FALSE(taskFilterRan);
|
||||
EXPECT_FALSE(taskPidRan);
|
||||
// TASK_ACCEL should not have run
|
||||
EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
|
||||
|
||||
/* Test that another task will run if the gyro task gets executed */
|
||||
// set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
|
||||
simulatedTime = startTime;
|
||||
cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
|
||||
cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
|
||||
// reset the flags
|
||||
resetGyroTaskTestFlags();
|
||||
|
||||
// make the TASK_FILTER and TASK_PID ready to run
|
||||
taskFilterReady = true;
|
||||
taskPidReady = true;
|
||||
|
||||
// run the scheduler
|
||||
scheduler();
|
||||
// TASK_GYRO, TASK_FILTER, and TASK_PID should all run
|
||||
EXPECT_TRUE(taskGyroRan);
|
||||
EXPECT_TRUE(taskFilterRan);
|
||||
EXPECT_TRUE(taskPidRan);
|
||||
// TASK_ACCEL should have run
|
||||
EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -85,6 +85,7 @@ TEST(SensorGyro, Calibrate)
|
|||
{
|
||||
pgResetAll();
|
||||
gyroInit();
|
||||
gyroSetTargetLooptime(1);
|
||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
||||
const bool read = gyroDevPtr->readFn(gyroDevPtr);
|
||||
EXPECT_TRUE(read);
|
||||
|
|
@ -119,16 +120,16 @@ TEST(SensorGyro, Update)
|
|||
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
||||
gyroInit();
|
||||
gyroSetTargetLooptime(1);
|
||||
gyroDevPtr->readFn = fakeGyroRead;
|
||||
gyroStartCalibration(false);
|
||||
EXPECT_FALSE(gyroIsCalibrationComplete());
|
||||
|
||||
timeUs_t currentTimeUs = 0;
|
||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
||||
gyroUpdate(currentTimeUs);
|
||||
gyroUpdate();
|
||||
while (!gyroIsCalibrationComplete()) {
|
||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
||||
gyroUpdate(currentTimeUs);
|
||||
gyroUpdate();
|
||||
}
|
||||
EXPECT_TRUE(gyroIsCalibrationComplete());
|
||||
EXPECT_EQ(5, gyroDevPtr->gyroZero[X]);
|
||||
|
|
@ -137,16 +138,16 @@ TEST(SensorGyro, Update)
|
|||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||
gyroUpdate(currentTimeUs);
|
||||
gyroUpdate();
|
||||
// expect zero values since gyro is calibrated
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||
fakeGyroSet(gyroDevPtr, 15, 26, 97);
|
||||
gyroUpdate(currentTimeUs);
|
||||
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADCf[X], 1e-3); // gyroADCf values are scaled
|
||||
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADCf[Y], 1e-3);
|
||||
EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADCf[Z], 1e-3);
|
||||
gyroUpdate();
|
||||
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
|
||||
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
|
||||
EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADC[Z], 1e-3);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
|
|
|||
|
|
@ -113,6 +113,7 @@ TEST(VtxTest, PitMode)
|
|||
|
||||
// STUBS
|
||||
extern "C" {
|
||||
uint8_t activePidLoopDenom = 1;
|
||||
uint32_t micros(void) { return simulationTime; }
|
||||
uint32_t millis(void) { return micros() / 1000; }
|
||||
bool rxIsReceivingSignal(void) { return simulationHaveRx; }
|
||||
|
|
@ -141,7 +142,7 @@ extern "C" {
|
|||
void writeServos(void) {};
|
||||
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
|
||||
bool isMixerUsingServos(void) { return false; }
|
||||
void gyroUpdate(timeUs_t) {}
|
||||
void gyroUpdate() {}
|
||||
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
|
||||
void updateRSSI(timeUs_t) {}
|
||||
bool failsafeIsMonitoring(void) { return false; }
|
||||
|
|
@ -184,4 +185,5 @@ extern "C" {
|
|||
bool compassIsCalibrationComplete(void) { return true; }
|
||||
bool isUpright(void) { return true; }
|
||||
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
|
||||
void gyroFiltering(timeUs_t) {};
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue