2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
23 #include "build/debug.h"
25 #include "blackbox/blackbox.h"
27 #include "common/axis.h"
28 #include "common/filter.h"
29 #include "common/maths.h"
30 #include "common/utils.h"
32 #include "config/feature.h"
33 #include "config/parameter_group.h"
34 #include "config/parameter_group_ids.h"
36 #include "drivers/gyro_sync.h"
37 #include "drivers/light_led.h"
38 #include "drivers/time.h"
39 #include "drivers/transponder_ir.h"
41 #include "sensors/acceleration.h"
42 #include "sensors/barometer.h"
43 #include "sensors/battery.h"
44 #include "sensors/boardalignment.h"
45 #include "sensors/gyro.h"
46 #include "sensors/sensors.h"
49 #include "fc/config.h"
50 #include "fc/controlrate_profile.h"
51 #include "fc/fc_core.h"
53 #include "fc/rc_adjustments.h"
54 #include "fc/rc_controls.h"
55 #include "fc/runtime_config.h"
57 #include "msp/msp_serial.h"
59 #include "io/asyncfatfs/asyncfatfs.h"
60 #include "io/beeper.h"
62 #include "io/motors.h"
63 #include "io/servos.h"
64 #include "io/serial.h"
65 #include "io/statusindicator.h"
66 #include "io/transponder_ir.h"
67 #include "io/vtx_control.h"
70 #include "scheduler/scheduler.h"
72 #include "telemetry/telemetry.h"
74 #include "flight/altitude.h"
75 #include "flight/failsafe.h"
76 #include "flight/imu.h"
77 #include "flight/mixer.h"
78 #include "flight/navigation.h"
79 #include "flight/pid.h"
80 #include "flight/servos.h"
86 bool canUpdateVTX(void);
87 #define VTX_IF_READY if (canUpdateVTX())
99 #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
101 #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
103 #if defined(GPS) || defined(MAG)
107 int16_t headFreeModeHold
;
109 uint8_t motorControlEnable
= false;
110 static bool reverseMotors
;
111 static uint32_t disarmAt
; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
114 static bool armingCalibrationWasInitialised
;
116 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t
, throttleCorrectionConfig
, PG_THROTTLE_CORRECTION_CONFIG
, 0);
118 PG_RESET_TEMPLATE(throttleCorrectionConfig_t
, throttleCorrectionConfig
,
119 .throttle_correction_value
= 0, // could 10 with althold or 40 for fpv
120 .throttle_correction_angle
= 800 // could be 80.0 deg with atlhold or 45.0 for fpv
123 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t
*rollAndPitchTrimsDelta
)
125 accelerometerConfigMutable()->accelerometerTrims
.values
.roll
+= rollAndPitchTrimsDelta
->values
.roll
;
126 accelerometerConfigMutable()->accelerometerTrims
.values
.pitch
+= rollAndPitchTrimsDelta
->values
.pitch
;
128 saveConfigAndNotify();
134 if (sensors(SENSOR_BARO
) && !isBaroCalibrationComplete()) {
139 // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
141 return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC
)) || (!isGyroCalibrationComplete());
144 void updateLEDs(void)
146 if (ARMING_FLAG(ARMED
)) {
149 if (IS_RC_MODE_ACTIVE(BOXARM
) == 0 || armingCalibrationWasInitialised
) {
150 ENABLE_ARMING_FLAG(OK_TO_ARM
);
153 if (!STATE(SMALL_ANGLE
)) {
154 DISABLE_ARMING_FLAG(OK_TO_ARM
);
157 if (isCalibrating() || (averageSystemLoadPercent
> 100)) {
159 DISABLE_ARMING_FLAG(OK_TO_ARM
);
161 if (ARMING_FLAG(OK_TO_ARM
)) {
174 armingCalibrationWasInitialised
= false;
176 if (ARMING_FLAG(ARMED
)) {
177 DISABLE_ARMING_FLAG(ARMED
);
180 if (blackboxConfig()->device
) {
185 beeper(BEEPER_DISARMING
); // emit disarm tone
191 static bool firstArmingCalibrationWasCompleted
;
193 if (armingConfig()->gyro_cal_on_first_arm
&& !firstArmingCalibrationWasCompleted
) {
194 gyroStartCalibration();
195 armingCalibrationWasInitialised
= true;
196 firstArmingCalibrationWasCompleted
= true;
199 if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
201 if (ARMING_FLAG(OK_TO_ARM
)) {
202 if (ARMING_FLAG(ARMED
)) {
205 if (IS_RC_MODE_ACTIVE(BOXFAILSAFE
)) {
208 if (!ARMING_FLAG(PREVENT_ARMING
)) {
210 if (!feature(FEATURE_3D
) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH
)) {
211 reverseMotors
= false;
212 for (unsigned index
= 0; index
< getMotorCount(); index
++) {
213 pwmWriteDshotCommand(index
, DSHOT_CMD_ROTATE_NORMAL
);
216 if (!feature(FEATURE_3D
) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH
)) {
217 reverseMotors
= true;
218 for (unsigned index
= 0; index
< getMotorCount(); index
++) {
219 pwmWriteDshotCommand(index
, DSHOT_CMD_ROTATE_REVERSE
);
223 ENABLE_ARMING_FLAG(ARMED
);
224 ENABLE_ARMING_FLAG(WAS_EVER_ARMED
);
225 headFreeModeHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
);
227 disarmAt
= millis() + armingConfig()->auto_disarm_delay
* 1000; // start disarm timeout, will be extended when throttle is nonzero
229 //beep to indicate arming
231 if (feature(FEATURE_GPS
) && STATE(GPS_FIX
) && GPS_numSat
>= 5)
232 beeper(BEEPER_ARMING_GPS_FIX
);
234 beeper(BEEPER_ARMING
);
236 beeper(BEEPER_ARMING
);
243 if (!ARMING_FLAG(ARMED
)) {
244 beeperConfirmationBeeps(1);
248 // Automatic ACC Offset Calibration
249 bool AccInflightCalibrationArmed
= false;
250 bool AccInflightCalibrationMeasurementDone
= false;
251 bool AccInflightCalibrationSavetoEEProm
= false;
252 bool AccInflightCalibrationActive
= false;
253 uint16_t InflightcalibratingA
= 0;
255 void handleInflightCalibrationStickPosition(void)
257 if (AccInflightCalibrationMeasurementDone
) {
258 // trigger saving into eeprom after landing
259 AccInflightCalibrationMeasurementDone
= false;
260 AccInflightCalibrationSavetoEEProm
= true;
262 AccInflightCalibrationArmed
= !AccInflightCalibrationArmed
;
263 if (AccInflightCalibrationArmed
) {
264 beeper(BEEPER_ACC_CALIBRATION
);
266 beeper(BEEPER_ACC_CALIBRATION_FAIL
);
271 static void updateInflightCalibrationState(void)
273 if (AccInflightCalibrationArmed
&& ARMING_FLAG(ARMED
) && rcData
[THROTTLE
] > rxConfig()->mincheck
&& !IS_RC_MODE_ACTIVE(BOXARM
)) { // Copter is airborne and you are turning it off via boxarm : start measurement
274 InflightcalibratingA
= 50;
275 AccInflightCalibrationArmed
= false;
277 if (IS_RC_MODE_ACTIVE(BOXCALIB
)) { // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
278 if (!AccInflightCalibrationActive
&& !AccInflightCalibrationMeasurementDone
)
279 InflightcalibratingA
= 50;
280 AccInflightCalibrationActive
= true;
281 } else if (AccInflightCalibrationMeasurementDone
&& !ARMING_FLAG(ARMED
)) {
282 AccInflightCalibrationMeasurementDone
= false;
283 AccInflightCalibrationSavetoEEProm
= true;
287 #if defined(GPS) || defined(MAG)
288 void updateMagHold(void)
290 if (ABS(rcCommand
[YAW
]) < 15 && FLIGHT_MODE(MAG_MODE
)) {
291 int16_t dif
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
) - magHold
;
296 dif
*= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed
);
297 if (STATE(SMALL_ANGLE
))
298 rcCommand
[YAW
] -= dif
* currentPidProfile
->pid
[PID_MAG
].P
/ 30; // 18 deg
300 magHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
);
305 void processRx(timeUs_t currentTimeUs
)
307 static bool armedBeeperOn
= false;
308 static bool airmodeIsActivated
;
310 calculateRxChannelsAndUpdateFailsafe(currentTimeUs
);
312 // in 3D mode, we need to be able to disarm by switch at any time
313 if (feature(FEATURE_3D
)) {
314 if (!IS_RC_MODE_ACTIVE(BOXARM
))
318 updateRSSI(currentTimeUs
);
320 if (feature(FEATURE_FAILSAFE
)) {
322 if (currentTimeUs
> FAILSAFE_POWER_ON_DELAY_US
&& !failsafeIsMonitoring()) {
323 failsafeStartMonitoring();
326 failsafeUpdateState();
329 const throttleStatus_e throttleStatus
= calculateThrottleStatus();
331 if (isAirmodeActive() && ARMING_FLAG(ARMED
)) {
332 if (rcCommand
[THROTTLE
] >= rxConfig()->airModeActivateThreshold
) airmodeIsActivated
= true; // Prevent Iterm from being reset
334 airmodeIsActivated
= false;
337 /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
338 This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
339 if (throttleStatus
== THROTTLE_LOW
&& !airmodeIsActivated
) {
340 pidResetErrorGyroState();
341 if (currentPidProfile
->pidAtMinThrottle
)
342 pidStabilisationState(PID_STABILISATION_ON
);
344 pidStabilisationState(PID_STABILISATION_OFF
);
346 pidStabilisationState(PID_STABILISATION_ON
);
349 // When armed and motors aren't spinning, do beeps and then disarm
350 // board after delay so users without buzzer won't lose fingers.
351 // mixTable constrains motor commands, so checking throttleStatus is enough
352 if (ARMING_FLAG(ARMED
)
353 && feature(FEATURE_MOTOR_STOP
)
354 && !STATE(FIXED_WING
)
355 && !feature(FEATURE_3D
)
356 && !isAirmodeActive()
358 if (isUsingSticksForArming()) {
359 if (throttleStatus
== THROTTLE_LOW
) {
360 if (armingConfig()->auto_disarm_delay
!= 0
361 && (int32_t)(disarmAt
- millis()) < 0
363 // auto-disarm configured and delay is over
365 armedBeeperOn
= false;
367 // still armed; do warning beeps while armed
368 beeper(BEEPER_ARMED
);
369 armedBeeperOn
= true;
372 // throttle is not low
373 if (armingConfig()->auto_disarm_delay
!= 0) {
374 // extend disarm time
375 disarmAt
= millis() + armingConfig()->auto_disarm_delay
* 1000;
380 armedBeeperOn
= false;
384 // arming is via AUX switch; beep while throttle low
385 if (throttleStatus
== THROTTLE_LOW
) {
386 beeper(BEEPER_ARMED
);
387 armedBeeperOn
= true;
388 } else if (armedBeeperOn
) {
390 armedBeeperOn
= false;
395 processRcStickPositions(throttleStatus
);
397 if (feature(FEATURE_INFLIGHT_ACC_CAL
)) {
398 updateInflightCalibrationState();
401 updateActivatedModes();
404 updateAdjustmentStates();
405 processRcAdjustments(currentControlRateProfile
);
408 bool canUseHorizonMode
= true;
410 if ((IS_RC_MODE_ACTIVE(BOXANGLE
) || (feature(FEATURE_FAILSAFE
) && failsafeIsActive())) && (sensors(SENSOR_ACC
))) {
411 // bumpless transfer to Level mode
412 canUseHorizonMode
= false;
414 if (!FLIGHT_MODE(ANGLE_MODE
)) {
415 ENABLE_FLIGHT_MODE(ANGLE_MODE
);
418 DISABLE_FLIGHT_MODE(ANGLE_MODE
); // failsafe support
421 if (IS_RC_MODE_ACTIVE(BOXHORIZON
) && canUseHorizonMode
) {
423 DISABLE_FLIGHT_MODE(ANGLE_MODE
);
425 if (!FLIGHT_MODE(HORIZON_MODE
)) {
426 ENABLE_FLIGHT_MODE(HORIZON_MODE
);
429 DISABLE_FLIGHT_MODE(HORIZON_MODE
);
432 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
438 #if defined(ACC) || defined(MAG)
439 if (sensors(SENSOR_ACC
) || sensors(SENSOR_MAG
)) {
440 #if defined(GPS) || defined(MAG)
441 if (IS_RC_MODE_ACTIVE(BOXMAG
)) {
442 if (!FLIGHT_MODE(MAG_MODE
)) {
443 ENABLE_FLIGHT_MODE(MAG_MODE
);
444 magHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
);
447 DISABLE_FLIGHT_MODE(MAG_MODE
);
450 if (IS_RC_MODE_ACTIVE(BOXHEADFREE
)) {
451 if (!FLIGHT_MODE(HEADFREE_MODE
)) {
452 ENABLE_FLIGHT_MODE(HEADFREE_MODE
);
455 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
457 if (IS_RC_MODE_ACTIVE(BOXHEADADJ
)) {
458 headFreeModeHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
); // acquire new heading
464 if (sensors(SENSOR_GPS
)) {
465 updateGpsWaypointsAndMode();
469 if (IS_RC_MODE_ACTIVE(BOXPASSTHRU
)) {
470 ENABLE_FLIGHT_MODE(PASSTHRU_MODE
);
472 DISABLE_FLIGHT_MODE(PASSTHRU_MODE
);
475 if (mixerConfig()->mixerMode
== MIXER_FLYING_WING
|| mixerConfig()->mixerMode
== MIXER_AIRPLANE
) {
476 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
480 if (feature(FEATURE_TELEMETRY
)) {
481 if ((!telemetryConfig()->telemetry_switch
&& ARMING_FLAG(ARMED
)) ||
482 (telemetryConfig()->telemetry_switch
&& IS_RC_MODE_ACTIVE(BOXTELEMETRY
))) {
484 releaseSharedTelemetryPorts();
486 // the telemetry state must be checked immediately so that shared serial ports are released.
487 telemetryCheckState();
488 mspSerialAllocatePorts();
494 vtxUpdateActivatedChannel();
497 handleVTXControlButton();
502 static void subTaskPidController(timeUs_t currentTimeUs
)
504 uint32_t startTime
= 0;
505 if (debugMode
== DEBUG_PIDLOOP
) {startTime
= micros();}
506 // PID - note this is function pointer set by setPIDController()
507 pidController(currentPidProfile
, &accelerometerConfig()->accelerometerTrims
, currentTimeUs
);
508 DEBUG_SET(DEBUG_PIDLOOP
, 1, micros() - startTime
);
511 static void subTaskMainSubprocesses(timeUs_t currentTimeUs
)
513 uint32_t startTime
= 0;
514 if (debugMode
== DEBUG_PIDLOOP
) {startTime
= micros();}
516 // Read out gyro temperature if used for telemmetry
517 if (feature(FEATURE_TELEMETRY
)) {
518 gyroReadTemperature();
522 if (sensors(SENSOR_MAG
)) {
527 #if defined(BARO) || defined(SONAR)
528 // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
530 if (sensors(SENSOR_BARO
) || sensors(SENSOR_SONAR
)) {
531 if (FLIGHT_MODE(BARO_MODE
) || FLIGHT_MODE(SONAR_MODE
)) {
537 // If we're armed, at minimum throttle, and we do arming via the
538 // sticks, do not process yaw input from the rx. We do this so the
539 // motors do not spin up while we are trying to arm or disarm.
540 // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
541 if (isUsingSticksForArming() && rcData
[THROTTLE
] <= rxConfig()->mincheck
542 #ifndef USE_QUAD_MIXER_ONLY
544 && !((mixerConfig()->mixerMode
== MIXER_TRI
|| mixerConfig()->mixerMode
== MIXER_CUSTOM_TRI
) && servoConfig()->tri_unarmed_servo
)
546 && mixerConfig()->mixerMode
!= MIXER_AIRPLANE
547 && mixerConfig()->mixerMode
!= MIXER_FLYING_WING
553 if (throttleCorrectionConfig()->throttle_correction_value
&& (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
))) {
554 rcCommand
[THROTTLE
] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value
);
560 if (sensors(SENSOR_GPS
)) {
561 if ((FLIGHT_MODE(GPS_HOME_MODE
) || FLIGHT_MODE(GPS_HOLD_MODE
)) && STATE(GPS_FIX_HOME
)) {
562 updateGpsStateForHomeAndHoldMode();
572 if (!cliMode
&& blackboxConfig()->device
) {
573 blackboxUpdate(currentTimeUs
);
576 UNUSED(currentTimeUs
);
580 transponderUpdate(currentTimeUs
);
582 DEBUG_SET(DEBUG_PIDLOOP
, 2, micros() - startTime
);
585 static void subTaskMotorUpdate(void)
587 uint32_t startTime
= 0;
588 if (debugMode
== DEBUG_CYCLETIME
) {
589 startTime
= micros();
590 static uint32_t previousMotorUpdateTime
;
591 const uint32_t currentDeltaTime
= startTime
- previousMotorUpdateTime
;
592 debug
[2] = currentDeltaTime
;
593 debug
[3] = currentDeltaTime
- targetPidLooptime
;
594 previousMotorUpdateTime
= startTime
;
595 } else if (debugMode
== DEBUG_PIDLOOP
) {
596 startTime
= micros();
599 mixTable(currentPidProfile
);
602 // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
603 if (isMixerUsingServos()) {
608 if (motorControlEnable
) {
611 DEBUG_SET(DEBUG_PIDLOOP
, 3, micros() - startTime
);
614 uint8_t setPidUpdateCountDown(void)
616 if (gyroConfig()->gyro_soft_lpf_hz
) {
617 return pidConfig()->pid_process_denom
- 1;
623 // Function for loop trigger
624 void taskMainPidLoop(timeUs_t currentTimeUs
)
626 static bool runTaskMainSubprocesses
;
627 static uint8_t pidUpdateCountdown
;
629 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
630 if(lockMainPID() != 0) return;
633 if (debugMode
== DEBUG_CYCLETIME
) {
634 debug
[0] = getTaskDeltaTime(TASK_SELF
);
635 debug
[1] = averageSystemLoadPercent
;
638 if (runTaskMainSubprocesses
) {
639 subTaskMainSubprocesses(currentTimeUs
);
640 runTaskMainSubprocesses
= false;
643 // DEBUG_PIDLOOP, timings for:
645 // 1 - pidController()
646 // 2 - subTaskMainSubprocesses()
647 // 3 - subTaskMotorUpdate()
648 uint32_t startTime
= 0;
649 if (debugMode
== DEBUG_PIDLOOP
) {startTime
= micros();}
651 DEBUG_SET(DEBUG_PIDLOOP
, 0, micros() - startTime
);
653 if (pidUpdateCountdown
) {
654 pidUpdateCountdown
--;
656 pidUpdateCountdown
= setPidUpdateCountDown();
657 subTaskPidController(currentTimeUs
);
658 subTaskMotorUpdate();
659 runTaskMainSubprocesses
= true;
663 bool isMotorsReversed()
665 return reverseMotors
;