diff --git a/Makefile b/Makefile index b162a2f130..0638ea1c47 100755 --- a/Makefile +++ b/Makefile @@ -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 \ diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index e8ccfec926..3677fbb287 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 27d5f4a0f7..b6f49406e9 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -20,15 +20,15 @@ #include #include #include -#include #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) { /* diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9c086b04f5..dd06e45ff6 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index fab21bbf83..1973c8f8e9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -24,6 +24,7 @@ #include #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 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); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 12fa52f969..d10399a1d4 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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; diff --git a/src/main/main.c b/src/main/main.c index ac19280cd0..f80327ba16 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -21,6 +21,7 @@ #include #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(); } } diff --git a/src/main/mw.c b/src/main/mw.c index 8461bf0b71..80c2157c08 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -21,12 +21,14 @@ #include #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(¤tProfile->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( - ¤tProfile->pidProfile, - currentControlRateProfile, - masterConfig.max_angle_inclination, - ¤tProfile->accelerometerTrims, - &masterConfig.rxConfig - ); + // PID - note this is function pointer set by setPIDController() + pid_controller( + ¤tProfile->pidProfile, + currentControlRateProfile, + masterConfig.max_angle_inclination, + ¤tProfile->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(¤tProfile->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(¤tProfile->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 diff --git a/src/main/scheduler.c b/src/main/scheduler.c new file mode 100755 index 0000000000..ea2ab14148 --- /dev/null +++ b/src/main/scheduler.c @@ -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 . + */ + +#include +#include +#include + +#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 + } +} diff --git a/src/main/scheduler.h b/src/main/scheduler.h new file mode 100755 index 0000000000..7e1e5062bc --- /dev/null +++ b/src/main/scheduler.h @@ -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 . + */ + +#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); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a4281f800b..e4ad51c3a9 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -20,6 +20,7 @@ #include #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) diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 1987a7c6b0..7ea2c1e5ba 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -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); diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 4c0649f380..cf32393b42 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -72,6 +72,9 @@ #if (FLASH_SIZE > 64) #define BLACKBOX +#else +#define SKIP_TASK_STATISTICS +#define SKIP_CLI_COMMAND_HELP #endif //#undef USE_CLI