rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved scheduling. Betaflight Port digitalentity/cf-scheduler 

Disallow arming if system load > 100 (waiting task count > 1)

Dont show inactive tasks in CLI

Realtime priority task and guard interval implementation

Dynamic guard interval. Bugfix for realtime scheduling hickups

Optimisations

Compile out CLI command help and CLI tasks command for CJMCU

Naming fixes // re-Add Gyro Sync // Fix port issues
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-18 12:36:05 +10:00 committed by borisbstyle
parent 8ecd05b911
commit fa49931b43
13 changed files with 840 additions and 311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -268,6 +268,7 @@ COMMON_SRC = build_config.c \
common/typeconversion.c \
common/encoding.c \
common/filter.c \
scheduler.c \
main.c \
mw.c \
flight/altitudehold.c \

2
src/main/flight/altitudehold.h
View File

@ -24,6 +24,8 @@
extern int32_t AltHold;
extern int32_t vario;
void calculateEstimatedAltitude(uint32_t currentTime);
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
void applyAltHold(airplaneConfig_t *airplaneConfig);
void updateAltHoldState(void);

60
src/main/flight/imu.c
View File

@ -20,15 +20,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "common/maths.h"
#include "common/filter.h"
#include "build_config.h"
#include "platform.h"
#include "debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
@ -62,30 +62,30 @@ int32_t accSum[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
float fc_acc;
float throttleAngleScale;
float fc_acc;
float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
static bool isAccelUpdatedAtLeastOnce = false;
static imuRuntimeConfig_t *imuRuntimeConfig;
static pidProfile_t *pidProfile;
static accDeadband_t *accDeadband;
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
static float rMat[3][3];
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
static float gyroScale;
static void imuCompureRotationMatrix(void)
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q1q1 = sq(q1);
float q2q2 = sq(q2);
float q3q3 = sq(q3);
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
@ -128,7 +128,7 @@ void imuInit(void)
gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
accVelScale = 9.80665f / acc_1G / 10000.0f;
imuCompureRotationMatrix();
imuComputeRotationMatrix();
}
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
@ -248,7 +248,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Use measured magnetic field vector
recipNorm = mx * mx + my * my + mz * mz;
recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
@ -275,7 +275,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Use measured acceleration vector
recipNorm = ax * ax + ay * ay + az * az;
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) {
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
@ -327,17 +327,17 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuCompureRotationMatrix();
imuComputeRotationMatrix();
}
static void imuUpdateEulerAngles(void)
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
/* Compute pitch/roll angles */
attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
@ -364,7 +364,7 @@ static bool imuIsAccelerometerHealthy(void)
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G));
// Accept accel readings only in range 0.90g - 1.10g
return (81 < accMagnitude) && (accMagnitude < 121);
@ -389,16 +389,15 @@ static void imuCalculateEstimatedAttitude(void)
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
// If reading is considered valid - apply filter
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
} else {
accSmooth[axis] = accADC[axis];
}
}
// Smooth and use only valid accelerometer readings
if (imuIsAccelerometerHealthy()) {
useAcc = true;
}
@ -409,7 +408,7 @@ static void imuCalculateEstimatedAttitude(void)
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(GPS_ground_course - attitude.values.yaw);
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
useYaw = true;
}
#endif
@ -425,15 +424,19 @@ static void imuCalculateEstimatedAttitude(void)
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
}
void imuUpdateGyro(void)
{
gyroUpdate();
}
void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims)
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
{
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
isAccelUpdatedAtLeastOnce = true;
}
}
void imuUpdateGyroAndAttitude(void)
{
gyroUpdate();
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
@ -442,6 +445,11 @@ void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims)
}
}
float getCosTiltAngle(void)
{
return rMat[2][2];
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
/*

2
src/main/flight/imu.h
View File

@ -79,6 +79,8 @@ void imuConfigure(
);
void calculateEstimatedAltitude(uint32_t currentTime);
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
void imuUpdateGyroAndAttitude(void);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);

30
src/main/io/serial_cli.c
View File

@ -24,6 +24,7 @@
#include <ctype.h>
#include "platform.h"
#include "scheduler.h"
#include "version.h"
#include "build_config.h"
@ -120,6 +121,9 @@ static void cliServoMix(char *cmdline);
static void cliSet(char *cmdline);
static void cliGet(char *cmdline);
static void cliStatus(char *cmdline);
#ifndef SKIP_TASK_STATISTICS
static void cliTasks(char *cmdline);
#endif
static void cliVersion(char *cmdline);
static void cliRxRange(char *cmdline);
@ -286,6 +290,9 @@ const clicmd_t cmdTable[] = {
"\treverse <servo> <source> r|n", cliServoMix),
#endif
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
#ifndef SKIP_TASK_STATISTICS
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
#endif
CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
};
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@ -2267,9 +2274,8 @@ static void cliStatus(char *cmdline)
{
UNUSED(cmdline);
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s)\r\n",
millis() / 1000, vbat, batteryCellCount, getBatteryStateString());
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s), System load: %d.%02d\r\n",
millis() / 1000, vbat, batteryCellCount, getBatteryStateString(), averageWaitingTasks100 / 100, averageWaitingTasks100 % 100);
printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
@ -2308,6 +2314,24 @@ static void cliStatus(char *cmdline)
printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
}
#ifndef SKIP_TASK_STATISTICS
static void cliTasks(char *cmdline)
{
UNUSED(cmdline);
cfTaskId_e taskId;
cfTaskInfo_t taskInfo;
printf("Task list:\r\n");
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
getTaskInfo(taskId, &taskInfo);
if (taskInfo.isEnabled) {
printf("%d - %s, max = %d us, avg = %d us, total = %d ms\r\n", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskInfo.totalExecutionTime / 1000);
}
}
}
#endif
static void cliVersion(char *cmdline)
{
UNUSED(cmdline);

6
src/main/io/serial_msp.c
View File

@ -1812,10 +1812,10 @@ static bool processInCommand(void)
waitForSerialPortToFinishTransmitting(currentPort->port);
// Start to activate here
// motor 1 => index 0
// search currentPort portIndex
/* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex]
uint8_t portIndex;
uint8_t portIndex;
for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
if (currentPort == &mspPorts[portIndex]) {
break;
@ -1831,7 +1831,7 @@ static bool processInCommand(void)
mspAllocateSerialPorts(&masterConfig.serialConfig);
/* restore currentPort and mspSerialPort
setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored
*/
*/
// former used MSP uart is active again
// restore MSP_SET_1WIRE as current command for correct headSerialReply(0)
currentPort->cmdMSP = MSP_SET_1WIRE;

50
src/main/main.c
View File

@ -21,6 +21,7 @@
#include <string.h>
#include "platform.h"
#include "scheduler.h"
#include "common/axis.h"
#include "common/color.h"
@ -49,6 +50,7 @@
#include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/gyro_sync.h"
#include "rx/rx.h"
@ -93,7 +95,6 @@
#include "build_config.h"
#include "debug.h"
extern uint32_t previousTime;
extern uint8_t motorControlEnable;
#ifdef SOFTSERIAL_LOOPBACK
@ -120,7 +121,6 @@ void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void loop(void);
void spektrumBind(rxConfig_t *rxConfig);
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
void sonarInit(const sonarHardware_t *sonarHardware);
@ -171,6 +171,7 @@ void init(void)
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
//i2cSetOverclock(masterConfig.i2c_overclock);
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
@ -262,7 +263,7 @@ void init(void)
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500 && !masterConfig.use_fast_pwm)
pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false;
pwm_params.idlePulse = 0; // brushed motors
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
@ -291,11 +292,11 @@ void init(void)
.isInverted = false
#endif
};
#ifdef NAZE
#ifdef AFROMINI
beeperConfig.gpioMode = Mode_Out_PP; // AFROMINI override
beeperConfig.isInverted = true;
#endif
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
@ -315,6 +316,7 @@ void init(void)
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
@ -384,7 +386,7 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -470,8 +472,6 @@ void init(void)
initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
@ -540,8 +540,42 @@ void processLoopback(void) {
int main(void) {
init();
/* Setup scheduler */
rescheduleTask(TASK_GYROPID, targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
setTaskEnabled(TASK_BEEPER, true);
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
while (1) {
loop();
scheduler();
processLoopback();
}
}

484
src/main/mw.c
View File

@ -21,12 +21,14 @@
#include <math.h>
#include "platform.h"
#include "scheduler.h"
#include "debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/filter.h"
#include "common/utils.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -36,7 +38,6 @@
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/bus_bst.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/gyro_sync.h"
@ -59,7 +60,6 @@
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/serial.h"
#include "io/i2c_bst.h"
#include "io/serial_cli.h"
#include "io/serial_msp.h"
#include "io/statusindicator.h"
@ -91,23 +91,22 @@ enum {
ALIGN_MAG = 2
};
//#define JITTER_DEBUG 0 // Specify debug value for jitter debug
/* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500)
#define VBATINTERVAL (6 * 3500)
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500)
#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro
#define PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER 80 // Prevent RX processing before expected loop trigger
#define PREVENT_BARO_READ_PRE_LOOP_TRIGGER 150 // Prevent BARO processing before expected loop trigger
#define GYRO_RATE 0.001f // Gyro refresh rate 1khz
#define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync
// AIR MODE Reset timers
#define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting
bool allowITermShrinkOnly = false;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT = GYRO_RATE; // dT set for gyro refresh rate
float dT;
int16_t magHold;
int16_t headFreeModeHold;
@ -117,9 +116,12 @@ uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint32_t currentTime;
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static bool isRXDataNew;
static filterStatePt1_t filteredCycleTimeState;
uint16_t filteredCycleTime;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -152,7 +154,7 @@ void updateGtuneState(void)
}
} else {
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
DISABLE_FLIGHT_MODE(GTUNE_MODE);
DISABLE_FLIGHT_MODE(GTUNE_MODE);
}
}
}
@ -175,18 +177,16 @@ void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static filterStatePt1_t filteredCycleTimeState;
uint16_t rxRefreshRate, filteredCycleTime;
uint16_t rxRefreshRate;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
initRxRefreshRate(&rxRefreshRate);
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
@ -211,8 +211,6 @@ void annexCode(void)
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
@ -283,27 +281,9 @@ void annexCode(void)
}
if (masterConfig.rxConfig.rcSmoothing) {
filterRc(); // rcCommand smoothing function
filterRc();
}
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
beeperUpdate(); //call periodic beeper handler
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
@ -315,7 +295,7 @@ void annexCode(void)
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
if (isCalibrating() || (averageWaitingTasks100 > 100)) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
@ -329,18 +309,6 @@ void annexCode(void)
warningLedUpdate();
}
#ifdef TELEMETRY
telemetryCheckState();
#endif
handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
@ -443,7 +411,7 @@ void updateInflightCalibrationState(void)
InflightcalibratingA = 50;
AccInflightCalibrationArmed = false;
}
if (IS_RC_MODE_ACTIVE(BOXCALIB)) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
if (IS_RC_MODE_ACTIVE(BOXCALIB)) { // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
AccInflightCalibrationActive = true;
@ -455,7 +423,7 @@ void updateInflightCalibrationState(void)
void updateMagHold(void)
{
if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
if (dif <= -180)
dif += 360;
@ -468,81 +436,6 @@ void updateMagHold(void)
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
typedef enum {
#ifdef MAG
UPDATE_COMPASS_TASK,
#endif
#ifdef BARO
UPDATE_BARO_TASK,
#endif
#ifdef SONAR
UPDATE_SONAR_TASK,
#endif
#if defined(BARO) || defined(SONAR)
CALCULATE_ALTITUDE_TASK,
#endif
UPDATE_DISPLAY_TASK
} periodicTasks;
#define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
void executePeriodicTasks(bool skipBaroUpdate)
{
static int periodicTaskIndex = 0;
switch (periodicTaskIndex++) {
#ifdef MAG
case UPDATE_COMPASS_TASK:
if (sensors(SENSOR_MAG)) {
updateCompass(&masterConfig.magZero);
}
break;
#endif
#ifdef BARO
case UPDATE_BARO_TASK:
if (sensors(SENSOR_BARO) && !(skipBaroUpdate)) {
baroUpdate(currentTime);
}
break;
#endif
#if defined(BARO) || defined(SONAR)
case CALCULATE_ALTITUDE_TASK:
if (false
#if defined(BARO)
|| (sensors(SENSOR_BARO) && isBaroReady())
#endif
#if defined(SONAR)
|| sensors(SENSOR_SONAR)
#endif
) {
calculateEstimatedAltitude(currentTime);
}
break;
#endif
#ifdef SONAR
case UPDATE_SONAR_TASK:
if (sensors(SENSOR_SONAR)) {
sonarUpdate();
}
break;
#endif
#ifdef DISPLAY
case UPDATE_DISPLAY_TASK:
if (feature(FEATURE_DISPLAY)) {
updateDisplay();
}
break;
#endif
}
if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
periodicTaskIndex = 0;
}
}
void processRx(void)
{
static bool armedBeeperOn = false;
@ -664,7 +557,7 @@ void processRx(void)
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
@ -745,94 +638,47 @@ void processRx(void)
}
void loop(void)
{
static uint32_t loopTime;
static bool haveProcessedRxOnceBeforeLoop = false;
bool skipBaroUpdate = false;
#if defined(BARO) || defined(SONAR)
static bool haveProcessedAnnexCodeOnce = false;
static bool haveProcessedAnnexCodeOnce = false;
#endif
updateRx(currentTime);
// Function for loop trigger
bool taskMainPidLoopCheck(uint32_t currentDeltaTime) {
bool loopTrigger = false;
if (shouldProcessRx(currentTime) && (!((int32_t)(currentTime - (loopTime - PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER)) >= 0) || (haveProcessedRxOnceBeforeLoop))) {
processRx();
isRXDataNew = true;
haveProcessedRxOnceBeforeLoop = true;
#ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
}
#endif
#ifdef SONAR
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
}
}
#endif
} else {
if ((int32_t)(currentTime - (loopTime - PREVENT_BARO_READ_PRE_LOOP_TRIGGER)) >= 0) {
skipBaroUpdate = true;
}
// not processing rx this iteration
executePeriodicTasks(skipBaroUpdate);
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
// change this based on available hardware
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsThread();
}
#endif
if (gyroSyncCheckUpdate() || (currentDeltaTime >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
loopTrigger = true;
}
currentTime = micros();
if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
return loopTrigger;
}
loopTime = currentTime + targetLooptime;
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
haveProcessedRxOnceBeforeLoop = false;
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
imuUpdateGyro();
#if defined JITTER_DEBUG
debug[JITTER_DEBUG] = cycleTime - filteredCycleTime;
#endif
// Determine current flight mode. When no acc needed in pid calculations we should only read gyro to reduce latency
if (flightModeFlags) {
imuUpdateAcc(&currentProfile->accelerometerTrims); // When level modes active read gyro and acc
}
imuUpdateGyroAndAttitude();
// Measure loop rate just after reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
annexCode();
annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
haveProcessedAnnexCodeOnce = true;
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
updateMagHold();
}
#endif
#ifdef GTUNE
updateGtuneState();
#endif
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
@ -841,80 +687,212 @@ void loop(void)
}
#endif
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
#endif
&& masterConfig.mixerMode != MIXER_AIRPLANE
&& masterConfig.mixerMode != MIXER_FLYING_WING
#endif
) {
rcCommand[YAW] = 0;
}
) {
rcCommand[YAW] = 0;
}
if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
}
if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
}
#ifdef GPS
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();
}
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();
}
}
#endif
// PID - note this is function pointer set by setPIDController()
pid_controller(
&currentProfile->pidProfile,
currentControlRateProfile,
masterConfig.max_angle_inclination,
&currentProfile->accelerometerTrims,
&masterConfig.rxConfig
);
// PID - note this is function pointer set by setPIDController()
pid_controller(
&currentProfile->pidProfile,
currentControlRateProfile,
masterConfig.max_angle_inclination,
&currentProfile->accelerometerTrims,
&masterConfig.rxConfig
);
mixTable();
mixTable();
#ifdef USE_SERVOS
filterServos();
writeServos();
filterServos();
writeServos();
#endif
if (motorControlEnable) {
writeMotors();
}
// When no level modes active read acc after motor update
if (!flightModeFlags) {
imuUpdateAcc(&currentProfile->accelerometerTrims);
}
if (motorControlEnable) {
writeMotors();
}
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
}
#endif
}
#ifdef TELEMETRY
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
#endif
#ifdef USE_BST
bstProcess();
bstMasterWriteLoop();
#endif
#ifdef LED_STRIP
if (feature(FEATURE_LED_STRIP)) {
updateLedStrip();
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox();
}
#endif
}
void taskUpdateAccelerometer(void)
{
imuUpdateAccelerometer(&currentProfile->accelerometerTrims);
}
void taskHandleSerial(void)
{
handleSerial();
}
void taskUpdateBeeper(void)
{
beeperUpdate(); //call periodic beeper handler
}
void taskUpdateBattery(void)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
}
bool taskUpdateRxCheck(void)
{
updateRx(currentTime);
return shouldProcessRx(currentTime);
}
void taskUpdateRxMain(void)
{
processRx();
isRXDataNew = true;
#ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
}
#endif
#ifdef SONAR
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
}
}
#endif
}
#ifdef GPS
void taskProcessGPS(void)
{
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
// change this based on available hardware
if (feature(FEATURE_GPS)) {
gpsThread();
}
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
}
#endif
#ifdef MAG
void taskUpdateCompass(void)
{
if (sensors(SENSOR_MAG)) {
updateCompass(&masterConfig.magZero);
}
}
#endif
#ifdef BARO
void taskUpdateBaro(void)
{
if (sensors(SENSOR_BARO)) {
uint32_t newDeadline = baroUpdate();
rescheduleTask(TASK_SELF, newDeadline);
}
}
#endif
#ifdef SONAR
void taskUpdateSonar(void)
{
if (sensors(SENSOR_SONAR)) {
sonarUpdate();
}
}
#endif
#if defined(BARO) || defined(SONAR)
void taskCalculateAltitude(void)
{
if (false
#if defined(BARO)
|| (sensors(SENSOR_BARO) && isBaroReady())
#endif
#if defined(SONAR)
|| sensors(SENSOR_SONAR)
#endif
) {
calculateEstimatedAltitude(currentTime);
}}
#endif
#ifdef DISPLAY
void taskUpdateDisplay(void)
{
if (feature(FEATURE_DISPLAY)) {
updateDisplay();
}
}
#endif
#ifdef TELEMETRY
void taskTelemetry(void)
{
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
#endif
#ifdef LED_STRIP
void taskLedStrip(void)
{
if (feature(FEATURE_LED_STRIP)) {
updateLedStrip();
}
}
#endif

396
src/main/scheduler.c Executable file
View File

@ -0,0 +1,396 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include "platform.h"
#include "scheduler.h"
#include "debug.h"
#include "common/maths.h"
#include "drivers/system.h"
//#define SCHEDULER_DEBUG
cfTaskId_e currentTaskId = TASK_NONE;
static uint32_t totalWaitingTasks;
static uint32_t totalWaitingTasksSamples;
static uint32_t realtimeGuardInterval;
uint32_t currentTime = 0;
uint16_t averageWaitingTasks100 = 0;
typedef struct {
/* Configuration */
const char * taskName;
bool (*checkFunc)(uint32_t currentDeltaTime);
void (*taskFunc)(void);
bool isEnabled;
uint32_t desiredPeriod; // target period of execution
uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero
/* Scheduling */
uint8_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation
uint32_t lastExecutedAt; // last time of invocation
uint32_t lastSignaledAt; // time of invocation event for event-driven tasks
uint16_t taskAgeCycles;
/* Statistics */
uint32_t averageExecutionTime; // Moving averate over 6 samples, used to calculate guard interval
uint32_t taskLatestDeltaTime; //
#ifndef SKIP_TASK_STATISTICS
uint32_t maxExecutionTime;
uint32_t totalExecutionTime; // total time consumed by task since boot
#endif
} cfTask_t;
bool taskMainPidLoopCheck(uint32_t currentDeltaTime);
void taskMainPidLoop(void);
void taskUpdateAccelerometer(void);
void taskHandleSerial(void);
void taskUpdateBeeper(void);
void taskUpdateBattery(void);
bool taskUpdateRxCheck(uint32_t currentDeltaTime);
void taskUpdateRxMain(void);
void taskProcessGPS(void);
void taskUpdateCompass(void);
void taskUpdateBaro(void);
void taskUpdateSonar(void);
void taskCalculateAltitude(void);
void taskUpdateDisplay(void);
void taskTelemetry(void);
void taskLedStrip(void);
void taskSystem(void);
static cfTask_t cfTasks[TASK_COUNT] = {
[TASK_SYSTEM] = {
.isEnabled = true,
.taskName = "SYSTEM",
.taskFunc = taskSystem,
.desiredPeriod = 1000000 / 10, // run every 100 ms
.staticPriority = TASK_PRIORITY_HIGH,
},
[TASK_GYROPID] = {
.taskName = "GYRO/PID",
.checkFunc = taskMainPidLoopCheck,
.taskFunc = taskMainPidLoop,
.desiredPeriod = 1000,
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_ACCEL] = {
.taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer,
.desiredPeriod = 1000000 / 100,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
.desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
.staticPriority = TASK_PRIORITY_LOW,
},
[TASK_BEEPER] = {
.taskName = "BEEPER",
.taskFunc = taskUpdateBeeper,
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_BATTERY] = {
.taskName = "BATTERY",
.taskFunc = taskUpdateBattery,
.desiredPeriod = 1000000 / 50, // 50 Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_RX] = {
.taskName = "RX",
.checkFunc = taskUpdateRxCheck,
.taskFunc = taskUpdateRxMain,
.desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
.staticPriority = TASK_PRIORITY_HIGH,
},
#ifdef GPS
[TASK_GPS] = {
.taskName = "GPS",
.taskFunc = taskProcessGPS,
.desiredPeriod = 1000000 / 10, // GPS usually don't go raster than 10Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef MAG
[TASK_COMPASS] = {
.taskName = "COMPASS",
.taskFunc = taskUpdateCompass,
.desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef BARO
[TASK_BARO] = {
.taskName = "BARO",
.taskFunc = taskUpdateBaro,
.desiredPeriod = 1000000 / 20,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef SONAR
[TASK_SONAR] = {
.taskName = "SONAR",
.taskFunc = taskUpdateSonar,
.desiredPeriod = 1000000 / 20,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#if defined(BARO) || defined(SONAR)
[TASK_ALTITUDE] = {
.taskName = "ALTITUDE",
.taskFunc = taskCalculateAltitude,
.desiredPeriod = 1000000 / 40,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#ifdef DISPLAY
[TASK_DISPLAY] = {
.taskName = "DISPLAY",
.taskFunc = taskUpdateDisplay,
.desiredPeriod = 1000000 / 10,
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
#ifdef TELEMETRY
[TASK_TELEMETRY] = {
.taskName = "TELEMETRY",
.taskFunc = taskTelemetry,
.desiredPeriod = 1000000 / 250, // 250 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef LED_STRIP
[TASK_LEDSTRIP] = {
.taskName = "LEDSTRIP",
.taskFunc = taskLedStrip,
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
};
#define REALTIME_GUARD_INTERVAL_MIN 10
#define REALTIME_GUARD_INTERVAL_MAX 300
#define REALTIME_GUARD_INTERVAL_MARGIN 25
void taskSystem(void)
{
uint8_t taskId;
/* Calculate system load */
if (totalWaitingTasksSamples > 0) {
averageWaitingTasks100 = 100 * totalWaitingTasks / totalWaitingTasksSamples;
totalWaitingTasksSamples = 0;
totalWaitingTasks = 0;
}
/* Calculate guard interval */
uint32_t maxNonRealtimeTaskTime = 0;
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
}
}
realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
#if defined SCHEDULER_DEBUG
debug[2] = realtimeGuardInterval;
#endif
}
#ifndef SKIP_TASK_STATISTICS
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
{
taskInfo->taskName = cfTasks[taskId].taskName;
taskInfo->isEnabled= cfTasks[taskId].isEnabled;
taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
taskInfo->staticPriority = cfTasks[taskId].staticPriority;
taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
}
#endif
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
{
if (taskId == TASK_SELF)
taskId = currentTaskId;
if (taskId < TASK_COUNT) {
cfTasks[taskId].desiredPeriod = MAX(100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
}
}
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
{
if (taskId == TASK_SELF)
taskId = currentTaskId;
if (taskId < TASK_COUNT) {
cfTasks[taskId].isEnabled = newEnabledState;
}
}
uint32_t getTaskDeltaTime(cfTaskId_e taskId)
{
if (taskId == TASK_SELF)
taskId = currentTaskId;
if (taskId < TASK_COUNT) {
return cfTasks[taskId].taskLatestDeltaTime;
}
else {
return 0;
}
}
void scheduler(void)
{
uint8_t taskId;
uint8_t selectedTaskId;
uint8_t selectedTaskDynPrio;
uint16_t waitingTasks = 0;
uint32_t timeToNextRealtimeTask = UINT32_MAX;
/* Cache currentTime */
currentTime = micros();
/* The task to be invoked */
selectedTaskId = TASK_NONE;
selectedTaskDynPrio = 0;
/* Check for realtime tasks */
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
if (cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME) {
uint32_t nextExecuteAt = cfTasks[taskId].lastExecutedAt + cfTasks[taskId].desiredPeriod;
if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
timeToNextRealtimeTask = 0;
}
else {
uint32_t newTimeInterval = nextExecuteAt - currentTime;
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
}
}
}
bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
/* Update task dynamic priorities */
for (taskId = 0; taskId < TASK_COUNT; taskId++) {
if (cfTasks[taskId].isEnabled) {
/* Task has checkFunc - event driven */
if (cfTasks[taskId].checkFunc != NULL) {
/* Increase priority for event driven tasks */
if (cfTasks[taskId].dynamicPriority > 0) {
cfTasks[taskId].taskAgeCycles = 1 + ((currentTime - cfTasks[taskId].lastSignaledAt) / cfTasks[taskId].desiredPeriod);
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
waitingTasks++;
}
else if (cfTasks[taskId].checkFunc(currentTime - cfTasks[taskId].lastExecutedAt)) {
cfTasks[taskId].lastSignaledAt = currentTime;
cfTasks[taskId].taskAgeCycles = 1;
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority;
waitingTasks++;
}
else {
cfTasks[taskId].taskAgeCycles = 0;
}
}
/* Task is time-driven, dynamicPriority is last execution age measured in desiredPeriods) */
else {
// Task age is calculated from last execution
cfTasks[taskId].taskAgeCycles = ((currentTime - cfTasks[taskId].lastExecutedAt) / cfTasks[taskId].desiredPeriod);
if (cfTasks[taskId].taskAgeCycles > 0) {
cfTasks[taskId].dynamicPriority = 1 + cfTasks[taskId].staticPriority * cfTasks[taskId].taskAgeCycles;
waitingTasks++;
}
}
/* limit new priority to avoid overflow of uint8_t */
cfTasks[taskId].dynamicPriority = MIN(cfTasks[taskId].dynamicPriority, TASK_PRIORITY_MAX);;
bool taskCanBeChosenForScheduling =
(outsideRealtimeGuardInterval) ||
(cfTasks[taskId].taskAgeCycles > 1) ||
(cfTasks[taskId].staticPriority == TASK_PRIORITY_REALTIME);
if (taskCanBeChosenForScheduling && (cfTasks[taskId].dynamicPriority > selectedTaskDynPrio)) {
selectedTaskDynPrio = cfTasks[taskId].dynamicPriority;
selectedTaskId = taskId;
}
}
}
totalWaitingTasksSamples += 1;
totalWaitingTasks += waitingTasks;
/* Found a task that should be run */
if (selectedTaskId != TASK_NONE) {
cfTasks[selectedTaskId].taskLatestDeltaTime = currentTime - cfTasks[selectedTaskId].lastExecutedAt;
cfTasks[selectedTaskId].lastExecutedAt = currentTime;
cfTasks[selectedTaskId].dynamicPriority = 0;
currentTaskId = selectedTaskId;
uint32_t currentTimeBeforeTaskCall = micros();
/* Execute task */
if (cfTasks[selectedTaskId].taskFunc != NULL) {
cfTasks[selectedTaskId].taskFunc();
}
uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
#ifndef SKIP_TASK_STATISTICS
cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
#endif
#if defined SCHEDULER_DEBUG
debug[3] = (micros() - currentTime) - taskExecutionTime;
#endif
}
else {
currentTaskId = TASK_NONE;
#if defined SCHEDULER_DEBUG
debug[3] = (micros() - currentTime);
#endif
}
}

90
src/main/scheduler.h Executable file
View File

@ -0,0 +1,90 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef enum {
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_HIGH = 5,
TASK_PRIORITY_REALTIME = 6,
TASK_PRIORITY_MAX = 255
} cfTaskPriority_e;
typedef struct {
const char * taskName;
bool isEnabled;
uint32_t desiredPeriod;
uint8_t staticPriority;
uint32_t maxExecutionTime;
uint32_t totalExecutionTime;
uint32_t lastExecutionTime;
uint32_t averageExecutionTime;
} cfTaskInfo_t;
typedef enum {
/* Actual tasks */
TASK_SYSTEM = 0,
TASK_GYROPID,
TASK_ACCEL,
TASK_SERIAL,
TASK_BEEPER,
TASK_BATTERY,
TASK_RX,
#ifdef GPS
TASK_GPS,
#endif
#ifdef MAG
TASK_COMPASS,
#endif
#ifdef BARO
TASK_BARO,
#endif
#ifdef SONAR
TASK_SONAR,
#endif
#if defined(BARO) || defined(SONAR)
TASK_ALTITUDE,
#endif
#ifdef DISPLAY
TASK_DISPLAY,
#endif
#ifdef TELEMETRY
TASK_TELEMETRY,
#endif
#ifdef LED_STRIP
TASK_LEDSTRIP,
#endif
/* Count of real tasks */
TASK_COUNT,
/* Service task IDs */
TASK_NONE = TASK_COUNT,
TASK_SELF
} cfTaskId_e;
extern uint16_t cpuLoad;
extern uint16_t averageWaitingTasks100;
void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
void scheduler(void);

23
src/main/sensors/barometer.c
View File

@ -20,6 +20,7 @@
#include <math.h>
#include "platform.h"
#include "scheduler.h"
#include "common/maths.h"
@ -112,8 +113,7 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres
typedef enum {
BAROMETER_NEEDS_SAMPLES = 0,
BAROMETER_NEEDS_CALCULATION,
BAROMETER_NEEDS_PROCESSING
BAROMETER_NEEDS_CALCULATION
} barometerState_e;
@ -121,37 +121,28 @@ bool isBaroReady(void) {
return baroReady;
}
void baroUpdate(uint32_t currentTime)
uint32_t baroUpdate(void)
{
static uint32_t baroDeadline = 0;
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
if ((int32_t)(currentTime - baroDeadline) < 0)
return;
baroDeadline = 0;
switch (state) {
default:
case BAROMETER_NEEDS_SAMPLES:
baro.get_ut();
baro.start_up();
state = BAROMETER_NEEDS_CALCULATION;
baroDeadline += baro.up_delay;
return baro.up_delay;
break;
case BAROMETER_NEEDS_CALCULATION:
baro.get_up();
baro.start_ut();
baroDeadline += baro.ut_delay;
baro.calculate(&baroPressure, &baroTemperature);
state = BAROMETER_NEEDS_PROCESSING;
break;
case BAROMETER_NEEDS_PROCESSING:
state = BAROMETER_NEEDS_SAMPLES;
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.ut_delay;
break;
}
baroDeadline += micros(); // make sure deadline is set after calling baro callbacks
}
int32_t baroCalculateAltitude(void)

4
src/main/sensors/barometer.h
View File

@ -26,7 +26,7 @@ typedef enum {
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
#define BARO_MAX BARO_BMP280
#define BARO_MAX BARO_MS5611
typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array
@ -42,7 +42,7 @@ extern int32_t baroTemperature; // Use temperature for telemetry
void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void);
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void baroUpdate(uint32_t currentTime);
uint32_t baroUpdate(void);
bool isBaroReady(void);
int32_t baroCalculateAltitude(void);
void performBaroCalibrationCycle(void);

3
src/main/target/CJMCU/target.h
View File

@ -72,6 +72,9 @@
#if (FLASH_SIZE > 64)
#define BLACKBOX
#else
#define SKIP_TASK_STATISTICS
#define SKIP_CLI_COMMAND_HELP
#endif
//#undef USE_CLI