2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
27 #include "build/debug.h"
33 #include "common/color.h"
34 #include "common/utils.h"
36 #include "config/feature.h"
38 #include "drivers/accgyro/accgyro.h"
39 #include "drivers/camera_control.h"
40 #include "drivers/compass/compass.h"
41 #include "drivers/sensor.h"
42 #include "drivers/serial.h"
43 #include "drivers/serial_usb_vcp.h"
44 #include "drivers/stack_check.h"
45 #include "drivers/transponder_ir.h"
46 #include "drivers/usb_io.h"
47 #include "drivers/vtx_common.h"
49 #include "config/config.h"
52 #include "fc/dispatch.h"
53 #include "fc/rc_controls.h"
54 #include "fc/runtime_config.h"
56 #include "flight/position.h"
57 #include "flight/imu.h"
58 #include "flight/mixer.h"
59 #include "flight/pid.h"
61 #include "io/asyncfatfs/asyncfatfs.h"
62 #include "io/beeper.h"
63 #include "io/dashboard.h"
65 #include "io/ledstrip.h"
66 #include "io/piniobox.h"
67 #include "io/serial.h"
68 #include "io/transponder_ir.h"
69 #include "io/vtx_tramp.h" // Will be gone
70 #include "io/rcdevice_cam.h"
71 #include "io/usb_cdc_hid.h"
75 #include "msp/msp_serial.h"
84 #include "scheduler/scheduler.h"
86 #include "sensors/acceleration.h"
87 #include "sensors/adcinternal.h"
88 #include "sensors/barometer.h"
89 #include "sensors/battery.h"
90 #include "sensors/compass.h"
91 #include "sensors/esc_sensor.h"
92 #include "sensors/gyro.h"
93 #include "sensors/sensors.h"
94 #include "sensors/rangefinder.h"
96 #include "telemetry/telemetry.h"
97 #include "telemetry/crsf.h"
103 #ifdef USE_USB_CDC_HID
104 //TODO: Make it platform independent in the future
106 #include "vcpf4/usbd_cdc_vcp.h"
107 #include "usbd_hid_core.h"
108 #elif defined(STM32F7)
109 #include "usbd_cdc_interface.h"
110 #include "usbd_hid.h"
116 // taskUpdateRxMain() has occasional peaks in execution time so normal moving average duration estimation doesn't work
117 // Decay the estimated max task duration by 1/(1 << RX_TASK_DECAY_SHIFT) on every invocation
118 #define RX_TASK_DECAY_SHIFT 5
119 // Add a margin to the task duration estimation
120 #define RX_TASK_MARGIN 1
122 static void taskMain(timeUs_t currentTimeUs
)
124 UNUSED(currentTimeUs
);
131 static void taskHandleSerial(timeUs_t currentTimeUs
)
133 UNUSED(currentTimeUs
);
136 DEBUG_SET(DEBUG_USB
, 0, usbCableIsInserted());
137 DEBUG_SET(DEBUG_USB
, 1, usbVcpIsConnected());
141 // in cli mode, all serial stuff goes to here. enter cli mode by sending #
147 bool evaluateMspData
= ARMING_FLAG(ARMED
) ? MSP_SKIP_NON_MSP_DATA
: MSP_EVALUATE_NON_MSP_DATA
;
148 mspSerialProcess(evaluateMspData
, mspFcProcessCommand
, mspFcProcessReply
);
151 static void taskBatteryAlerts(timeUs_t currentTimeUs
)
153 if (!ARMING_FLAG(ARMED
)) {
154 // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup.
155 batteryUpdatePresence();
157 batteryUpdateStates(currentTimeUs
);
158 batteryUpdateAlarms();
162 static void taskUpdateAccelerometer(timeUs_t currentTimeUs
)
164 accUpdate(currentTimeUs
, &accelerometerConfigMutable()->accelerometerTrims
);
176 static rxState_e rxState
= RX_STATE_CHECK
;
178 bool taskUpdateRxMainInProgress()
180 return (rxState
!= RX_STATE_CHECK
);
183 static void taskUpdateRxMain(timeUs_t currentTimeUs
)
185 static timeUs_t rxStateDurationFracUs
[RX_STATE_COUNT
];
186 timeUs_t executeTimeUs
;
187 rxState_e oldRxState
= rxState
;
189 // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
190 if (rxState
!= RX_STATE_UPDATE
) {
191 schedulerIgnoreTaskExecRate();
197 rxState
= RX_STATE_PROCESS
;
200 case RX_STATE_PROCESS
:
201 if (!processRx(currentTimeUs
)) {
202 rxState
= RX_STATE_CHECK
;
205 rxState
= RX_STATE_MODES
;
209 processRxModes(currentTimeUs
);
210 rxState
= RX_STATE_UPDATE
;
213 case RX_STATE_UPDATE
:
214 // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
216 updateArmingStatus();
218 #ifdef USE_USB_CDC_HID
219 if (!ARMING_FLAG(ARMED
)) {
223 rxState
= RX_STATE_CHECK
;
227 if (schedulerGetIgnoreTaskExecTime()) {
231 executeTimeUs
= micros() - currentTimeUs
+ RX_TASK_MARGIN
;
233 if (executeTimeUs
> (rxStateDurationFracUs
[oldRxState
] >> RX_TASK_DECAY_SHIFT
)) {
234 rxStateDurationFracUs
[oldRxState
] = executeTimeUs
<< RX_TASK_DECAY_SHIFT
;
236 // Slowly decay the max time
237 rxStateDurationFracUs
[oldRxState
]--;
240 schedulerSetNextStateTime(rxStateDurationFracUs
[rxState
] >> RX_TASK_DECAY_SHIFT
);
245 static void taskUpdateBaro(timeUs_t currentTimeUs
)
247 UNUSED(currentTimeUs
);
249 if (sensors(SENSOR_BARO
)) {
250 const uint32_t newDeadline
= baroUpdate(currentTimeUs
);
251 if (newDeadline
!= 0) {
252 rescheduleTask(TASK_SELF
, newDeadline
);
258 #if defined(USE_RANGEFINDER)
259 void taskUpdateRangefinder(timeUs_t currentTimeUs
)
261 UNUSED(currentTimeUs
);
263 if (!sensors(SENSOR_RANGEFINDER
)) {
269 rangefinderProcess(getCosTiltAngle());
273 #if defined(USE_BARO) || defined(USE_GPS)
274 static void taskCalculateAltitude(timeUs_t currentTimeUs
)
276 calculateEstimatedAltitude(currentTimeUs
);
278 #endif // USE_BARO || USE_GPS
281 static void taskTelemetry(timeUs_t currentTimeUs
)
283 if (!cliMode
&& featureIsEnabled(FEATURE_TELEMETRY
)) {
284 subTaskTelemetryPollSensors(currentTimeUs
);
286 telemetryProcess(currentTimeUs
);
291 #ifdef USE_CAMERA_CONTROL
292 static void taskCameraControl(uint32_t currentTime
)
294 if (ARMING_FLAG(ARMED
)) {
298 cameraControlProcess(currentTime
);
302 #define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \
303 .taskName = taskNameParam, \
304 .subTaskName = subTaskNameParam, \
305 .checkFunc = checkFuncParam, \
306 .taskFunc = taskFuncParam, \
307 .desiredPeriodUs = desiredPeriodParam, \
308 .staticPriority = staticPriorityParam \
311 // Task info in .bss (unitialised data)
312 task_t tasks
[TASK_COUNT
];
314 // Task ID data in .data (initialised data)
315 task_id_t task_ids
[TASK_COUNT
] = {
316 [TASK_SYSTEM
] = DEFINE_TASK("SYSTEM", "LOAD", NULL
, taskSystemLoad
, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH
),
317 [TASK_MAIN
] = DEFINE_TASK("SYSTEM", "UPDATE", NULL
, taskMain
, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH
),
318 [TASK_SERIAL
] = DEFINE_TASK("SERIAL", NULL
, NULL
, taskHandleSerial
, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW
), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
319 [TASK_BATTERY_ALERTS
] = DEFINE_TASK("BATTERY_ALERTS", NULL
, NULL
, taskBatteryAlerts
, TASK_PERIOD_HZ(5), TASK_PRIORITY_MEDIUM
),
320 [TASK_BATTERY_VOLTAGE
] = DEFINE_TASK("BATTERY_VOLTAGE", NULL
, NULL
, batteryUpdateVoltage
, TASK_PERIOD_HZ(SLOW_VOLTAGE_TASK_FREQ_HZ
), TASK_PRIORITY_MEDIUM
), // Freq may be updated in tasksInit
321 [TASK_BATTERY_CURRENT
] = DEFINE_TASK("BATTERY_CURRENT", NULL
, NULL
, batteryUpdateCurrentMeter
, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM
),
323 #ifdef USE_TRANSPONDER
324 [TASK_TRANSPONDER
] = DEFINE_TASK("TRANSPONDER", NULL
, NULL
, transponderUpdate
, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW
),
327 #ifdef USE_STACK_CHECK
328 [TASK_STACK_CHECK
] = DEFINE_TASK("STACKCHECK", NULL
, NULL
, taskStackCheck
, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE
),
331 [TASK_GYRO
] = DEFINE_TASK("GYRO", NULL
, NULL
, taskGyroSample
, TASK_GYROPID_DESIRED_PERIOD
, TASK_PRIORITY_REALTIME
),
332 [TASK_FILTER
] = DEFINE_TASK("FILTER", NULL
, NULL
, taskFiltering
, TASK_GYROPID_DESIRED_PERIOD
, TASK_PRIORITY_REALTIME
),
333 [TASK_PID
] = DEFINE_TASK("PID", NULL
, NULL
, taskMainPidLoop
, TASK_GYROPID_DESIRED_PERIOD
, TASK_PRIORITY_REALTIME
),
335 [TASK_ACCEL
] = DEFINE_TASK("ACC", NULL
, NULL
, taskUpdateAccelerometer
, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM
),
336 [TASK_ATTITUDE
] = DEFINE_TASK("ATTITUDE", NULL
, NULL
, imuUpdateAttitude
, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM
),
338 [TASK_RX
] = DEFINE_TASK("RX", NULL
, rxUpdateCheck
, taskUpdateRxMain
, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH
), // If event-based scheduling doesn't work, fallback to periodic scheduling
339 [TASK_DISPATCH
] = DEFINE_TASK("DISPATCH", NULL
, NULL
, dispatchProcess
, TASK_PERIOD_HZ(1000), TASK_PRIORITY_HIGH
),
342 [TASK_BEEPER
] = DEFINE_TASK("BEEPER", NULL
, NULL
, beeperUpdate
, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW
),
346 [TASK_GPS
] = DEFINE_TASK("GPS", NULL
, NULL
, gpsUpdate
, TASK_PERIOD_HZ(TASK_GPS_RATE
), TASK_PRIORITY_MEDIUM
), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer)
350 [TASK_COMPASS
] = DEFINE_TASK("COMPASS", NULL
, NULL
, compassUpdate
,TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW
),
354 [TASK_BARO
] = DEFINE_TASK("BARO", NULL
, NULL
, taskUpdateBaro
, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW
),
357 #if defined(USE_BARO) || defined(USE_GPS)
358 [TASK_ALTITUDE
] = DEFINE_TASK("ALTITUDE", NULL
, NULL
, taskCalculateAltitude
, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW
),
362 [TASK_DASHBOARD
] = DEFINE_TASK("DASHBOARD", NULL
, NULL
, dashboardUpdate
, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW
),
366 [TASK_OSD
] = DEFINE_TASK("OSD", NULL
, osdUpdateCheck
, osdUpdate
, TASK_PERIOD_HZ(OSD_FRAMERATE_DEFAULT_HZ
), TASK_PRIORITY_LOW
),
370 [TASK_TELEMETRY
] = DEFINE_TASK("TELEMETRY", NULL
, NULL
, taskTelemetry
, TASK_PERIOD_HZ(250), TASK_PRIORITY_LOW
),
374 [TASK_LEDSTRIP
] = DEFINE_TASK("LEDSTRIP", NULL
, NULL
, ledStripUpdate
, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW
),
378 [TASK_BST_MASTER_PROCESS
] = DEFINE_TASK("BST_MASTER_PROCESS", NULL
, NULL
, taskBstMasterProcess
, TASK_PERIOD_HZ(50), TASK_PRIORITY_IDLE
),
381 #ifdef USE_ESC_SENSOR
382 [TASK_ESC_SENSOR
] = DEFINE_TASK("ESC_SENSOR", NULL
, NULL
, escSensorProcess
, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW
),
386 [TASK_CMS
] = DEFINE_TASK("CMS", NULL
, NULL
, cmsHandler
, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW
),
389 #ifdef USE_VTX_CONTROL
390 [TASK_VTXCTRL
] = DEFINE_TASK("VTXCTRL", NULL
, NULL
, vtxUpdate
, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE
),
394 [TASK_RCDEVICE
] = DEFINE_TASK("RCDEVICE", NULL
, NULL
, rcdeviceUpdate
, TASK_PERIOD_HZ(20), TASK_PRIORITY_MEDIUM
),
397 #ifdef USE_CAMERA_CONTROL
398 [TASK_CAMCTRL
] = DEFINE_TASK("CAMCTRL", NULL
, NULL
, taskCameraControl
, TASK_PERIOD_HZ(5), TASK_PRIORITY_IDLE
),
401 #ifdef USE_ADC_INTERNAL
402 [TASK_ADC_INTERNAL
] = DEFINE_TASK("ADCINTERNAL", NULL
, NULL
, adcInternalProcess
, TASK_PERIOD_HZ(1), TASK_PRIORITY_IDLE
),
406 [TASK_PINIOBOX
] = DEFINE_TASK("PINIOBOX", NULL
, NULL
, pinioBoxUpdate
, TASK_PERIOD_HZ(20), TASK_PRIORITY_IDLE
),
409 #ifdef USE_RANGEFINDER
410 [TASK_RANGEFINDER
] = DEFINE_TASK("RANGEFINDER", NULL
, NULL
, taskUpdateRangefinder
, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE
),
414 [TASK_SPEED_NEGOTIATION
] = DEFINE_TASK("SPEED_NEGOTIATION", NULL
, NULL
, speedNegotiationProcess
, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE
),
418 task_t
*getTask(unsigned taskId
)
420 return &tasks
[taskId
];
425 for (int i
= 0; i
< TASK_COUNT
; i
++) {
426 tasks
[i
].id
= &task_ids
[i
];
431 setTaskEnabled(TASK_MAIN
, true);
433 setTaskEnabled(TASK_SERIAL
, true);
434 rescheduleTask(TASK_SERIAL
, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz
));
436 const bool useBatteryVoltage
= batteryConfig()->voltageMeterSource
!= VOLTAGE_METER_NONE
;
437 setTaskEnabled(TASK_BATTERY_VOLTAGE
, useBatteryVoltage
);
439 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
440 // If vbat motor output compensation is used, use fast vbat samplingTime
441 if (isSagCompensationConfigured()) {
442 rescheduleTask(TASK_BATTERY_VOLTAGE
, TASK_PERIOD_HZ(FAST_VOLTAGE_TASK_FREQ_HZ
));
446 const bool useBatteryCurrent
= batteryConfig()->currentMeterSource
!= CURRENT_METER_NONE
;
447 setTaskEnabled(TASK_BATTERY_CURRENT
, useBatteryCurrent
);
448 const bool useBatteryAlerts
= batteryConfig()->useVBatAlerts
|| batteryConfig()->useConsumptionAlerts
|| featureIsEnabled(FEATURE_OSD
);
449 setTaskEnabled(TASK_BATTERY_ALERTS
, (useBatteryVoltage
|| useBatteryCurrent
) && useBatteryAlerts
);
451 #ifdef USE_STACK_CHECK
452 setTaskEnabled(TASK_STACK_CHECK
, true);
455 if (sensors(SENSOR_GYRO
)) {
456 rescheduleTask(TASK_GYRO
, gyro
.sampleLooptime
);
457 rescheduleTask(TASK_FILTER
, gyro
.targetLooptime
);
458 rescheduleTask(TASK_PID
, gyro
.targetLooptime
);
459 setTaskEnabled(TASK_GYRO
, true);
460 setTaskEnabled(TASK_FILTER
, true);
461 setTaskEnabled(TASK_PID
, true);
462 schedulerEnableGyro();
466 if (sensors(SENSOR_ACC
) && acc
.sampleRateHz
) {
467 setTaskEnabled(TASK_ACCEL
, true);
468 rescheduleTask(TASK_ACCEL
, TASK_PERIOD_HZ(acc
.sampleRateHz
));
469 setTaskEnabled(TASK_ATTITUDE
, true);
473 #ifdef USE_RANGEFINDER
474 if (sensors(SENSOR_RANGEFINDER
)) {
475 setTaskEnabled(TASK_RANGEFINDER
, featureIsEnabled(FEATURE_RANGEFINDER
));
479 setTaskEnabled(TASK_RX
, true);
481 setTaskEnabled(TASK_DISPATCH
, dispatchIsEnabled());
484 setTaskEnabled(TASK_BEEPER
, true);
488 setTaskEnabled(TASK_GPS
, featureIsEnabled(FEATURE_GPS
));
492 setTaskEnabled(TASK_COMPASS
, sensors(SENSOR_MAG
));
496 setTaskEnabled(TASK_BARO
, sensors(SENSOR_BARO
));
499 #if defined(USE_BARO) || defined(USE_GPS)
500 setTaskEnabled(TASK_ALTITUDE
, sensors(SENSOR_BARO
) || featureIsEnabled(FEATURE_GPS
));
504 setTaskEnabled(TASK_DASHBOARD
, featureIsEnabled(FEATURE_DASHBOARD
));
508 if (featureIsEnabled(FEATURE_TELEMETRY
)) {
509 setTaskEnabled(TASK_TELEMETRY
, true);
510 if (rxRuntimeState
.serialrxProvider
== SERIALRX_JETIEXBUS
) {
511 // Reschedule telemetry to 500hz for Jeti Exbus
512 rescheduleTask(TASK_TELEMETRY
, TASK_PERIOD_HZ(500));
513 } else if (rxRuntimeState
.serialrxProvider
== SERIALRX_CRSF
) {
514 // Reschedule telemetry to 500hz, 2ms for CRSF
515 rescheduleTask(TASK_TELEMETRY
, TASK_PERIOD_HZ(500));
521 setTaskEnabled(TASK_LEDSTRIP
, featureIsEnabled(FEATURE_LED_STRIP
));
524 #ifdef USE_TRANSPONDER
525 setTaskEnabled(TASK_TRANSPONDER
, featureIsEnabled(FEATURE_TRANSPONDER
));
529 rescheduleTask(TASK_OSD
, TASK_PERIOD_HZ(osdConfig()->framerate_hz
));
530 setTaskEnabled(TASK_OSD
, featureIsEnabled(FEATURE_OSD
) && osdGetDisplayPort(NULL
));
534 setTaskEnabled(TASK_BST_MASTER_PROCESS
, true);
537 #ifdef USE_ESC_SENSOR
538 setTaskEnabled(TASK_ESC_SENSOR
, featureIsEnabled(FEATURE_ESC_SENSOR
));
541 #ifdef USE_ADC_INTERNAL
542 setTaskEnabled(TASK_ADC_INTERNAL
, true);
546 pinioBoxTaskControl();
550 #ifdef USE_MSP_DISPLAYPORT
551 setTaskEnabled(TASK_CMS
, true);
553 setTaskEnabled(TASK_CMS
, featureIsEnabled(FEATURE_OSD
) || featureIsEnabled(FEATURE_DASHBOARD
));
557 #ifdef USE_VTX_CONTROL
558 #if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
559 setTaskEnabled(TASK_VTXCTRL
, true);
563 #ifdef USE_CAMERA_CONTROL
564 setTaskEnabled(TASK_CAMCTRL
, true);
568 setTaskEnabled(TASK_RCDEVICE
, rcdeviceIsEnabled());
572 const bool useCRSF
= rxRuntimeState
.serialrxProvider
== SERIALRX_CRSF
;
573 setTaskEnabled(TASK_SPEED_NEGOTIATION
, useCRSF
);