Trigger gyro SPI DMA reads in EXTI handler if supported and lock gyroTask loop to gyro to eliminate missed updates and jitter
This commit is contained in:
parent
415d4db5aa
commit
d5f62be013
|
@ -99,4 +99,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"D_LPF",
|
||||
"VTX_TRAMP",
|
||||
"GHST",
|
||||
"SCHEDULER_DETERMINISM"
|
||||
};
|
||||
|
|
|
@ -115,6 +115,7 @@ typedef enum {
|
|||
DEBUG_D_LPF,
|
||||
DEBUG_VTX_TRAMP,
|
||||
DEBUG_GHST,
|
||||
DEBUG_SCHEDULER_DETERMINISM,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
@ -4782,6 +4782,19 @@ static void cliStatus(const char *cmdName, char *cmdline)
|
|||
cliPrintf(" gyro %d", pos + 1);
|
||||
}
|
||||
}
|
||||
#ifdef USE_SPI
|
||||
#ifdef USE_GYRO_EXTI
|
||||
if (gyroActiveDev()->gyroModeSPI != GYRO_EXTI_NO_INT) {
|
||||
cliPrintf(" locked");
|
||||
}
|
||||
if (gyroActiveDev()->gyroModeSPI == GYRO_EXTI_INT_DMA) {
|
||||
cliPrintf(" dma");
|
||||
}
|
||||
#endif
|
||||
if (spiGetExtDeviceCount(&gyroActiveDev()->dev) > 1) {
|
||||
cliPrintf(" shared");
|
||||
}
|
||||
#endif
|
||||
cliPrintLinefeed();
|
||||
|
||||
#if defined(USE_SENSOR_NAMES)
|
||||
|
@ -4867,7 +4880,6 @@ static void cliStatus(const char *cmdName, char *cmdline)
|
|||
cliPrintLinefeed();
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
static void cliTasks(const char *cmdName, char *cmdline)
|
||||
{
|
||||
UNUSED(cmdName);
|
||||
|
@ -4923,10 +4935,14 @@ static void cliTasks(const char *cmdName, char *cmdline)
|
|||
getCheckFuncInfo(&checkFuncInfo);
|
||||
cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTimeUs, checkFuncInfo.averageExecutionTimeUs, checkFuncInfo.totalExecutionTimeUs / 1000);
|
||||
cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
|
||||
if (debugMode == DEBUG_SCHEDULER_DETERMINISM) {
|
||||
extern int32_t schedLoopStartCycles, taskGuardCycles;
|
||||
|
||||
cliPrintLinef("Scheduler start cycles %d guard cycles %d", schedLoopStartCycles, taskGuardCycles);
|
||||
}
|
||||
schedulerResetCheckFunctionMaxExecutionTime();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static void printVersion(const char *cmdName, bool printBoardInfo)
|
||||
{
|
||||
|
@ -6584,9 +6600,7 @@ const clicmd_t cmdTable[] = {
|
|||
"\treverse <servo> <source> r|n", cliServoMix),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
|
||||
#endif
|
||||
#ifdef USE_TIMER_MGMT
|
||||
CLI_COMMAND_DEF("timer", "show/set timers", "<> | <pin> list | <pin> [af<alternate function>|none|<option(deprecated)>] | list | show", cliTimer),
|
||||
#endif
|
||||
|
@ -6832,8 +6846,6 @@ void cliEnter(serialPort_t *serialPort)
|
|||
cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
|
||||
cliErrorWriter = cliWriter;
|
||||
|
||||
schedulerSetCalulateTaskStatistics(systemConfig()->task_statistics);
|
||||
|
||||
#ifndef MINIMAL_CLI
|
||||
cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'");
|
||||
#else
|
||||
|
|
|
@ -694,7 +694,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "mag_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.roll) },
|
||||
{ "mag_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.pitch) },
|
||||
{ "mag_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.yaw) },
|
||||
{ "mag_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_bustype) },
|
||||
{ "mag_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_busType) },
|
||||
{ "mag_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_device) },
|
||||
{ "mag_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) },
|
||||
{ "mag_spi_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_device) },
|
||||
|
@ -1469,9 +1469,7 @@ const clivalue_t valueTable[] = {
|
|||
#if defined(STM32F4) || defined(STM32G4)
|
||||
{ "system_hse_mhz", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, hseMhz) },
|
||||
#endif
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
{ "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) },
|
||||
#endif
|
||||
{ "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) },
|
||||
{ "rate_6pos_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, rateProfile6PosSwitch) },
|
||||
#ifdef USE_OVERCLOCK
|
||||
|
|
|
@ -44,6 +44,7 @@ typedef uint32_t timeUs_t;
|
|||
#define TIMEZONE_OFFSET_MINUTES_MAX 780 // +13 hours
|
||||
|
||||
static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
|
||||
static inline int32_t cmpTimeCycles(uint32_t a, uint32_t b) { return (int32_t)(a - b); }
|
||||
|
||||
#define FORMATTED_DATE_TIME_BUFSIZE 30
|
||||
|
||||
|
|
|
@ -25,6 +25,7 @@
|
|||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/sensor_alignment.h"
|
||||
#include "common/time.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
@ -79,6 +80,13 @@ typedef enum {
|
|||
GYRO_RATE_32_kHz,
|
||||
} gyroRateKHz_e;
|
||||
|
||||
typedef enum {
|
||||
GYRO_EXTI_INIT = 0,
|
||||
GYRO_EXTI_INT_DMA,
|
||||
GYRO_EXTI_INT,
|
||||
GYRO_EXTI_NO_INT
|
||||
} gyroModeSPI_e;
|
||||
|
||||
typedef struct gyroDev_s {
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
pthread_mutex_t lock;
|
||||
|
@ -97,7 +105,15 @@ typedef struct gyroDev_s {
|
|||
mpuDetectionResult_t mpuDetectionResult;
|
||||
sensor_align_e gyroAlign;
|
||||
gyroRateKHz_e gyroRateKHz;
|
||||
bool dataReady;
|
||||
gyroModeSPI_e gyroModeSPI;
|
||||
#ifdef USE_GYRO_EXTI
|
||||
uint32_t detectedEXTI;
|
||||
uint32_t gyroLastEXTI;
|
||||
uint32_t gyroSyncEXTI;
|
||||
int32_t gyroShortPeriod;
|
||||
int32_t gyroDmaMaxDuration;
|
||||
#endif
|
||||
volatile bool dataReady;
|
||||
bool gyro_high_fsr;
|
||||
uint8_t hardware_lpf;
|
||||
uint8_t hardware_32khz_lpf;
|
||||
|
|
|
@ -67,6 +67,9 @@
|
|||
|
||||
#define MPU_INQUIRY_MASK 0x7E
|
||||
|
||||
// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
|
||||
#define GYRO_EXTI_DETECT_THRESHOLD 1000
|
||||
|
||||
#ifdef USE_I2C_GYRO
|
||||
static void mpu6050FindRevision(gyroDev_t *gyro)
|
||||
{
|
||||
|
@ -107,21 +110,60 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
|||
* Gyro interrupt service routine
|
||||
*/
|
||||
#ifdef USE_GYRO_EXTI
|
||||
#ifdef USE_SPI_GYRO
|
||||
// Called in ISR context
|
||||
// Gyro read has just completed
|
||||
busStatus_e mpuIntcallback(uint32_t arg)
|
||||
{
|
||||
volatile gyroDev_t *gyro = (gyroDev_t *)arg;
|
||||
int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
|
||||
|
||||
if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
|
||||
gyro->gyroDmaMaxDuration = gyroDmaDuration;
|
||||
}
|
||||
|
||||
gyro->dataReady = true;
|
||||
|
||||
return BUS_READY;
|
||||
}
|
||||
|
||||
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
// Non-blocking, so this needs to be static
|
||||
static busSegment_t segments[] = {
|
||||
{NULL, NULL, 15, true, mpuIntcallback},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
|
||||
// Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does
|
||||
// not have an associated timer
|
||||
uint32_t nowCycles = getCycleCounter();
|
||||
int32_t gyroLastPeriod = cmpTimeCycles(nowCycles, gyro->gyroLastEXTI);
|
||||
// This detects the short (~79us) EXTI interval of an MPU6xxx gyro
|
||||
if ((gyro->gyroShortPeriod == 0) || (gyroLastPeriod < gyro->gyroShortPeriod)) {
|
||||
gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
|
||||
}
|
||||
gyro->gyroLastEXTI = nowCycles;
|
||||
|
||||
if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
|
||||
segments[0].txData = gyro->dev.txBuf;;
|
||||
segments[0].rxData = &gyro->dev.rxBuf[1];
|
||||
|
||||
if (!spiIsBusy(&gyro->dev)) {
|
||||
spiSequence(&gyro->dev, &segments[0]);
|
||||
}
|
||||
}
|
||||
|
||||
gyro->detectedEXTI++;
|
||||
}
|
||||
#else
|
||||
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
static uint32_t lastCalledAtUs = 0;
|
||||
const uint32_t nowUs = micros();
|
||||
debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
|
||||
lastCalledAtUs = nowUs;
|
||||
#endif
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
const uint32_t now2Us = micros();
|
||||
debug[1] = (uint16_t)(now2Us - nowUs);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void mpuIntExtiInit(gyroDev_t *gyro)
|
||||
{
|
||||
|
@ -177,37 +219,128 @@ bool mpuGyroRead(gyroDev_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_SPI_GYRO
|
||||
bool mpuAccReadSPI(accDev_t *acc)
|
||||
{
|
||||
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
STATIC_DMA_DATA_AUTO uint8_t data[7];
|
||||
switch (acc->gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
// Ensure any prior DMA has completed before continuing
|
||||
spiWaitClaim(&acc->gyro->dev);
|
||||
|
||||
const bool ack = spiReadWriteBufRB(&acc->gyro->dev, dataToSend, data, 7);
|
||||
if (!ack) {
|
||||
return false;
|
||||
acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
|
||||
|
||||
busSegment_t segments[] = {
|
||||
{NULL, NULL, 7, true, NULL},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
segments[0].txData = acc->gyro->dev.txBuf;
|
||||
segments[0].rxData = &acc->gyro->dev.rxBuf[1];
|
||||
|
||||
spiSequence(&acc->gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&acc->gyro->dev);
|
||||
|
||||
// Fall through
|
||||
FALLTHROUGH;
|
||||
}
|
||||
|
||||
acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
|
||||
acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
|
||||
acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
|
||||
// This data was read from the gyro, which is the same SPI device as the acc
|
||||
uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
|
||||
acc->ADCRaw[X] = __builtin_bswap16(accData[1]);
|
||||
acc->ADCRaw[Y] = __builtin_bswap16(accData[2]);
|
||||
acc->ADCRaw[Z] = __builtin_bswap16(accData[3]);
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INIT:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool mpuGyroReadSPI(gyroDev_t *gyro)
|
||||
{
|
||||
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
STATIC_DMA_DATA_AUTO uint8_t data[7];
|
||||
uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
|
||||
switch (gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INIT:
|
||||
{
|
||||
// Initialise the tx buffer to all 0xff
|
||||
memset(gyro->dev.txBuf, 0xff, 16);
|
||||
#ifdef USE_GYRO_EXTI
|
||||
// Check that minimum number of interrupts have been detected
|
||||
|
||||
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
|
||||
if (!ack) {
|
||||
return false;
|
||||
// We need some offset from the gyro interrupts to ensure sampling after the interrupt
|
||||
gyro->gyroDmaMaxDuration = 5;
|
||||
// Using DMA for gyro access upsets the scheduler on the F4
|
||||
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
|
||||
if (spiUseDMA(&gyro->dev)) {
|
||||
// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
|
||||
spiSetAtomicWait(&gyro->dev);
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
|
||||
gyro->dev.callbackArg = (uint32_t)gyro;
|
||||
gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
|
||||
} else {
|
||||
// Interrupts are present, but no DMA
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT;
|
||||
}
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
|
||||
gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
|
||||
gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
// Ensure any prior DMA has completed before continuing
|
||||
spiWaitClaim(&gyro->dev);
|
||||
|
||||
gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80;
|
||||
|
||||
busSegment_t segments[] = {
|
||||
{NULL, NULL, 7, true, NULL},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
segments[0].txData = gyro->dev.txBuf;
|
||||
segments[0].rxData = &gyro->dev.rxBuf[1];
|
||||
|
||||
spiSequence(&gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&gyro->dev);
|
||||
|
||||
gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[1]);
|
||||
gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[2]);
|
||||
gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[3]);
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[5]);
|
||||
gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[6]);
|
||||
gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[7]);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
@ -24,8 +24,6 @@
|
|||
#include "drivers/exti.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \
|
||||
|| defined(USE_GYRO_SPI_ICM20689)
|
||||
#define GYRO_USES_SPI
|
||||
|
|
|
@ -41,8 +41,6 @@
|
|||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_mpu6050.h"
|
||||
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
// MPU6050, Standard address 0x68
|
||||
// MPU_INT on PB13 on rev4 Naze32 hardware
|
||||
#define MPU6050_ADDRESS 0x68
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
@ -33,6 +35,7 @@
|
|||
#include "drivers/io_impl.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
// 10 MHz max SPI frequency
|
||||
|
@ -123,6 +126,9 @@ typedef enum {
|
|||
BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB
|
||||
} bmi270ConfigValues_e;
|
||||
|
||||
// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
|
||||
#define GYRO_EXTI_DETECT_THRESHOLD 1000
|
||||
|
||||
// BMI270 register reads are 16bits with the first byte a "dummy" value 0
|
||||
// that must be ignored. The result is in the second byte.
|
||||
static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId)
|
||||
|
@ -246,11 +252,52 @@ static void bmi270Config(gyroDev_t *gyro)
|
|||
|
||||
extiCallbackRec_t bmi270IntCallbackRec;
|
||||
|
||||
#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
/*
|
||||
* Gyro interrupt service routine
|
||||
*/
|
||||
#ifdef USE_GYRO_EXTI
|
||||
// Called in ISR context
|
||||
// Gyro read has just completed
|
||||
busStatus_e bmi270Intcallback(uint32_t arg)
|
||||
{
|
||||
volatile gyroDev_t *gyro = (gyroDev_t *)arg;
|
||||
int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
|
||||
|
||||
if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
|
||||
gyro->gyroDmaMaxDuration = gyroDmaDuration;
|
||||
}
|
||||
|
||||
gyro->dataReady = true;
|
||||
|
||||
return BUS_READY;
|
||||
}
|
||||
|
||||
void bmi270ExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
// Non-blocking, so this needs to be static
|
||||
static busSegment_t segments[] = {
|
||||
{NULL, NULL, 14, true, bmi270Intcallback},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
// Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does
|
||||
// not have an associated timer
|
||||
uint32_t nowCycles = getCycleCounter();
|
||||
gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
|
||||
gyro->gyroLastEXTI = nowCycles;
|
||||
|
||||
if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
|
||||
segments[0].txData = gyro->dev.txBuf;
|
||||
segments[0].rxData = gyro->dev.rxBuf;
|
||||
|
||||
if (!spiIsBusy(&gyro->dev)) {
|
||||
spiSequence(&gyro->dev, &segments[0]);
|
||||
}
|
||||
}
|
||||
|
||||
gyro->detectedEXTI++;
|
||||
|
||||
}
|
||||
|
||||
static void bmi270IntExtiInit(gyroDev_t *gyro)
|
||||
|
@ -266,56 +313,135 @@ static void bmi270IntExtiInit(gyroDev_t *gyro)
|
|||
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
|
||||
EXTIEnable(mpuIntIO, true);
|
||||
}
|
||||
#else
|
||||
void bmi270ExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
static bool bmi270AccRead(accDev_t *acc)
|
||||
{
|
||||
enum {
|
||||
IDX_REG = 0,
|
||||
IDX_SKIP,
|
||||
IDX_ACCEL_XOUT_L,
|
||||
IDX_ACCEL_XOUT_H,
|
||||
IDX_ACCEL_YOUT_L,
|
||||
IDX_ACCEL_YOUT_H,
|
||||
IDX_ACCEL_ZOUT_L,
|
||||
IDX_ACCEL_ZOUT_H,
|
||||
BUFFER_SIZE,
|
||||
};
|
||||
switch (acc->gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
// Ensure any prior DMA has completed before continuing
|
||||
spiWaitClaim(&acc->gyro->dev);
|
||||
|
||||
STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
|
||||
STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
|
||||
acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
|
||||
|
||||
spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
|
||||
busSegment_t segments[] = {
|
||||
{NULL, NULL, 8, true, NULL},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
segments[0].txData = acc->gyro->dev.txBuf;
|
||||
segments[0].rxData = acc->gyro->dev.rxBuf;
|
||||
|
||||
acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]);
|
||||
acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]);
|
||||
acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]);
|
||||
spiSequence(&acc->gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&acc->gyro->dev);
|
||||
|
||||
// Fall through
|
||||
FALLTHROUGH;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
|
||||
// This data was read from the gyro, which is the same SPI device as the acc
|
||||
uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
|
||||
acc->ADCRaw[X] = accData[1];
|
||||
acc->ADCRaw[Y] = accData[2];
|
||||
acc->ADCRaw[Z] = accData[3];
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INIT:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool bmi270GyroReadRegister(gyroDev_t *gyro)
|
||||
{
|
||||
enum {
|
||||
IDX_REG = 0,
|
||||
IDX_SKIP,
|
||||
IDX_GYRO_XOUT_L,
|
||||
IDX_GYRO_XOUT_H,
|
||||
IDX_GYRO_YOUT_L,
|
||||
IDX_GYRO_YOUT_H,
|
||||
IDX_GYRO_ZOUT_L,
|
||||
IDX_GYRO_ZOUT_H,
|
||||
BUFFER_SIZE,
|
||||
};
|
||||
uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
|
||||
switch (gyro->gyroModeSPI) {
|
||||
case GYRO_EXTI_INIT:
|
||||
{
|
||||
// Initialise the tx buffer to all 0x00
|
||||
memset(gyro->dev.txBuf, 0x00, 14);
|
||||
#ifdef USE_GYRO_EXTI
|
||||
// Check that minimum number of interrupts have been detected
|
||||
|
||||
STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
|
||||
STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
|
||||
// We need some offset from the gyro interrupts to ensure sampling after the interrupt
|
||||
gyro->gyroDmaMaxDuration = 5;
|
||||
// Using DMA for gyro access upsets the scheduler on the F4
|
||||
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
|
||||
if (spiUseDMA(&gyro->dev)) {
|
||||
// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
|
||||
spiSetAtomicWait(&gyro->dev);
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
|
||||
gyro->dev.callbackArg = (uint32_t)gyro;
|
||||
gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
|
||||
} else {
|
||||
// Interrupts are present, but no DMA
|
||||
gyro->gyroModeSPI = GYRO_EXTI_INT;
|
||||
}
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
|
||||
case GYRO_EXTI_INT:
|
||||
case GYRO_EXTI_NO_INT:
|
||||
{
|
||||
// Ensure any prior DMA has completed before continuing
|
||||
spiWaitClaim(&gyro->dev);
|
||||
|
||||
gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]);
|
||||
gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]);
|
||||
gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]);
|
||||
gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80;
|
||||
|
||||
busSegment_t segments[] = {
|
||||
{NULL, NULL, 8, true, NULL},
|
||||
{NULL, NULL, 0, true, NULL},
|
||||
};
|
||||
segments[0].txData = gyro->dev.txBuf;
|
||||
segments[0].rxData = gyro->dev.rxBuf;
|
||||
|
||||
spiSequence(&gyro->dev, &segments[0]);
|
||||
|
||||
// Wait for completion
|
||||
spiWait(&gyro->dev);
|
||||
|
||||
gyro->gyroADCRaw[X] = gyroData[1];
|
||||
gyro->gyroADCRaw[Y] = gyroData[2];
|
||||
gyro->gyroADCRaw[Z] = gyroData[3];
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case GYRO_EXTI_INT_DMA:
|
||||
{
|
||||
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
|
||||
// up an old value.
|
||||
gyro->gyroADCRaw[X] = gyroData[4];
|
||||
gyro->gyroADCRaw[Y] = gyroData[5];
|
||||
gyro->gyroADCRaw[Z] = gyroData[6];
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
@ -401,7 +527,7 @@ static void bmi270SpiGyroInit(gyroDev_t *gyro)
|
|||
{
|
||||
bmi270Config(gyro);
|
||||
|
||||
#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
#if defined(USE_GYRO_EXTI)
|
||||
bmi270IntExtiInit(gyro);
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -52,6 +52,8 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
|
|||
// 20 MHz max SPI frequency
|
||||
#define MPU6000_MAX_SPI_CLK_HZ 20000000
|
||||
|
||||
#define MPU6000_SHORT_THRESHOLD 82 // Any interrupt interval less than this will be recognised as the short interval of ~79us
|
||||
|
||||
// Bits
|
||||
#define BIT_SLEEP 0x40
|
||||
#define BIT_H_RESET 0x80
|
||||
|
@ -235,7 +237,9 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
|
|||
gyro->initFn = mpu6000SpiGyroInit;
|
||||
gyro->readFn = mpuGyroReadSPI;
|
||||
gyro->scale = GYRO_SCALE_2000DPS;
|
||||
|
||||
#ifdef USE_GYRO_EXTI
|
||||
gyro->gyroShortPeriod = clockMicrosToCycles(MPU6000_SHORT_THRESHOLD);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -53,7 +53,7 @@ FAST_DATA_ZERO_INIT bool useDshotTelemetry = false;
|
|||
|
||||
FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
|
||||
|
||||
FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
|
||||
FAST_CODE_NOINLINE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 16; i++) {
|
||||
|
@ -66,7 +66,7 @@ FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t p
|
|||
return DSHOT_DMA_BUFFER_SIZE;
|
||||
}
|
||||
|
||||
FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
|
||||
FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 4; i++) {
|
||||
|
|
|
@ -44,6 +44,8 @@
|
|||
|
||||
#include "light_ws2811strip.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#ifdef USE_LEDSTRIP_CACHE_MGMT
|
||||
// WS2811_DMA_BUFFER_SIZE is multiples of uint32_t
|
||||
// Number of bytes required for buffer
|
||||
|
@ -179,6 +181,7 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat)
|
|||
{
|
||||
// don't wait - risk of infinite block, just get an update next time round
|
||||
if (!ws2811Initialised || ws2811LedDataTransferInProgress) {
|
||||
ignoreTaskStateTime();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
@ -70,7 +70,7 @@
|
|||
#define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0)
|
||||
#define NVIC_PRIO_USB_WUP NVIC_BUILD_PRIORITY(1, 0)
|
||||
#define NVIC_PRIO_SONAR_ECHO NVIC_BUILD_PRIORITY(0x0f, 0x0f)
|
||||
#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
|
||||
#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0, 1)
|
||||
#define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
|
||||
#define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
|
||||
#define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0)
|
||||
|
|
|
@ -68,7 +68,7 @@ void cycleCounterInit(void)
|
|||
|
||||
#if defined(DWT_LAR_UNLOCK_VALUE)
|
||||
#if defined(STM32F7) || defined(STM32H7)
|
||||
DWT->LAR = DWT_LAR_UNLOCK_VALUE;
|
||||
ITM->LAR = DWT_LAR_UNLOCK_VALUE;
|
||||
#elif defined(STM32F3) || defined(STM32F4)
|
||||
// Note: DWT_Type does not contain LAR member.
|
||||
#define DWT_LAR
|
||||
|
@ -146,7 +146,7 @@ uint32_t micros(void)
|
|||
return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
|
||||
}
|
||||
|
||||
inline uint32_t getCycleCounter(void)
|
||||
uint32_t getCycleCounter(void)
|
||||
{
|
||||
return DWT->CYCCNT;
|
||||
}
|
||||
|
@ -156,6 +156,17 @@ uint32_t clockCyclesToMicros(uint32_t clockCycles)
|
|||
return clockCycles / usTicks;
|
||||
}
|
||||
|
||||
// Note that this conversion is signed as this is used for periods rather than absolute timestamps
|
||||
int32_t clockCyclesTo10thMicros(int32_t clockCycles)
|
||||
{
|
||||
return 10 * clockCycles / (int32_t)usTicks;
|
||||
}
|
||||
|
||||
uint32_t clockMicrosToCycles(uint32_t micros)
|
||||
{
|
||||
return micros * usTicks;
|
||||
}
|
||||
|
||||
// Return system uptime in milliseconds (rollover in 49 days)
|
||||
uint32_t millis(void)
|
||||
{
|
||||
|
|
|
@ -66,6 +66,8 @@ void systemResetToBootloader(bootloaderRequestType_e requestType);
|
|||
bool isMPUSoftReset(void);
|
||||
void cycleCounterInit(void);
|
||||
uint32_t clockCyclesToMicros(uint32_t clockCycles);
|
||||
int32_t clockCyclesTo10thMicros(int32_t clockCycles);
|
||||
uint32_t clockMicrosToCycles(uint32_t micros);
|
||||
uint32_t getCycleCounter(void);
|
||||
#if defined(STM32H7) || defined(STM32G4)
|
||||
void systemCheckResetReason(void);
|
||||
|
|
|
@ -81,6 +81,8 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/adcinternal.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
@ -91,8 +93,6 @@
|
|||
#include "sensors/sensors.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/crsf.h"
|
||||
|
||||
|
@ -170,9 +170,9 @@ bool taskUpdateRxMainInProgress()
|
|||
|
||||
static void taskUpdateRxMain(timeUs_t currentTimeUs)
|
||||
{
|
||||
// Where we are using a state machine call ignoreTaskTime() for all states bar one
|
||||
// Where we are using a state machine call ignoreTaskStateTime() for all states bar one
|
||||
if (rxState != MODES) {
|
||||
ignoreTaskTime();
|
||||
ignoreTaskStateTime();
|
||||
}
|
||||
|
||||
switch (rxState) {
|
||||
|
@ -418,7 +418,6 @@ void tasksInit(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
|
||||
.taskName = taskNameParam, \
|
||||
.subTaskName = subTaskNameParam, \
|
||||
|
@ -427,15 +426,6 @@ void tasksInit(void)
|
|||
.desiredPeriodUs = desiredPeriodParam, \
|
||||
.staticPriority = staticPriorityParam \
|
||||
}
|
||||
#else
|
||||
#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
|
||||
.checkFunc = checkFuncParam, \
|
||||
.taskFunc = taskFuncParam, \
|
||||
.desiredPeriodUs = desiredPeriodParam, \
|
||||
.staticPriority = staticPriorityParam \
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
task_t tasks[TASK_COUNT] = {
|
||||
[TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
|
||||
|
|
|
@ -45,6 +45,8 @@
|
|||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
|
@ -591,6 +593,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
|
|||
acc.accADC[X] = 0;
|
||||
acc.accADC[Y] = 0;
|
||||
acc.accADC[Z] = 0;
|
||||
ignoreTaskStateTime();
|
||||
}
|
||||
}
|
||||
#endif // USE_ACC
|
||||
|
|
|
@ -543,7 +543,6 @@ static void showSensorsPage(void)
|
|||
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
static void showTasksPage(void)
|
||||
{
|
||||
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
||||
|
@ -568,7 +567,6 @@ static void showTasksPage(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
static void showBBPage(void)
|
||||
|
@ -624,9 +622,7 @@ static const pageEntry_t pages[PAGE_COUNT] = {
|
|||
{ PAGE_RX, "RX", showRxPage, PAGE_FLAGS_NONE },
|
||||
{ PAGE_BATTERY, "BATTERY", showBatteryPage, PAGE_FLAGS_NONE },
|
||||
{ PAGE_SENSORS, "SENSORS", showSensorsPage, PAGE_FLAGS_NONE },
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
{ PAGE_TASKS, "TASKS", showTasksPage, PAGE_FLAGS_NONE },
|
||||
#endif
|
||||
#ifdef USE_BLACKBOX
|
||||
{ PAGE_BB, "BLACK BOX", showBBPage, PAGE_FLAGS_NONE },
|
||||
#endif
|
||||
|
|
|
@ -44,9 +44,7 @@ typedef enum {
|
|||
PAGE_RX,
|
||||
PAGE_PROFILE,
|
||||
PAGE_RPROF,
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
PAGE_TASKS,
|
||||
#endif
|
||||
#ifdef USE_GPS
|
||||
PAGE_GPS,
|
||||
#endif
|
||||
|
|
|
@ -755,14 +755,14 @@ void gpsUpdate(timeUs_t currentTimeUs)
|
|||
updateGPSRescueState();
|
||||
}
|
||||
#endif
|
||||
// Call ignoreTaskTime() unless this took appreciable time
|
||||
// Call ignoreTaskShortExecTime() unless this took appreciable time
|
||||
// Note that this will mess up the rate/Hz display under tasks, but the code
|
||||
// takes widely varying time to complete
|
||||
endTimeUs = micros();
|
||||
if ((endTimeUs - currentTimeUs) > maxTimeUs) {
|
||||
maxTimeUs = endTimeUs - currentTimeUs;
|
||||
} else {
|
||||
ignoreTaskTime();
|
||||
ignoreTaskShortExecTime();
|
||||
// Decay max time
|
||||
maxTimeUs--;
|
||||
}
|
||||
|
|
|
@ -1066,8 +1066,8 @@ static void applyStatusProfile(timeUs_t now) {
|
|||
}
|
||||
|
||||
if (!timActive) {
|
||||
// Call ignoreTaskTime() unless data is being processed
|
||||
ignoreTaskTime();
|
||||
// Call ignoreTaskShortExecTime() unless data is being processed
|
||||
ignoreTaskShortExecTime();
|
||||
return; // no change this update, keep old state
|
||||
}
|
||||
|
||||
|
@ -1253,8 +1253,8 @@ void ledStripUpdate(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
|
||||
if (!isWS2811LedStripReady()) {
|
||||
// Call ignoreTaskTime() unless data is being processed
|
||||
ignoreTaskTime();
|
||||
// Call ignoreTaskShortExecTime() unless data is being processed
|
||||
ignoreTaskShortExecTime();
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -1282,8 +1282,8 @@ void ledStripUpdate(timeUs_t currentTimeUs)
|
|||
break;
|
||||
}
|
||||
} else {
|
||||
// Call ignoreTaskTime() unless data is being processed
|
||||
ignoreTaskTime();
|
||||
// Call ignoreTaskShortExecTime() unless data is being processed
|
||||
ignoreTaskShortExecTime();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -87,6 +87,8 @@
|
|||
#include "rx/crsf.h"
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
@ -1049,6 +1051,8 @@ void osdUpdate(timeUs_t currentTimeUs)
|
|||
// Frsky osd need a display redraw after search for MAX7456 devices
|
||||
if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
|
||||
displayRedraw(osdDisplayPort);
|
||||
} else {
|
||||
ignoreTaskShortExecTime();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
@ -1062,6 +1066,7 @@ void osdUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
// don't touch buffers if DMA transaction is in progress
|
||||
if (displayIsTransferInProgress(osdDisplayPort)) {
|
||||
ignoreTaskShortExecTime();
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -1069,6 +1074,7 @@ void osdUpdate(timeUs_t currentTimeUs)
|
|||
static uint32_t idlecounter = 0;
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (idlecounter++ % 4 != 0) {
|
||||
ignoreTaskShortExecTime();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
@ -1091,6 +1097,7 @@ void osdUpdate(timeUs_t currentTimeUs)
|
|||
if (doDrawScreen) {
|
||||
displayDrawScreen(osdDisplayPort);
|
||||
}
|
||||
ignoreTaskShortExecTime();
|
||||
}
|
||||
++counter;
|
||||
}
|
||||
|
|
|
@ -26,6 +26,8 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
|
@ -34,14 +36,15 @@
|
|||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/core.h"
|
||||
#include "fc/tasks.h"
|
||||
|
||||
#include "scheduler.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
|
||||
#include "sensors/gyro_init.h"
|
||||
|
||||
// DEBUG_SCHEDULER, timings for:
|
||||
// 0 - gyroUpdate()
|
||||
|
@ -49,21 +52,48 @@
|
|||
// 2 - time spent in scheduler
|
||||
// 3 - time spent executing check function
|
||||
|
||||
// DEBUG_SCHEDULER_DETERMINISM, requires USE_LATE_TASK_STATISTICS to be defined
|
||||
// 0 - Gyro task start cycle time in 10th of a us
|
||||
// 1 - ID of late task
|
||||
// 2 - Amount task is late in 10th of a us
|
||||
// 3 - Gyro lock skew in clock cycles
|
||||
|
||||
extern task_t tasks[];
|
||||
|
||||
static FAST_DATA_ZERO_INIT task_t *currentTask = NULL;
|
||||
static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskTime;
|
||||
static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecRate;
|
||||
static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecTime;
|
||||
|
||||
// More than this many cycles overdue will be considered late
|
||||
#define DEBUG_SCHEDULER_DETERMINISM_THRESHOLD 10
|
||||
|
||||
int32_t schedLoopStartCycles;
|
||||
static int32_t schedLoopStartMinCycles;
|
||||
static int32_t schedLoopStartMaxCycles;
|
||||
static uint32_t schedLoopStartDeltaDownCycles;
|
||||
static uint32_t schedLoopStartDeltaUpCycles;
|
||||
|
||||
int32_t taskGuardCycles;
|
||||
static int32_t taskGuardMinCycles;
|
||||
static int32_t taskGuardMaxCycles;
|
||||
static uint32_t taskGuardDeltaDownCycles;
|
||||
static uint32_t taskGuardDeltaUpCycles;
|
||||
|
||||
static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasks;
|
||||
static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasksSamples;
|
||||
|
||||
static FAST_DATA_ZERO_INIT bool calculateTaskStatistics;
|
||||
FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
|
||||
|
||||
static FAST_DATA_ZERO_INIT int taskQueuePos = 0;
|
||||
STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0;
|
||||
|
||||
static FAST_DATA int periodCalculationBasisOffset = offsetof(task_t, lastExecutedAtUs);
|
||||
static bool optimizeSchedRate;
|
||||
|
||||
static FAST_DATA_ZERO_INIT bool gyroEnabled;
|
||||
|
||||
static int32_t desiredPeriodCycles;
|
||||
static uint32_t lastTargetCycles;
|
||||
|
||||
// No need for a linked list for the queue, since items are only inserted at startup
|
||||
|
||||
STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
|
||||
|
@ -145,7 +175,6 @@ void taskSystemLoad(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
timeUs_t checkFuncMaxExecutionTimeUs;
|
||||
timeUs_t checkFuncTotalExecutionTimeUs;
|
||||
timeUs_t checkFuncMovingSumExecutionTimeUs;
|
||||
|
@ -158,14 +187,12 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
|
|||
checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
}
|
||||
#endif
|
||||
|
||||
void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
|
||||
{
|
||||
taskInfo->isEnabled = queueContains(getTask(taskId));
|
||||
taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs;
|
||||
taskInfo->staticPriority = getTask(taskId)->staticPriority;
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
taskInfo->taskName = getTask(taskId)->taskName;
|
||||
taskInfo->subTaskName = getTask(taskId)->subTaskName;
|
||||
taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
|
||||
|
@ -179,7 +206,6 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
|
|||
taskInfo->runCount = getTask(taskId)->runCount;
|
||||
taskInfo->execTime = getTask(taskId)->execTime;
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
|
||||
|
@ -191,6 +217,11 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
|
|||
task_t *task = getTask(taskId);
|
||||
task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
|
||||
}
|
||||
|
||||
// Catch the case where the gyro loop is adjusted
|
||||
if (taskId == TASK_GYRO) {
|
||||
desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs);
|
||||
}
|
||||
}
|
||||
|
||||
void setTaskEnabled(taskId_e taskId, bool enabled)
|
||||
|
@ -216,20 +247,22 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
|
|||
}
|
||||
}
|
||||
|
||||
// Called by tasks executing what are known to be short states
|
||||
void ignoreTaskTime()
|
||||
// Called by tasks executing what are known to be short states. Tasks should call this routine in all states
|
||||
// except the one which takes the longest to execute.
|
||||
void ignoreTaskStateTime()
|
||||
{
|
||||
ignoreCurrentTaskTime = true;
|
||||
ignoreCurrentTaskExecRate = true;
|
||||
ignoreCurrentTaskExecTime = true;
|
||||
}
|
||||
|
||||
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
|
||||
// Called by tasks without state machines executing in what is known to be a shorter time than peak
|
||||
void ignoreTaskShortExecTime()
|
||||
{
|
||||
calculateTaskStatistics = calculateTaskStatisticsToUse;
|
||||
ignoreCurrentTaskExecTime = true;
|
||||
}
|
||||
|
||||
void schedulerResetTaskStatistics(taskId_e taskId)
|
||||
{
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (taskId == TASK_SELF) {
|
||||
currentTask->movingSumExecutionTimeUs = 0;
|
||||
currentTask->movingSumDeltaTimeUs = 0;
|
||||
|
@ -241,14 +274,10 @@ void schedulerResetTaskStatistics(taskId_e taskId)
|
|||
getTask(taskId)->totalExecutionTimeUs = 0;
|
||||
getTask(taskId)->maxExecutionTimeUs = 0;
|
||||
}
|
||||
#else
|
||||
UNUSED(taskId);
|
||||
#endif
|
||||
}
|
||||
|
||||
void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
|
||||
{
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (taskId == TASK_SELF) {
|
||||
currentTask->maxExecutionTimeUs = 0;
|
||||
} else if (taskId < TASK_COUNT) {
|
||||
|
@ -259,37 +288,46 @@ void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
|
|||
task->runCount = 0;
|
||||
#endif
|
||||
}
|
||||
#else
|
||||
UNUSED(taskId);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
void schedulerResetCheckFunctionMaxExecutionTime(void)
|
||||
{
|
||||
checkFuncMaxExecutionTimeUs = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
void schedulerInit(void)
|
||||
{
|
||||
calculateTaskStatistics = true;
|
||||
queueClear();
|
||||
queueAdd(getTask(TASK_SYSTEM));
|
||||
|
||||
schedLoopStartMinCycles = clockMicrosToCycles(SCHED_START_LOOP_MIN_US);
|
||||
schedLoopStartMaxCycles = clockMicrosToCycles(SCHED_START_LOOP_MAX_US);
|
||||
schedLoopStartCycles = schedLoopStartMinCycles;
|
||||
schedLoopStartDeltaDownCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_DOWN_STEP;
|
||||
schedLoopStartDeltaUpCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_UP_STEP;
|
||||
|
||||
taskGuardMinCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MIN_US);
|
||||
taskGuardMaxCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MAX_US);
|
||||
taskGuardCycles = taskGuardMinCycles;
|
||||
taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP;
|
||||
taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP;
|
||||
|
||||
desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs);
|
||||
|
||||
lastTargetCycles = getCycleCounter();
|
||||
}
|
||||
|
||||
void schedulerOptimizeRate(bool optimizeRate)
|
||||
{
|
||||
periodCalculationBasisOffset = optimizeRate ? offsetof(task_t, lastDesiredAt) : offsetof(task_t, lastExecutedAtUs);
|
||||
optimizeSchedRate = optimizeRate;
|
||||
}
|
||||
|
||||
inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
|
||||
{
|
||||
if (task->staticPriority == TASK_PRIORITY_REALTIME) {
|
||||
return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
|
||||
} else {
|
||||
return task->lastExecutedAtUs;
|
||||
if ((task->staticPriority == TASK_PRIORITY_REALTIME) && (optimizeSchedRate)) {
|
||||
return task->lastDesiredAt;
|
||||
}
|
||||
return task->lastExecutedAtUs;
|
||||
}
|
||||
|
||||
FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
|
||||
|
@ -298,38 +336,33 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
|
|||
|
||||
if (selectedTask) {
|
||||
currentTask = selectedTask;
|
||||
ignoreCurrentTaskTime = false;
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
ignoreCurrentTaskExecRate = false;
|
||||
ignoreCurrentTaskExecTime = false;
|
||||
float period = currentTimeUs - selectedTask->lastExecutedAtUs;
|
||||
#endif
|
||||
selectedTask->lastExecutedAtUs = currentTimeUs;
|
||||
selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs;
|
||||
selectedTask->dynamicPriority = 0;
|
||||
|
||||
// Execute task
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (calculateTaskStatistics) {
|
||||
const timeUs_t currentTimeBeforeTaskCallUs = micros();
|
||||
selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
|
||||
taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
|
||||
if (!ignoreCurrentTaskTime) {
|
||||
selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
|
||||
selectedTask->lastStatsAtUs = currentTimeUs;
|
||||
selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
|
||||
const timeUs_t currentTimeBeforeTaskCallUs = micros();
|
||||
selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
|
||||
taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
|
||||
if (!ignoreCurrentTaskExecRate) {
|
||||
// Record task execution rate and max execution time
|
||||
selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
|
||||
selectedTask->lastStatsAtUs = currentTimeUs;
|
||||
selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
|
||||
}
|
||||
if (!ignoreCurrentTaskExecTime) {
|
||||
// Update estimate of expected task duration
|
||||
selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
}
|
||||
selectedTask->totalExecutionTimeUs += taskExecutionTimeUs; // time consumed by scheduler + task
|
||||
selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
|
||||
#if defined(USE_LATE_TASK_STATISTICS)
|
||||
selectedTask->runCount++;
|
||||
selectedTask->runCount++;
|
||||
#endif
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs);
|
||||
selectedTask->taskFunc(currentTimeUs);
|
||||
}
|
||||
}
|
||||
|
||||
return taskExecutionTimeUs;
|
||||
|
@ -350,79 +383,147 @@ static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynami
|
|||
|
||||
FAST_CODE void scheduler(void)
|
||||
{
|
||||
#if defined(USE_LATE_TASK_STATISTICS)
|
||||
static task_t *lastTask;
|
||||
#endif
|
||||
// Cache currentTime
|
||||
const timeUs_t schedulerStartTimeUs = micros();
|
||||
timeUs_t currentTimeUs = schedulerStartTimeUs;
|
||||
timeUs_t currentTimeUs;
|
||||
uint32_t nowCycles;
|
||||
timeUs_t taskExecutionTimeUs = 0;
|
||||
task_t *selectedTask = NULL;
|
||||
uint16_t selectedTaskDynamicPriority = 0;
|
||||
uint16_t waitingTasks = 0;
|
||||
bool realtimeTaskRan = false;
|
||||
timeDelta_t gyroTaskDelayUs = 0;
|
||||
uint32_t nextTargetCycles = 0;
|
||||
int32_t schedLoopRemainingCycles;
|
||||
|
||||
if (gyroEnabled) {
|
||||
// Realtime gyro/filtering/PID tasks get complete priority
|
||||
task_t *gyroTask = getTask(TASK_GYRO);
|
||||
const timeUs_t gyroExecuteTimeUs = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriodUs;
|
||||
gyroTaskDelayUs = cmpTimeUs(gyroExecuteTimeUs, currentTimeUs); // time until the next expected gyro sample
|
||||
if (gyroTaskDelayUs <= 0) {
|
||||
taskExecutionTimeUs = schedulerExecuteTask(gyroTask, currentTimeUs);
|
||||
nowCycles = getCycleCounter();
|
||||
#if defined(UNIT_TEST)
|
||||
lastTargetCycles = clockMicrosToCycles(gyroTask->lastExecutedAtUs);
|
||||
#endif
|
||||
nextTargetCycles = lastTargetCycles + desiredPeriodCycles;
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
|
||||
if (schedLoopRemainingCycles < -desiredPeriodCycles) {
|
||||
/* A task has so grossly overrun that at entire gyro cycle has been skipped
|
||||
* This is most likely to occur when connected to the configurator via USB as the serial
|
||||
* task is non-deterministic
|
||||
* Recover as best we can, advancing scheduling by a whole number of cycles
|
||||
*/
|
||||
nextTargetCycles += desiredPeriodCycles * (1 + (schedLoopRemainingCycles / -desiredPeriodCycles));
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
}
|
||||
|
||||
// Tune out the time lost between completing the last task execution and re-entering the scheduler
|
||||
if ((schedLoopRemainingCycles < schedLoopStartMinCycles) &&
|
||||
(schedLoopStartCycles < schedLoopStartMaxCycles)) {
|
||||
schedLoopStartCycles += schedLoopStartDeltaUpCycles;
|
||||
}
|
||||
|
||||
// Once close to the timing boundary, poll for it's arrival
|
||||
if (schedLoopRemainingCycles < schedLoopStartCycles) {
|
||||
// Record the interval between scheduling the gyro task
|
||||
static int32_t lastRealtimeStartCycles = 0;
|
||||
|
||||
if (schedLoopStartCycles > schedLoopStartMinCycles) {
|
||||
schedLoopStartCycles -= schedLoopStartDeltaDownCycles;
|
||||
}
|
||||
#if !defined(UNIT_TEST)
|
||||
while (schedLoopRemainingCycles > 0) {
|
||||
nowCycles = getCycleCounter();
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
}
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastRealtimeStartCycles)));
|
||||
lastRealtimeStartCycles = nowCycles;
|
||||
|
||||
currentTimeUs = clockCyclesToMicros(nowCycles);
|
||||
taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
|
||||
|
||||
if (gyroFilterReady()) {
|
||||
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
|
||||
}
|
||||
if (pidLoopReady()) {
|
||||
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
|
||||
}
|
||||
currentTimeUs = micros();
|
||||
realtimeTaskRan = true;
|
||||
|
||||
#if defined(USE_LATE_TASK_STATISTICS)
|
||||
// Late, so make a note of the offending task
|
||||
if (gyroTaskDelayUs < -1) {
|
||||
if (lastTask) {
|
||||
lastTask->lateCount++;
|
||||
lastTargetCycles = nextTargetCycles;
|
||||
|
||||
#ifdef USE_GYRO_EXTI
|
||||
gyroDev_t *gyro = gyroActiveDev();
|
||||
|
||||
// Bring the scheduler into lock with the gyro
|
||||
if (gyro->gyroModeSPI != GYRO_EXTI_NO_INT) {
|
||||
// Track the actual gyro rate over given number of cycle times and set the expected timebase
|
||||
static uint32_t terminalGyroRateCount = 0;
|
||||
static int32_t sampleRateStartCycles;
|
||||
|
||||
if ((terminalGyroRateCount == 0)) {
|
||||
terminalGyroRateCount = gyro->detectedEXTI + GYRO_RATE_COUNT;
|
||||
sampleRateStartCycles = nowCycles;
|
||||
}
|
||||
|
||||
if (gyro->detectedEXTI >= terminalGyroRateCount) {
|
||||
// Calculate the number of clock cycles on average between gyro interrupts
|
||||
uint32_t sampleCycles = nowCycles - sampleRateStartCycles;
|
||||
desiredPeriodCycles = sampleCycles / GYRO_RATE_COUNT;
|
||||
sampleRateStartCycles = nowCycles;
|
||||
terminalGyroRateCount += GYRO_RATE_COUNT;
|
||||
}
|
||||
|
||||
// Track the actual gyro rate over given number of cycle times and remove skew
|
||||
static uint32_t terminalGyroLockCount = 0;
|
||||
static int32_t accGyroSkew = 0;
|
||||
|
||||
int32_t gyroSkew = cmpTimeCycles(nextTargetCycles, gyro->gyroSyncEXTI) % desiredPeriodCycles;
|
||||
if (gyroSkew > (desiredPeriodCycles / 2)) {
|
||||
gyroSkew -= desiredPeriodCycles;
|
||||
}
|
||||
|
||||
accGyroSkew += gyroSkew;
|
||||
|
||||
if ((terminalGyroLockCount == 0)) {
|
||||
terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT;
|
||||
}
|
||||
|
||||
if (gyro->detectedEXTI >= terminalGyroLockCount) {
|
||||
terminalGyroLockCount += GYRO_LOCK_COUNT;
|
||||
|
||||
// Move the desired start time of the gyroTask
|
||||
lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT);
|
||||
DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 3, clockCyclesTo10thMicros(accGyroSkew/GYRO_LOCK_COUNT));
|
||||
accGyroSkew = 0;
|
||||
}
|
||||
}
|
||||
lastTask = NULL;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!gyroEnabled || realtimeTaskRan || (gyroTaskDelayUs > GYRO_TASK_GUARD_INTERVAL_US)) {
|
||||
// The task to be invoked
|
||||
nowCycles = getCycleCounter();
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
|
||||
|
||||
if (!gyroEnabled || (schedLoopRemainingCycles > (int32_t)clockMicrosToCycles(CHECK_GUARD_MARGIN_US))) {
|
||||
currentTimeUs = micros();
|
||||
|
||||
// Update task dynamic priorities
|
||||
for (task_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 currentTimeBeforeCheckFuncCallUs = micros();
|
||||
#else
|
||||
const timeUs_t currentTimeBeforeCheckFuncCallUs = currentTimeUs;
|
||||
#endif
|
||||
// Increase priority for event driven tasks
|
||||
if (task->dynamicPriority > 0) {
|
||||
task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAtUs) / task->desiredPeriodUs);
|
||||
task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->desiredPeriodUs);
|
||||
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||
waitingTasks++;
|
||||
} else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, cmpTimeUs(currentTimeBeforeCheckFuncCallUs, task->lastExecutedAtUs))) {
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCallUs);
|
||||
#endif
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
if (calculateTaskStatistics) {
|
||||
const uint32_t checkFuncExecutionTimeUs = micros() - currentTimeBeforeCheckFuncCallUs;
|
||||
checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task
|
||||
checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
|
||||
}
|
||||
#endif
|
||||
task->lastSignaledAtUs = currentTimeBeforeCheckFuncCallUs;
|
||||
} else if (task->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
|
||||
const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
|
||||
checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task
|
||||
checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
|
||||
task->lastSignaledAtUs = currentTimeUs;
|
||||
task->taskAgeCycles = 1;
|
||||
task->dynamicPriority = 1 + task->staticPriority;
|
||||
waitingTasks++;
|
||||
|
@ -432,7 +533,7 @@ FAST_CODE void scheduler(void)
|
|||
} 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->desiredPeriodUs);
|
||||
task->taskAgeCycles = (cmpTimeUs(currentTimeUs, getPeriodCalculationBasis(task)) / task->desiredPeriodUs);
|
||||
if (task->taskAgeCycles > 0) {
|
||||
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
|
||||
waitingTasks++;
|
||||
|
@ -450,35 +551,46 @@ FAST_CODE void scheduler(void)
|
|||
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->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US;
|
||||
timeDelta_t taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
|
||||
#if defined(USE_LATE_TASK_STATISTICS)
|
||||
selectedTask->execTime = taskRequiredTimeUs;
|
||||
selectedTask->execTime = taskRequiredTimeUs;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
// Add in the time spent so far in check functions and the scheduler logic
|
||||
taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
|
||||
if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
|
||||
taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
|
||||
} else {
|
||||
selectedTask = NULL;
|
||||
}
|
||||
int32_t taskRequiredTimeCycles = (int32_t)clockMicrosToCycles((uint32_t)taskRequiredTimeUs);
|
||||
|
||||
nowCycles = getCycleCounter();
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
|
||||
// Allow a little extra time
|
||||
taskRequiredTimeCycles += taskGuardCycles;
|
||||
|
||||
if (!gyroEnabled || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
|
||||
taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
|
||||
nowCycles = getCycleCounter();
|
||||
schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
|
||||
|
||||
if (schedLoopRemainingCycles < taskGuardMinCycles) {
|
||||
if (taskGuardCycles < taskGuardMaxCycles) {
|
||||
taskGuardCycles += taskGuardDeltaUpCycles;
|
||||
}
|
||||
} else if (taskGuardCycles > taskGuardMinCycles) {
|
||||
taskGuardCycles -= taskGuardDeltaDownCycles;
|
||||
}
|
||||
#if defined(USE_LATE_TASK_STATISTICS)
|
||||
lastTask = selectedTask;
|
||||
#endif
|
||||
if (schedLoopRemainingCycles < DEBUG_SCHEDULER_DETERMINISM_THRESHOLD) {
|
||||
DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, selectedTask - tasks);
|
||||
DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(schedLoopRemainingCycles));
|
||||
selectedTask->lateCount++ ;
|
||||
}
|
||||
#endif // USE_LATE_TASK_STATISTICS
|
||||
} else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) {
|
||||
// If a task has been unable to run, then reduce it's recorded estimated run time to ensure
|
||||
// it's ultimate scheduling
|
||||
selectedTask->movingSumExecutionTimeUs *= TASK_AGE_EXPEDITE_SCALE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if defined(SCHEDULER_DEBUG)
|
||||
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler
|
||||
#else
|
||||
UNUSED(taskExecutionTimeUs);
|
||||
#endif
|
||||
|
||||
#if defined(UNIT_TEST)
|
||||
readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks);
|
||||
|
@ -494,3 +606,9 @@ uint16_t getAverageSystemLoadPercent(void)
|
|||
{
|
||||
return averageSystemLoadPercent;
|
||||
}
|
||||
|
||||
float schedulerGetCycleTimeMultipler(void)
|
||||
{
|
||||
return (float)clockMicrosToCycles(getTask(TASK_GYRO)->desiredPeriodUs) / desiredPeriodCycles;
|
||||
|
||||
}
|
||||
|
|
|
@ -27,14 +27,29 @@
|
|||
#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
|
||||
|
||||
#define LOAD_PERCENTAGE_ONE 100
|
||||
|
||||
#define SCHED_START_LOOP_MIN_US 4 // Wait at start of scheduler loop if gyroTask is nearly due
|
||||
#define SCHED_START_LOOP_MAX_US 12
|
||||
#define SCHED_START_LOOP_DOWN_STEP 50 // Fraction of a us to reduce start loop wait
|
||||
#define SCHED_START_LOOP_UP_STEP 1 // Fraction of a us to increase start loop wait
|
||||
|
||||
#define TASK_GUARD_MARGIN_MIN_US 2 // Add an amount to the estimate of a task duration
|
||||
#define TASK_GUARD_MARGIN_MAX_US 5
|
||||
#define TASK_GUARD_MARGIN_DOWN_STEP 50 // Fraction of a us to reduce task guard margin
|
||||
#define TASK_GUARD_MARGIN_UP_STEP 1 // Fraction of a us to increase task guard margin
|
||||
|
||||
#define CHECK_GUARD_MARGIN_US 2 // Add a margin to the amount of time allowed for a check function to run
|
||||
|
||||
#define TASK_AGE_EXPEDITE_COUNT 1 // Make aged tasks more schedulable
|
||||
#define TASK_AGE_EXPEDITE_SCALE 0.9 // By scaling their expected execution time
|
||||
|
||||
// Gyro interrupt counts over which to measure loop time and skew
|
||||
#define GYRO_RATE_COUNT 32000
|
||||
#define GYRO_LOCK_COUNT 400
|
||||
|
||||
typedef enum {
|
||||
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
|
||||
|
@ -164,10 +179,8 @@ typedef enum {
|
|||
|
||||
typedef struct {
|
||||
// Configuration
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
const char * taskName;
|
||||
const char * subTaskName;
|
||||
#endif
|
||||
bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
|
||||
void (*taskFunc)(timeUs_t currentTimeUs);
|
||||
timeDelta_t desiredPeriodUs; // target period of execution
|
||||
|
@ -181,7 +194,6 @@ typedef struct {
|
|||
timeUs_t lastSignaledAtUs; // time of invocation event for event-driven tasks
|
||||
timeUs_t lastDesiredAt; // time of last desired execution
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
// Statistics
|
||||
float movingAverageCycleTimeUs;
|
||||
timeUs_t movingSumExecutionTimeUs; // moving sum over 32 samples
|
||||
|
@ -194,7 +206,6 @@ typedef struct {
|
|||
uint32_t lateCount;
|
||||
timeUs_t execTime;
|
||||
#endif
|
||||
#endif
|
||||
} task_t;
|
||||
|
||||
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
|
||||
|
@ -202,8 +213,8 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
|
|||
void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
|
||||
void setTaskEnabled(taskId_e taskId, bool newEnabledState);
|
||||
timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
|
||||
void ignoreTaskTime();
|
||||
void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
|
||||
void ignoreTaskStateTime();
|
||||
void ignoreTaskShortExecTime();
|
||||
void schedulerResetTaskStatistics(taskId_e taskId);
|
||||
void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
|
||||
void schedulerResetCheckFunctionMaxExecutionTime(void);
|
||||
|
@ -215,3 +226,4 @@ void taskSystemLoad(timeUs_t currentTimeUs);
|
|||
void schedulerOptimizeRate(bool optimizeRate);
|
||||
void schedulerEnableGyro(void);
|
||||
uint16_t getAverageSystemLoadPercent(void);
|
||||
float schedulerGetCycleTimeMultipler(void);
|
||||
|
|
|
@ -389,7 +389,13 @@ uint32_t baroUpdate(void)
|
|||
// Tell the scheduler to ignore how long this task takes unless the pressure is being read
|
||||
// as that takes the longest
|
||||
if (state != BAROMETER_NEEDS_PRESSURE_READ) {
|
||||
ignoreTaskTime();
|
||||
ignoreTaskStateTime();
|
||||
}
|
||||
|
||||
if (busBusy(&baro.dev.dev, NULL)) {
|
||||
// If the bus is busy, simply return to have another go later
|
||||
ignoreTaskStateTime();
|
||||
return sleepTime;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
|
@ -422,7 +428,7 @@ uint32_t baroUpdate(void)
|
|||
if (baro.dev.read_up(&baro.dev)) {
|
||||
state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
|
||||
} else {
|
||||
ignoreTaskTime();
|
||||
ignoreTaskStateTime();
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
|
@ -81,26 +81,26 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
|
|||
// 3. Slave I2C device on SPI gyro
|
||||
|
||||
#if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963))
|
||||
compassConfig->mag_bustype = BUS_TYPE_SPI;
|
||||
compassConfig->mag_busType = BUS_TYPE_SPI;
|
||||
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE));
|
||||
compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN);
|
||||
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
|
||||
compassConfig->mag_i2c_address = 0;
|
||||
#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
|
||||
compassConfig->mag_bustype = BUS_TYPE_I2C;
|
||||
compassConfig->mag_busType = BUS_TYPE_I2C;
|
||||
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
|
||||
compassConfig->mag_i2c_address = 0;
|
||||
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
|
||||
compassConfig->mag_spi_csn = IO_TAG_NONE;
|
||||
#elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
|
||||
compassConfig->mag_bustype = BUS_TYPE_MPU_SLAVE;
|
||||
compassConfig->mag_busType = BUS_TYPE_MPU_SLAVE;
|
||||
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
|
||||
compassConfig->mag_i2c_address = 0;
|
||||
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
|
||||
compassConfig->mag_spi_csn = IO_TAG_NONE;
|
||||
#else
|
||||
compassConfig->mag_hardware = MAG_NONE;
|
||||
compassConfig->mag_bustype = BUS_TYPE_NONE;
|
||||
compassConfig->mag_busType = BUS_TYPE_NONE;
|
||||
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
|
||||
compassConfig->mag_i2c_address = 0;
|
||||
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
|
||||
|
@ -115,7 +115,7 @@ static uint8_t magInit = 0;
|
|||
void compassPreInit(void)
|
||||
{
|
||||
#ifdef USE_SPI
|
||||
if (compassConfig()->mag_bustype == BUS_TYPE_SPI) {
|
||||
if (compassConfig()->mag_busType == BUS_TYPE_SPI) {
|
||||
spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1);
|
||||
}
|
||||
#endif
|
||||
|
@ -136,7 +136,7 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
|
|||
magDev->magIntExtiTag = compassConfig()->interruptTag;
|
||||
#endif
|
||||
|
||||
switch (compassConfig()->mag_bustype) {
|
||||
switch (compassConfig()->mag_busType) {
|
||||
#ifdef USE_I2C
|
||||
case BUS_TYPE_I2C:
|
||||
i2cBusSetInstance(dev, compassConfig()->mag_i2c_device);
|
||||
|
|
|
@ -49,7 +49,7 @@ extern mag_t mag;
|
|||
typedef struct compassConfig_s {
|
||||
uint8_t mag_alignment; // mag alignment
|
||||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
uint8_t mag_bustype;
|
||||
uint8_t mag_busType;
|
||||
uint8_t mag_i2c_device;
|
||||
uint8_t mag_i2c_address;
|
||||
uint8_t mag_spi_device;
|
||||
|
|
|
@ -78,6 +78,11 @@
|
|||
#define ACTIVE_GYRO (&gyro.gyroSensor1)
|
||||
#endif
|
||||
|
||||
// The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
|
||||
// This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
|
||||
// gyro types are supported with SPI DMA.
|
||||
#define GYRO_BUF_SIZE 32
|
||||
|
||||
static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
|
||||
|
||||
static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
|
||||
|
@ -635,6 +640,11 @@ bool gyroInit(void)
|
|||
}
|
||||
|
||||
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
|
||||
// SPI DMA buffer required per device
|
||||
gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
|
||||
gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
|
||||
|
||||
gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
|
||||
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
|
||||
|
@ -646,6 +656,10 @@ bool gyroInit(void)
|
|||
}
|
||||
|
||||
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
|
||||
// SPI DMA buffer required per device
|
||||
gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
|
||||
gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
|
||||
gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
|
||||
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;
|
||||
|
|
|
@ -32,7 +32,6 @@
|
|||
#define USE_EXTI
|
||||
#define MAG_INT_EXTI PC14
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
|
|
|
@ -41,7 +41,6 @@
|
|||
#define BEEPER_PIN PA5
|
||||
|
||||
#define USE_EXTI
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
// Using MPU6050 for the moment.
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC14
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -38,7 +38,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC14
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -36,7 +36,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA8
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -47,7 +47,6 @@
|
|||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
|
|
@ -33,8 +33,6 @@
|
|||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
|
|
@ -134,7 +134,7 @@
|
|||
#define USE_RX_FRSKY_SPI_X
|
||||
#define USE_RX_SFHSS_SPI
|
||||
#define USE_RX_REDPINE_SPI
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
|
||||
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X
|
||||
#define USE_RX_FRSKY_SPI_TELEMETRY
|
||||
|
||||
|
|
|
@ -115,7 +115,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -33,7 +33,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -54,7 +54,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
@ -110,7 +109,7 @@
|
|||
#define UART1_RX_PIN PB7
|
||||
#else
|
||||
#define UART1_RX_PIN PA10
|
||||
#endif
|
||||
#endif
|
||||
#define UART1_TX_PIN PA9
|
||||
|
||||
#define USE_UART4
|
||||
|
|
|
@ -48,7 +48,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -32,10 +32,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
|||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST7_CH5
|
||||
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA2_ST6_CH0
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST2_CH5
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3
|
||||
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6
|
||||
DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL
|
||||
DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL
|
||||
|
||||
};
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define USE_LATE_TASK_STATISTICS
|
||||
|
||||
#ifdef FURYF4OSD
|
||||
#define TARGET_BOARD_IDENTIFIER "FY4O"
|
||||
#define USBD_PRODUCT_STRING "FuryF4OSD"
|
||||
|
@ -158,4 +160,4 @@
|
|||
#define TARGET_IO_PORTD (BIT(2))
|
||||
|
||||
#define USABLE_TIMER_CHANNEL_COUNT 7
|
||||
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) )
|
||||
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) )
|
||||
|
|
|
@ -56,7 +56,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
@ -73,7 +72,7 @@
|
|||
#if defined(FLYWOOF405)
|
||||
//------MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#endif
|
||||
|
||||
#if defined(KAKUTEF4V2) || defined(FLYWOOF405) // There is invertor on RXD3(PB11), so PB10/PB11 can't be used as I2C2.
|
||||
|
@ -154,8 +153,8 @@
|
|||
#define USE_PINIOBOX
|
||||
#else
|
||||
#define ESCSERIAL_TIMER_TX_PIN PC7 // (HARDARE=0,PPM)
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1 //ICM20689
|
||||
#define SPI1_NSS_PIN PC4
|
||||
|
|
|
@ -48,7 +48,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -115,7 +115,7 @@
|
|||
|
||||
#define USE_ADC
|
||||
#define ADC_INSTANCE ADC3
|
||||
#define ADC3_DMA_OPT 0
|
||||
#define ADC3_DMA_OPT 1
|
||||
|
||||
#define VBAT_ADC_PIN PC1
|
||||
#define RSSI_ADC_PIN PC2
|
||||
|
|
|
@ -35,9 +35,7 @@
|
|||
|
||||
#define USE_EXTI
|
||||
#define MAG_INT_EXTI PC14
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MAG_DATA_READY_INTERRUPT
|
||||
#define USE_MAG_DATA_READY_SIGNAL
|
||||
|
||||
// SPI2
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB2
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define USE_LATE_TASK_STATISTICS
|
||||
|
||||
#if defined(AIRBOTF4)
|
||||
#define TARGET_BOARD_IDENTIFIER "AIR4"
|
||||
#define USBD_PRODUCT_STRING "AirbotF4"
|
||||
|
|
|
@ -19,6 +19,9 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define USE_LATE_TASK_STATISTICS
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "REVN"
|
||||
|
||||
#define USBD_PRODUCT_STRING "Revo Nano"
|
||||
|
|
|
@ -334,6 +334,25 @@ uint32_t millis(void) {
|
|||
return millis64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
uint32_t clockCyclesToMicros(uint32_t clockCycles)
|
||||
{
|
||||
return clockCycles;
|
||||
}
|
||||
|
||||
int32_t clockCyclesTo10thMicros(int32_t clockCycles)
|
||||
{
|
||||
return clockCycles;
|
||||
}
|
||||
|
||||
uint32_t clockMicrosToCycles(uint32_t micros)
|
||||
{
|
||||
return micros;
|
||||
}
|
||||
uint32_t getCycleCounter(void)
|
||||
{
|
||||
return (uint32_t) (micros64() & 0xFFFFFFFF);
|
||||
}
|
||||
|
||||
void microsleep(uint32_t usec) {
|
||||
struct timespec ts;
|
||||
ts.tv_sec = 0;
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
|
|
@ -43,7 +43,6 @@
|
|||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
|
|
@ -69,7 +69,6 @@
|
|||
#define GYRO_1_EXTI_PIN PC4
|
||||
#endif
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#if defined(PIRXF4)
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
|
|
|
@ -28,8 +28,6 @@
|
|||
// -Wpadded can be turned on to check padding of structs
|
||||
//#pragma GCC diagnostic warning "-Wpadded"
|
||||
|
||||
//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons
|
||||
|
||||
#ifdef STM32F1
|
||||
#define MINIMAL_CLI
|
||||
// Using RX DMA disables the use of receive callbacks
|
||||
|
@ -92,6 +90,7 @@
|
|||
#define USE_TIMER_MGMT
|
||||
#define USE_PERSISTENT_OBJECTS
|
||||
#define USE_CUSTOM_DEFAULTS_ADDRESS
|
||||
#define USE_LATE_TASK_STATISTICS
|
||||
#endif // STM32F7
|
||||
|
||||
#ifdef STM32H7
|
||||
|
@ -133,6 +132,7 @@
|
|||
#define USE_MCO
|
||||
#define USE_DMA_SPEC
|
||||
#define USE_TIMER_MGMT
|
||||
#define USE_LATE_TASK_STATISTICS
|
||||
#endif
|
||||
|
||||
#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
|
||||
|
@ -231,7 +231,6 @@ extern uint8_t _dmaram_end__;
|
|||
|
||||
#define USE_CLI
|
||||
#define USE_SERIAL_PASSTHROUGH
|
||||
#define USE_TASK_STATISTICS
|
||||
#define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values
|
||||
#define USE_IMU_CALC
|
||||
#define USE_PPM
|
||||
|
|
|
@ -202,6 +202,10 @@ uint16_t batteryWarningVoltage;
|
|||
uint8_t useHottAlarmSoundPeriod (void) { return 0; }
|
||||
const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000}; // see baudRate_e
|
||||
|
||||
uint8_t debugMode;
|
||||
int32_t schedLoopStartCycles;
|
||||
int32_t taskGuardCycles;
|
||||
|
||||
uint32_t micros(void) {return 0;}
|
||||
|
||||
int32_t getAmperage(void) {
|
||||
|
@ -324,7 +328,6 @@ uint8_t serialRead(serialPort_t *){return 0;}
|
|||
void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); }
|
||||
void serialWriteBufShim(void *, const uint8_t *, int) {}
|
||||
bufWriter_t *bufWriterInit(uint8_t *, int, bufWrite_t, void *) {return NULL;}
|
||||
void schedulerSetCalulateTaskStatistics(bool) {}
|
||||
void setArmingDisabled(armingDisableFlags_e) {}
|
||||
|
||||
void waitForSerialPortToFinishTransmitting(serialPort_t *) {}
|
||||
|
|
|
@ -250,4 +250,6 @@ void mixerSetThrottleAngleCorrection(int) {};
|
|||
bool gpsRescueIsRunning(void) { return false; }
|
||||
bool isFixedWing(void) { return false; }
|
||||
void pinioBoxTaskControl(void) {}
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
void ignoreTaskStateTime(void) {}
|
||||
}
|
||||
|
|
|
@ -395,7 +395,7 @@ bool isFlipOverAfterCrashActive(void) { return false; }
|
|||
|
||||
void ws2811LedStripEnable(void) { }
|
||||
|
||||
void setUsedLedCount(unsigned) { };
|
||||
void setUsedLedCount(unsigned) { }
|
||||
void pinioBoxTaskControl(void) {}
|
||||
void ignoreTaskTime(void) {}
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
}
|
||||
|
|
|
@ -559,4 +559,7 @@ extern "C" {
|
|||
float getMotorOutputLow(void) { return 1000.0; }
|
||||
|
||||
float getMotorOutputHigh(void) { return 2047.0; }
|
||||
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
void ignoreTaskStateTime(void) {}
|
||||
}
|
||||
|
|
|
@ -1319,4 +1319,6 @@ extern "C" {
|
|||
bool isUpright(void) { return true; }
|
||||
float getMotorOutputLow(void) { return 1000.0; }
|
||||
float getMotorOutputHigh(void) { return 2047.0; }
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
void ignoreTaskStateTime(void) {}
|
||||
}
|
||||
|
|
|
@ -51,10 +51,18 @@ extern "C" {
|
|||
bool taskPidRan = false;
|
||||
bool taskFilterReady = false;
|
||||
bool taskPidReady = false;
|
||||
uint8_t activePidLoopDenom = 1;
|
||||
|
||||
int16_t debug[1];
|
||||
uint8_t debugMode = 0;
|
||||
|
||||
// set up micros() to simulate time
|
||||
uint32_t simulatedTime = 0;
|
||||
uint32_t micros(void) { return simulatedTime; }
|
||||
uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
|
||||
int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
|
||||
uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
|
||||
uint32_t getCycleCounter(void) {return simulatedTime * 10;}
|
||||
|
||||
// set up tasks to take a simulated representative time to execute
|
||||
bool gyroFilterReady(void) { return taskFilterReady; }
|
||||
|
@ -463,6 +471,7 @@ TEST(SchedulerUnittest, TestGyroTask)
|
|||
|
||||
// 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);
|
||||
|
@ -507,102 +516,3 @@ TEST(SchedulerUnittest, TestGyroTask)
|
|||
EXPECT_EQ(static_cast<task_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<taskId_e>(taskId), false);
|
||||
}
|
||||
setTaskEnabled(TASK_GYRO, true);
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
// set the average run time for TASK_ACCEL
|
||||
tasks[TASK_ACCEL].movingSumExecutionTimeUs = 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;
|
||||
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
|
||||
tasks[TASK_ACCEL].lastExecutedAtUs = 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(&tasks[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;
|
||||
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
|
||||
tasks[TASK_ACCEL].lastExecutedAtUs = 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<task_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;
|
||||
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
|
||||
tasks[TASK_ACCEL].lastExecutedAtUs = 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<task_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;
|
||||
tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
|
||||
tasks[TASK_ACCEL].lastExecutedAtUs = 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(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
}
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
#define SCHEDULER_DELAY_LIMIT 1
|
||||
#define TASK_GYROPID_DESIRED_PERIOD 100
|
||||
|
||||
#define DMA_DATA
|
||||
#define DMA_DATA_ZERO_INIT
|
||||
|
||||
#define USE_ACC
|
||||
|
@ -63,7 +64,6 @@
|
|||
#define USE_UART5
|
||||
#define USE_SOFTSERIAL1
|
||||
#define USE_SOFTSERIAL2
|
||||
#define USE_TASK_STATISTICS
|
||||
|
||||
#define SERIAL_PORT_COUNT 8
|
||||
|
||||
|
|
|
@ -32,6 +32,8 @@ extern "C" {
|
|||
|
||||
extern "C" {
|
||||
void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex);
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
void ignoreTaskStateTime(void) {}
|
||||
}
|
||||
|
||||
TEST(WS2812, updateDMABuffer) {
|
||||
|
@ -89,3 +91,4 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) {
|
|||
|
||||
void ws2811LedStripDMAEnable(void) {}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue