Reorganisation of tasks into fc_tasks.c
This commit is contained in:
parent
92d2e3ae91
commit
0ccb7040f0
2
Makefile
2
Makefile
|
@ -397,6 +397,7 @@ COMMON_SRC = \
|
||||||
$(TARGET_DIR_SRC) \
|
$(TARGET_DIR_SRC) \
|
||||||
main.c \
|
main.c \
|
||||||
fc/mw.c \
|
fc/mw.c \
|
||||||
|
fc/fc_tasks.c \
|
||||||
common/encoding.c \
|
common/encoding.c \
|
||||||
common/filter.c \
|
common/filter.c \
|
||||||
common/maths.c \
|
common/maths.c \
|
||||||
|
@ -463,7 +464,6 @@ COMMON_SRC = \
|
||||||
rx/sumh.c \
|
rx/sumh.c \
|
||||||
rx/xbus.c \
|
rx/xbus.c \
|
||||||
scheduler/scheduler.c \
|
scheduler/scheduler.c \
|
||||||
scheduler/scheduler_tasks.c \
|
|
||||||
sensors/acceleration.c \
|
sensors/acceleration.c \
|
||||||
sensors/battery.c \
|
sensors/battery.c \
|
||||||
sensors/boardalignment.c \
|
sensors/boardalignment.c \
|
||||||
|
|
|
@ -0,0 +1,485 @@
|
||||||
|
/*
|
||||||
|
* 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 "common/axis.h"
|
||||||
|
#include "common/color.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/sensor.h"
|
||||||
|
#include "drivers/accgyro.h"
|
||||||
|
#include "drivers/compass.h"
|
||||||
|
#include "drivers/serial.h"
|
||||||
|
|
||||||
|
#include "fc/fc_tasks.h"
|
||||||
|
#include "fc/mw.h"
|
||||||
|
#include "fc/rc_controls.h"
|
||||||
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
|
#include "flight/pid.h"
|
||||||
|
|
||||||
|
#include "io/beeper.h"
|
||||||
|
#include "io/display.h"
|
||||||
|
#include "io/gps.h"
|
||||||
|
#include "io/ledstrip.h"
|
||||||
|
#include "io/osd.h"
|
||||||
|
#include "io/serial.h"
|
||||||
|
#include "io/serial_cli.h"
|
||||||
|
#include "io/transponder_ir.h"
|
||||||
|
|
||||||
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
#include "telemetry/telemetry.h"
|
||||||
|
|
||||||
|
#include "sensors/sensors.h"
|
||||||
|
#include "sensors/acceleration.h"
|
||||||
|
#include "sensors/barometer.h"
|
||||||
|
#include "sensors/battery.h"
|
||||||
|
#include "sensors/compass.h"
|
||||||
|
#include "sensors/gyro.h"
|
||||||
|
#include "sensors/sonar.h"
|
||||||
|
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
|
#include "config/config.h"
|
||||||
|
#include "config/feature.h"
|
||||||
|
#include "config/config_profile.h"
|
||||||
|
#include "config/config_master.h"
|
||||||
|
|
||||||
|
|
||||||
|
/* VBAT monitoring interval (in microseconds) - 1s*/
|
||||||
|
#define VBATINTERVAL (6 * 3500)
|
||||||
|
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
|
||||||
|
#define IBATINTERVAL (6 * 3500)
|
||||||
|
|
||||||
|
|
||||||
|
cfTask_t cfTasks[TASK_COUNT] = {
|
||||||
|
[TASK_SYSTEM] = {
|
||||||
|
.taskName = "SYSTEM",
|
||||||
|
.taskFunc = taskSystem,
|
||||||
|
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
||||||
|
.staticPriority = TASK_PRIORITY_HIGH,
|
||||||
|
},
|
||||||
|
|
||||||
|
[TASK_GYROPID] = {
|
||||||
|
.taskName = "PID",
|
||||||
|
.subTaskName = "GYRO",
|
||||||
|
.taskFunc = taskMainPidLoopCheck,
|
||||||
|
.desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
|
||||||
|
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||||
|
},
|
||||||
|
|
||||||
|
[TASK_ACCEL] = {
|
||||||
|
.taskName = "ACCEL",
|
||||||
|
.taskFunc = taskUpdateAccelerometer,
|
||||||
|
.desiredPeriod = 1000000 / 1000, // every 1ms
|
||||||
|
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||||
|
},
|
||||||
|
|
||||||
|
[TASK_ATTITUDE] = {
|
||||||
|
.taskName = "ATTITUDE",
|
||||||
|
.taskFunc = taskUpdateAttitude,
|
||||||
|
.desiredPeriod = 1000000 / 100,
|
||||||
|
.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,
|
||||||
|
},
|
||||||
|
|
||||||
|
[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_BATTERY] = {
|
||||||
|
.taskName = "BATTERY",
|
||||||
|
.taskFunc = taskUpdateBattery,
|
||||||
|
.desiredPeriod = 1000000 / 50, // 50 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||||
|
},
|
||||||
|
|
||||||
|
#ifdef BEEPER
|
||||||
|
[TASK_BEEPER] = {
|
||||||
|
.taskName = "BEEPER",
|
||||||
|
.taskFunc = taskUpdateBeeper,
|
||||||
|
.desiredPeriod = 1000000 / 100, // 100 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#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_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef BARO
|
||||||
|
[TASK_BARO] = {
|
||||||
|
.taskName = "BARO",
|
||||||
|
.taskFunc = taskUpdateBaro,
|
||||||
|
.desiredPeriod = 1000000 / 20,
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
[TASK_SONAR] = {
|
||||||
|
.taskName = "SONAR",
|
||||||
|
.taskFunc = taskUpdateSonar,
|
||||||
|
.desiredPeriod = 1000000 / 20,
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(BARO) || defined(SONAR)
|
||||||
|
[TASK_ALTITUDE] = {
|
||||||
|
.taskName = "ALTITUDE",
|
||||||
|
.taskFunc = taskCalculateAltitude,
|
||||||
|
.desiredPeriod = 1000000 / 40,
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef TRANSPONDER
|
||||||
|
[TASK_TRANSPONDER] = {
|
||||||
|
.taskName = "TRANSPONDER",
|
||||||
|
.taskFunc = taskTransponder,
|
||||||
|
.desiredPeriod = 1000000 / 250, // 250 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef DISPLAY
|
||||||
|
[TASK_DISPLAY] = {
|
||||||
|
.taskName = "DISPLAY",
|
||||||
|
.taskFunc = taskUpdateDisplay,
|
||||||
|
.desiredPeriod = 1000000 / 10,
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
#ifdef OSD
|
||||||
|
[TASK_OSD] = {
|
||||||
|
.taskName = "OSD",
|
||||||
|
.taskFunc = taskUpdateOsd,
|
||||||
|
.desiredPeriod = 1000000 / 60, // 60 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
#ifdef TELEMETRY
|
||||||
|
[TASK_TELEMETRY] = {
|
||||||
|
.taskName = "TELEMETRY",
|
||||||
|
.taskFunc = taskTelemetry,
|
||||||
|
.desiredPeriod = 1000000 / 250, // 250 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef LED_STRIP
|
||||||
|
[TASK_LEDSTRIP] = {
|
||||||
|
.taskName = "LEDSTRIP",
|
||||||
|
.taskFunc = taskLedStrip,
|
||||||
|
.desiredPeriod = 1000000 / 100, // 100 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_LOW,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_BST
|
||||||
|
[TASK_BST_MASTER_PROCESS] = {
|
||||||
|
.taskName = "BST_MASTER_PROCESS",
|
||||||
|
.taskFunc = taskBstMasterProcess,
|
||||||
|
.desiredPeriod = 1000000 / 50, // 50 Hz
|
||||||
|
.staticPriority = TASK_PRIORITY_IDLE,
|
||||||
|
},
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
void taskUpdateAccelerometer(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
UNUSED(currentTime);
|
||||||
|
|
||||||
|
imuUpdateAccelerometer(&masterConfig.accelerometerTrims);
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskUpdateAttitude(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
imuUpdateAttitude(currentTime);
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskHandleSerial(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
UNUSED(currentTime);
|
||||||
|
|
||||||
|
handleSerial();
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskUpdateBeeper(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
beeperUpdate(currentTime); //call periodic beeper handler
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskUpdateBattery(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
#ifdef USE_ADC
|
||||||
|
static uint32_t vbatLastServiced = 0;
|
||||||
|
if (feature(FEATURE_VBAT)) {
|
||||||
|
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
|
||||||
|
vbatLastServiced = currentTime;
|
||||||
|
updateBattery();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static uint32_t ibatLastServiced = 0;
|
||||||
|
if (feature(FEATURE_CURRENT_METER)) {
|
||||||
|
const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
|
||||||
|
|
||||||
|
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
|
||||||
|
ibatLastServiced = currentTime;
|
||||||
|
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime)
|
||||||
|
{
|
||||||
|
UNUSED(currentDeltaTime);
|
||||||
|
return rxUpdate(currentTime);
|
||||||
|
}
|
||||||
|
|
||||||
|
void taskUpdateRxMain(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
processRx(currentTime);
|
||||||
|
isRXDataNew = true;
|
||||||
|
|
||||||
|
#if !defined(BARO) && !defined(SONAR)
|
||||||
|
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
|
||||||
|
updateRcCommands();
|
||||||
|
#endif
|
||||||
|
updateLEDs();
|
||||||
|
|
||||||
|
#ifdef BARO
|
||||||
|
if (sensors(SENSOR_BARO)) {
|
||||||
|
updateAltHoldState();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
if (sensors(SENSOR_SONAR)) {
|
||||||
|
updateSonarAltHoldState();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef GPS
|
||||||
|
void taskProcessGPS(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
// 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(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (sensors(SENSOR_MAG)) {
|
||||||
|
updateCompass(currentTime, &masterConfig.magZero);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef BARO
|
||||||
|
void taskUpdateBaro(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
UNUSED(currentTime);
|
||||||
|
|
||||||
|
if (sensors(SENSOR_BARO)) {
|
||||||
|
const uint32_t newDeadline = baroUpdate();
|
||||||
|
if (newDeadline != 0) {
|
||||||
|
rescheduleTask(TASK_SELF, newDeadline);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
void taskUpdateSonar(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
UNUSED(currentTime);
|
||||||
|
|
||||||
|
if (sensors(SENSOR_SONAR)) {
|
||||||
|
sonarUpdate();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(BARO) || defined(SONAR)
|
||||||
|
void taskCalculateAltitude(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (false
|
||||||
|
#if defined(BARO)
|
||||||
|
|| (sensors(SENSOR_BARO) && isBaroReady())
|
||||||
|
#endif
|
||||||
|
#if defined(SONAR)
|
||||||
|
|| sensors(SENSOR_SONAR)
|
||||||
|
#endif
|
||||||
|
) {
|
||||||
|
calculateEstimatedAltitude(currentTime);
|
||||||
|
}}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef DISPLAY
|
||||||
|
void taskUpdateDisplay(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (feature(FEATURE_DISPLAY)) {
|
||||||
|
updateDisplay(currentTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef TELEMETRY
|
||||||
|
void taskTelemetry(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
telemetryCheckState();
|
||||||
|
|
||||||
|
if (!cliMode && feature(FEATURE_TELEMETRY)) {
|
||||||
|
telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef LED_STRIP
|
||||||
|
void taskLedStrip(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (feature(FEATURE_LED_STRIP)) {
|
||||||
|
updateLedStrip(currentTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef TRANSPONDER
|
||||||
|
void taskTransponder(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (feature(FEATURE_TRANSPONDER)) {
|
||||||
|
updateTransponder(currentTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef OSD
|
||||||
|
void taskUpdateOsd(uint32_t currentTime)
|
||||||
|
{
|
||||||
|
if (feature(FEATURE_OSD)) {
|
||||||
|
updateOsd(currentTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void fcTasksInit(void)
|
||||||
|
{
|
||||||
|
schedulerInit();
|
||||||
|
rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait
|
||||||
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
|
if (sensors(SENSOR_ACC)) {
|
||||||
|
setTaskEnabled(TASK_ACCEL, true);
|
||||||
|
rescheduleTask(TASK_ACCEL, accSamplingInterval);
|
||||||
|
}
|
||||||
|
|
||||||
|
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
|
||||||
|
setTaskEnabled(TASK_SERIAL, true);
|
||||||
|
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
|
||||||
|
setTaskEnabled(TASK_RX, true);
|
||||||
|
|
||||||
|
#ifdef BEEPER
|
||||||
|
setTaskEnabled(TASK_BEEPER, true);
|
||||||
|
#endif
|
||||||
|
#ifdef GPS
|
||||||
|
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
|
||||||
|
#endif
|
||||||
|
#ifdef MAG
|
||||||
|
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
|
||||||
|
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
|
||||||
|
// fixme temporary solution for AK6983 via slave I2C on MPU9250
|
||||||
|
rescheduleTask(TASK_COMPASS, 1000000 / 40);
|
||||||
|
#endif
|
||||||
|
#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));
|
||||||
|
// Reschedule telemetry to 500hz for Jeti Exbus
|
||||||
|
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
|
||||||
|
rescheduleTask(TASK_TELEMETRY, 2000);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#ifdef LED_STRIP
|
||||||
|
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
|
||||||
|
#endif
|
||||||
|
#ifdef TRANSPONDER
|
||||||
|
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
|
||||||
|
#endif
|
||||||
|
#ifdef OSD
|
||||||
|
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
|
||||||
|
#endif
|
||||||
|
#ifdef USE_BST
|
||||||
|
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
|
@ -43,3 +43,8 @@ void taskBstReadWrite(uint32_t currentTime);
|
||||||
void taskBstMasterProcess(uint32_t currentTime);
|
void taskBstMasterProcess(uint32_t currentTime);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
|
||||||
|
|
||||||
|
|
||||||
|
void fcTasksInit(void);
|
||||||
|
|
|
@ -16,17 +16,16 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#include "blackbox/blackbox.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
|
||||||
|
@ -35,54 +34,39 @@
|
||||||
#include "drivers/compass.h"
|
#include "drivers/compass.h"
|
||||||
#include "drivers/light_led.h"
|
#include "drivers/light_led.h"
|
||||||
|
|
||||||
#include "drivers/gpio.h"
|
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
|
||||||
#include "drivers/gyro_sync.h"
|
#include "drivers/gyro_sync.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
#include "sensors/sonar.h"
|
|
||||||
#include "sensors/compass.h"
|
|
||||||
#include "sensors/acceleration.h"
|
#include "sensors/acceleration.h"
|
||||||
#include "sensors/barometer.h"
|
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
|
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
#include "io/display.h"
|
|
||||||
#include "io/motors.h"
|
#include "io/motors.h"
|
||||||
#include "io/servos.h"
|
#include "io/servos.h"
|
||||||
|
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/rc_curves.h"
|
#include "fc/rc_curves.h"
|
||||||
#include "io/gimbal.h"
|
|
||||||
#include "io/gps.h"
|
|
||||||
#include "io/ledstrip.h"
|
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
#include "io/serial_cli.h"
|
#include "io/serial_cli.h"
|
||||||
#include "io/serial_msp.h"
|
#include "io/serial_msp.h"
|
||||||
#include "io/statusindicator.h"
|
#include "io/statusindicator.h"
|
||||||
#include "io/asyncfatfs/asyncfatfs.h"
|
|
||||||
#include "io/transponder_ir.h"
|
#include "io/transponder_ir.h"
|
||||||
#include "io/osd.h"
|
#include "io/asyncfatfs/asyncfatfs.h"
|
||||||
|
|
||||||
#include "io/vtx.h"
|
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/msp.h"
|
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "scheduler/scheduler.h"
|
||||||
#include "blackbox/blackbox.h"
|
|
||||||
|
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
|
||||||
#include "flight/altitudehold.h"
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/gtune.h"
|
#include "flight/gtune.h"
|
||||||
#include "flight/navigation.h"
|
|
||||||
|
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
@ -90,9 +74,6 @@
|
||||||
#include "config/config_master.h"
|
#include "config/config_master.h"
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
|
||||||
#include "scheduler/scheduler_tasks.h"
|
|
||||||
|
|
||||||
// June 2013 V2.2-dev
|
// June 2013 V2.2-dev
|
||||||
|
|
||||||
enum {
|
enum {
|
||||||
|
@ -101,10 +82,6 @@ enum {
|
||||||
ALIGN_MAG = 2
|
ALIGN_MAG = 2
|
||||||
};
|
};
|
||||||
|
|
||||||
/* VBAT monitoring interval (in microseconds) - 1s*/
|
|
||||||
#define VBATINTERVAL (6 * 3500)
|
|
||||||
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
|
|
||||||
#define IBATINTERVAL (6 * 3500)
|
|
||||||
|
|
||||||
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
|
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
|
||||||
|
|
||||||
|
@ -122,7 +99,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
|
||||||
|
|
||||||
extern uint8_t PIDweight[3];
|
extern uint8_t PIDweight[3];
|
||||||
|
|
||||||
static bool isRXDataNew;
|
uint16_t filteredCycleTime;
|
||||||
|
bool isRXDataNew;
|
||||||
static bool armingCalibrationWasInitialised;
|
static bool armingCalibrationWasInitialised;
|
||||||
float setpointRate[3];
|
float setpointRate[3];
|
||||||
float rcInput[3];
|
float rcInput[3];
|
||||||
|
@ -297,7 +275,7 @@ void processRcCommand(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void updateRcCommands(void)
|
void updateRcCommands(void)
|
||||||
{
|
{
|
||||||
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
|
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
|
||||||
int32_t prop;
|
int32_t prop;
|
||||||
|
@ -361,7 +339,7 @@ static void updateRcCommands(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void updateLEDs(void)
|
void updateLEDs(void)
|
||||||
{
|
{
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
LED0_ON;
|
LED0_ON;
|
||||||
|
@ -869,6 +847,7 @@ void taskMainPidLoopCheck(uint32_t currentTime)
|
||||||
runTaskMainSubprocesses = true;
|
runTaskMainSubprocesses = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
<<<<<<< 92d2e3ae91522c306728193a386d350c612249cc
|
||||||
|
|
||||||
void taskUpdateAccelerometer(uint32_t currentTime)
|
void taskUpdateAccelerometer(uint32_t currentTime)
|
||||||
{
|
{
|
||||||
|
@ -1064,3 +1043,5 @@ void taskUpdateOsd(uint32_t currentTime)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
=======
|
||||||
|
>>>>>>> Reorganisation of tasks into fc_tasks.c
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
extern int16_t magHold;
|
extern int16_t magHold;
|
||||||
|
extern bool isRXDataNew;
|
||||||
|
|
||||||
union rollAndPitchTrims_u;
|
union rollAndPitchTrims_u;
|
||||||
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
|
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
|
||||||
|
@ -25,3 +26,8 @@ void handleInflightCalibrationStickPosition();
|
||||||
|
|
||||||
void mwDisarm(void);
|
void mwDisarm(void);
|
||||||
void mwArm(void);
|
void mwArm(void);
|
||||||
|
|
||||||
|
void processRx(uint32_t currentTime);
|
||||||
|
void updateLEDs(void);
|
||||||
|
void updateRcCommands(void);
|
||||||
|
|
||||||
|
|
|
@ -62,6 +62,9 @@
|
||||||
#include "bus_bst.h"
|
#include "bus_bst.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#include "fc/fc_tasks.h"
|
||||||
|
#include "fc/rc_controls.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/spektrum.h"
|
#include "rx/spektrum.h"
|
||||||
|
|
||||||
|
@ -111,8 +114,6 @@
|
||||||
#include "config/config_master.h"
|
#include "config/config_master.h"
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
|
|
||||||
|
|
||||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||||
#include "hardware_revision.h"
|
#include "hardware_revision.h"
|
||||||
#endif
|
#endif
|
||||||
|
@ -658,6 +659,7 @@ void init(void)
|
||||||
latchActiveFeatures();
|
latchActiveFeatures();
|
||||||
motorControlEnable = true;
|
motorControlEnable = true;
|
||||||
|
|
||||||
|
fcTasksInit();
|
||||||
systemState |= SYSTEM_STATE_READY;
|
systemState |= SYSTEM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -675,67 +677,6 @@ void processLoopback(void) {
|
||||||
#define processLoopback()
|
#define processLoopback()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void main_init(void)
|
|
||||||
{
|
|
||||||
init();
|
|
||||||
|
|
||||||
/* Setup scheduler */
|
|
||||||
schedulerInit();
|
|
||||||
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
|
||||||
|
|
||||||
if (sensors(SENSOR_ACC)) {
|
|
||||||
setTaskEnabled(TASK_ACCEL, true);
|
|
||||||
rescheduleTask(TASK_ACCEL, accSamplingInterval);
|
|
||||||
}
|
|
||||||
|
|
||||||
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
|
|
||||||
setTaskEnabled(TASK_SERIAL, true);
|
|
||||||
#ifdef BEEPER
|
|
||||||
setTaskEnabled(TASK_BEEPER, true);
|
|
||||||
#endif
|
|
||||||
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));
|
|
||||||
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
|
|
||||||
// fixme temporary solution for AK6983 via slave I2C on MPU9250
|
|
||||||
rescheduleTask(TASK_COMPASS, 1000000 / 40);
|
|
||||||
#endif
|
|
||||||
#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));
|
|
||||||
// Reschedule telemetry to 500hz for Jeti Exbus
|
|
||||||
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000);
|
|
||||||
#endif
|
|
||||||
#ifdef LED_STRIP
|
|
||||||
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
|
|
||||||
#endif
|
|
||||||
#ifdef TRANSPONDER
|
|
||||||
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
|
|
||||||
#endif
|
|
||||||
#ifdef OSD
|
|
||||||
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
|
|
||||||
#endif
|
|
||||||
#ifdef USE_BST
|
|
||||||
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void main_step(void)
|
void main_step(void)
|
||||||
{
|
{
|
||||||
|
@ -746,8 +687,8 @@ void main_step(void)
|
||||||
#ifndef NOMAIN
|
#ifndef NOMAIN
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
main_init();
|
init();
|
||||||
while(1) {
|
while (true) {
|
||||||
main_step();
|
main_step();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -27,7 +27,6 @@
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
#include "scheduler/scheduler_tasks.h"
|
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
|
|
@ -1,184 +0,0 @@
|
||||||
/*
|
|
||||||
* 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/scheduler.h"
|
|
||||||
#include "scheduler/scheduler_tasks.h"
|
|
||||||
|
|
||||||
cfTask_t cfTasks[TASK_COUNT] = {
|
|
||||||
[TASK_SYSTEM] = {
|
|
||||||
.taskName = "SYSTEM",
|
|
||||||
.taskFunc = taskSystem,
|
|
||||||
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
|
||||||
.staticPriority = TASK_PRIORITY_HIGH,
|
|
||||||
},
|
|
||||||
|
|
||||||
[TASK_GYROPID] = {
|
|
||||||
.taskName = "PID",
|
|
||||||
.subTaskName = "GYRO",
|
|
||||||
.taskFunc = taskMainPidLoopCheck,
|
|
||||||
.desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
|
|
||||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
|
||||||
},
|
|
||||||
|
|
||||||
[TASK_ACCEL] = {
|
|
||||||
.taskName = "ACCEL",
|
|
||||||
.taskFunc = taskUpdateAccelerometer,
|
|
||||||
.desiredPeriod = 1000000 / 1000, // every 1ms
|
|
||||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
|
||||||
},
|
|
||||||
|
|
||||||
[TASK_ATTITUDE] = {
|
|
||||||
.taskName = "ATTITUDE",
|
|
||||||
.taskFunc = taskUpdateAttitude,
|
|
||||||
.desiredPeriod = 1000000 / 100,
|
|
||||||
.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,
|
|
||||||
},
|
|
||||||
|
|
||||||
[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_BATTERY] = {
|
|
||||||
.taskName = "BATTERY",
|
|
||||||
.taskFunc = taskUpdateBattery,
|
|
||||||
.desiredPeriod = 1000000 / 50, // 50 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
|
||||||
},
|
|
||||||
|
|
||||||
#ifdef BEEPER
|
|
||||||
[TASK_BEEPER] = {
|
|
||||||
.taskName = "BEEPER",
|
|
||||||
.taskFunc = taskUpdateBeeper,
|
|
||||||
.desiredPeriod = 1000000 / 100, // 100 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#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_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef BARO
|
|
||||||
[TASK_BARO] = {
|
|
||||||
.taskName = "BARO",
|
|
||||||
.taskFunc = taskUpdateBaro,
|
|
||||||
.desiredPeriod = 1000000 / 20,
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef SONAR
|
|
||||||
[TASK_SONAR] = {
|
|
||||||
.taskName = "SONAR",
|
|
||||||
.taskFunc = taskUpdateSonar,
|
|
||||||
.desiredPeriod = 1000000 / 20,
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(BARO) || defined(SONAR)
|
|
||||||
[TASK_ALTITUDE] = {
|
|
||||||
.taskName = "ALTITUDE",
|
|
||||||
.taskFunc = taskCalculateAltitude,
|
|
||||||
.desiredPeriod = 1000000 / 40,
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef TRANSPONDER
|
|
||||||
[TASK_TRANSPONDER] = {
|
|
||||||
.taskName = "TRANSPONDER",
|
|
||||||
.taskFunc = taskTransponder,
|
|
||||||
.desiredPeriod = 1000000 / 250, // 250 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef DISPLAY
|
|
||||||
[TASK_DISPLAY] = {
|
|
||||||
.taskName = "DISPLAY",
|
|
||||||
.taskFunc = taskUpdateDisplay,
|
|
||||||
.desiredPeriod = 1000000 / 10,
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
#ifdef OSD
|
|
||||||
[TASK_OSD] = {
|
|
||||||
.taskName = "OSD",
|
|
||||||
.taskFunc = taskUpdateOsd,
|
|
||||||
.desiredPeriod = 1000000 / 60, // 60 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
#ifdef TELEMETRY
|
|
||||||
[TASK_TELEMETRY] = {
|
|
||||||
.taskName = "TELEMETRY",
|
|
||||||
.taskFunc = taskTelemetry,
|
|
||||||
.desiredPeriod = 1000000 / 250, // 250 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef LED_STRIP
|
|
||||||
[TASK_LEDSTRIP] = {
|
|
||||||
.taskName = "LEDSTRIP",
|
|
||||||
.taskFunc = taskLedStrip,
|
|
||||||
.desiredPeriod = 1000000 / 100, // 100 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_LOW,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_BST
|
|
||||||
[TASK_BST_MASTER_PROCESS] = {
|
|
||||||
.taskName = "BST_MASTER_PROCESS",
|
|
||||||
.taskFunc = taskBstMasterProcess,
|
|
||||||
.desiredPeriod = 1000000 / 50, // 50 Hz
|
|
||||||
.staticPriority = TASK_PRIORITY_IDLE,
|
|
||||||
},
|
|
||||||
#endif
|
|
||||||
};
|
|
Loading…
Reference in New Issue