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/>.
25 #include "common/maths.h"
26 #include "common/axis.h"
27 #include "common/color.h"
29 #include "drivers/sensor.h"
30 #include "drivers/accgyro.h"
31 #include "drivers/compass.h"
32 #include "drivers/light_led.h"
34 #include "drivers/gpio.h"
35 #include "drivers/system.h"
36 #include "drivers/serial.h"
37 #include "drivers/timer.h"
38 #include "drivers/pwm_rx.h"
40 #include "sensors/sensors.h"
41 #include "sensors/boardalignment.h"
42 #include "sensors/sonar.h"
43 #include "sensors/compass.h"
44 #include "sensors/acceleration.h"
45 #include "sensors/barometer.h"
46 #include "sensors/gyro.h"
47 #include "sensors/battery.h"
49 #include "io/beeper.h"
50 #include "io/display.h"
51 #include "io/escservo.h"
52 #include "io/rc_controls.h"
53 #include "io/rc_curves.h"
54 #include "io/gimbal.h"
56 #include "io/ledstrip.h"
57 #include "io/serial.h"
58 #include "io/serial_cli.h"
59 #include "io/serial_msp.h"
60 #include "io/statusindicator.h"
65 #include "telemetry/telemetry.h"
66 #include "blackbox/blackbox.h"
68 #include "flight/mixer.h"
69 #include "flight/pid.h"
70 #include "flight/imu.h"
71 #include "flight/altitudehold.h"
72 #include "flight/failsafe.h"
73 #include "flight/autotune.h"
74 #include "flight/navigation.h"
77 #include "config/runtime_config.h"
78 #include "config/config.h"
79 #include "config/config_profile.h"
80 #include "config/config_master.h"
90 /* for VBAT monitoring frequency */
91 #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
94 uint32_t currentTime
= 0;
95 uint32_t previousTime
= 0;
96 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
99 int16_t headFreeModeHold
;
101 int16_t telemTemperature1
; // gyro sensor temperature
102 static uint32_t disarmAt
; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
104 extern uint8_t dynP8
[3], dynI8
[3], dynD8
[3];
105 extern failsafe_t
*failsafe
;
107 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
108 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
110 extern pidControllerFuncPtr pid_controller
;
112 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t
*rollAndPitchTrimsDelta
)
114 currentProfile
->accelerometerTrims
.values
.roll
+= rollAndPitchTrimsDelta
->values
.roll
;
115 currentProfile
->accelerometerTrims
.values
.pitch
+= rollAndPitchTrimsDelta
->values
.pitch
;
117 saveConfigAndNotify();
122 void updateAutotuneState(void)
124 static bool landedAfterAutoTuning
= false;
125 static bool autoTuneWasUsed
= false;
127 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE
)) {
128 if (!FLIGHT_MODE(AUTOTUNE_MODE
)) {
129 if (ARMING_FLAG(ARMED
)) {
130 if (isAutotuneIdle() || landedAfterAutoTuning
) {
132 landedAfterAutoTuning
= false;
134 autotuneBeginNextPhase(¤tProfile
->pidProfile
);
135 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE
);
136 autoTuneWasUsed
= true;
138 if (havePidsBeenUpdatedByAutotune()) {
139 saveConfigAndNotify();
147 if (FLIGHT_MODE(AUTOTUNE_MODE
)) {
149 DISABLE_FLIGHT_MODE(AUTOTUNE_MODE
);
152 if (!ARMING_FLAG(ARMED
) && autoTuneWasUsed
) {
153 landedAfterAutoTuning
= true;
161 if (sensors(SENSOR_BARO
) && !isBaroCalibrationComplete()) {
166 // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
168 return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC
)) || (!isGyroCalibrationComplete());
174 int32_t axis
, prop1
= 0, prop2
;
176 static batteryState_e batteryState
= BATTERY_OK
;
177 static uint8_t vbatTimer
= 0;
178 static int32_t vbatCycleTime
= 0;
180 // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
181 if (rcData
[THROTTLE
] < currentControlRateProfile
->tpa_breakpoint
) {
184 if (rcData
[THROTTLE
] < 2000) {
185 prop2
= 100 - (uint16_t)currentControlRateProfile
->dynThrPID
* (rcData
[THROTTLE
] - currentControlRateProfile
->tpa_breakpoint
) / (2000 - currentControlRateProfile
->tpa_breakpoint
);
187 prop2
= 100 - currentControlRateProfile
->dynThrPID
;
191 for (axis
= 0; axis
< 3; axis
++) {
192 tmp
= MIN(ABS(rcData
[axis
] - masterConfig
.rxConfig
.midrc
), 500);
193 if (axis
== ROLL
|| axis
== PITCH
) {
194 if (currentProfile
->rcControlsConfig
.deadband
) {
195 if (tmp
> currentProfile
->rcControlsConfig
.deadband
) {
196 tmp
-= currentProfile
->rcControlsConfig
.deadband
;
203 rcCommand
[axis
] = lookupPitchRollRC
[tmp2
] + (tmp
- tmp2
* 100) * (lookupPitchRollRC
[tmp2
+ 1] - lookupPitchRollRC
[tmp2
]) / 100;
204 prop1
= 100 - (uint16_t)currentControlRateProfile
->rollPitchRate
* tmp
/ 500;
205 prop1
= (uint16_t)prop1
* prop2
/ 100;
206 } else if (axis
== YAW
) {
207 if (currentProfile
->rcControlsConfig
.yaw_deadband
) {
208 if (tmp
> currentProfile
->rcControlsConfig
.yaw_deadband
) {
209 tmp
-= currentProfile
->rcControlsConfig
.yaw_deadband
;
214 rcCommand
[axis
] = tmp
* -masterConfig
.yaw_control_direction
;
215 prop1
= 100 - (uint16_t)currentControlRateProfile
->yawRate
* ABS(tmp
) / 500;
217 // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
218 dynP8
[axis
] = (uint16_t)currentProfile
->pidProfile
.P8
[axis
] * prop1
/ 100;
219 dynI8
[axis
] = (uint16_t)currentProfile
->pidProfile
.I8
[axis
] * prop1
/ 100;
220 dynD8
[axis
] = (uint16_t)currentProfile
->pidProfile
.D8
[axis
] * prop1
/ 100;
222 if (rcData
[axis
] < masterConfig
.rxConfig
.midrc
)
223 rcCommand
[axis
] = -rcCommand
[axis
];
226 tmp
= constrain(rcData
[THROTTLE
], masterConfig
.rxConfig
.mincheck
, PWM_RANGE_MAX
);
227 tmp
= (uint32_t)(tmp
- masterConfig
.rxConfig
.mincheck
) * PWM_RANGE_MIN
/ (PWM_RANGE_MAX
- masterConfig
.rxConfig
.mincheck
); // [MINCHECK;2000] -> [0;1000]
229 rcCommand
[THROTTLE
] = lookupThrottleRC
[tmp2
] + (tmp
- tmp2
* 100) * (lookupThrottleRC
[tmp2
+ 1] - lookupThrottleRC
[tmp2
]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
231 if (FLIGHT_MODE(HEADFREE_MODE
)) {
232 float radDiff
= degreesToRadians(heading
- headFreeModeHold
);
233 float cosDiff
= cosf(radDiff
);
234 float sinDiff
= sinf(radDiff
);
235 int16_t rcCommand_PITCH
= rcCommand
[PITCH
] * cosDiff
+ rcCommand
[ROLL
] * sinDiff
;
236 rcCommand
[ROLL
] = rcCommand
[ROLL
] * cosDiff
- rcCommand
[PITCH
] * sinDiff
;
237 rcCommand
[PITCH
] = rcCommand_PITCH
;
240 if (feature(FEATURE_VBAT
| FEATURE_CURRENT_METER
)) {
241 vbatCycleTime
+= cycleTime
;
242 if (!(++vbatTimer
% VBATFREQ
)) {
244 if (feature(FEATURE_VBAT
)) {
245 updateBatteryVoltage();
246 batteryState
= calculateBatteryState();
249 if (feature(FEATURE_CURRENT_METER
)) {
250 updateCurrentMeter(vbatCycleTime
);
256 beepcodeUpdateState(batteryState
);
258 if (ARMING_FLAG(ARMED
)) {
261 if (IS_RC_MODE_ACTIVE(BOXARM
) == 0) {
262 ENABLE_ARMING_FLAG(OK_TO_ARM
);
265 if (isCalibrating()) {
267 DISABLE_ARMING_FLAG(OK_TO_ARM
);
270 if (!STATE(SMALL_ANGLE
)) {
271 DISABLE_ARMING_FLAG(OK_TO_ARM
);
274 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE
)) {
275 DISABLE_ARMING_FLAG(OK_TO_ARM
);
278 if (ARMING_FLAG(OK_TO_ARM
)) {
281 enableWarningLed(currentTime
);
284 updateWarningLed(currentTime
);
288 checkTelemetryState();
294 if (sensors(SENSOR_GPS
)) {
295 updateGpsIndicator(currentTime
);
299 // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
300 if (gyro
.temperature
)
301 gyro
.temperature(&telemTemperature1
);
306 if (ARMING_FLAG(ARMED
)) {
307 DISABLE_ARMING_FLAG(ARMED
);
310 if (feature(FEATURE_TELEMETRY
)) {
311 // the telemetry state must be checked immediately so that shared serial ports are released.
312 checkTelemetryState();
313 mspAllocateSerialPorts(&masterConfig
.serialConfig
);
318 if (feature(FEATURE_BLACKBOX
)) {
325 #define TELEMETRY_FUNCTION_MASK (FUNCTION_FRSKY_TELEMETRY | FUNCTION_HOTT_TELEMETRY | FUNCTION_MSP_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY)
329 if (ARMING_FLAG(OK_TO_ARM
)) {
330 if (ARMING_FLAG(ARMED
)) {
333 if (!ARMING_FLAG(PREVENT_ARMING
)) {
334 ENABLE_ARMING_FLAG(ARMED
);
335 headFreeModeHold
= heading
;
338 if (feature(FEATURE_TELEMETRY
)) {
341 serialPort_t
*sharedPort
= findSharedSerialPort(TELEMETRY_FUNCTION_MASK
, FUNCTION_MSP
);
343 mspReleasePortIfAllocated(sharedPort
);
344 sharedPort
= findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK
, FUNCTION_MSP
);
350 if (feature(FEATURE_BLACKBOX
)) {
351 serialPort_t
*sharedBlackboxAndMspPort
= findSharedSerialPort(FUNCTION_BLACKBOX
, FUNCTION_MSP
);
352 if (sharedBlackboxAndMspPort
) {
353 mspReleasePortIfAllocated(sharedBlackboxAndMspPort
);
358 disarmAt
= millis() + masterConfig
.auto_disarm_delay
* 1000; // start disarm timeout, will be extended when throttle is nonzero
364 if (!ARMING_FLAG(ARMED
)) {
365 blinkLedAndSoundBeeper(2, 255, 1);
369 // Automatic ACC Offset Calibration
370 bool AccInflightCalibrationArmed
= false;
371 bool AccInflightCalibrationMeasurementDone
= false;
372 bool AccInflightCalibrationSavetoEEProm
= false;
373 bool AccInflightCalibrationActive
= false;
374 uint16_t InflightcalibratingA
= 0;
376 void handleInflightCalibrationStickPosition(void)
378 if (AccInflightCalibrationMeasurementDone
) {
379 // trigger saving into eeprom after landing
380 AccInflightCalibrationMeasurementDone
= false;
381 AccInflightCalibrationSavetoEEProm
= true;
383 AccInflightCalibrationArmed
= !AccInflightCalibrationArmed
;
384 if (AccInflightCalibrationArmed
) {
385 queueConfirmationBeep(4);
387 queueConfirmationBeep(6);
392 void updateInflightCalibrationState(void)
394 if (AccInflightCalibrationArmed
&& ARMING_FLAG(ARMED
) && rcData
[THROTTLE
] > masterConfig
.rxConfig
.mincheck
&& !IS_RC_MODE_ACTIVE(BOXARM
)) { // Copter is airborne and you are turning it off via boxarm : start measurement
395 InflightcalibratingA
= 50;
396 AccInflightCalibrationArmed
= false;
398 if (IS_RC_MODE_ACTIVE(BOXCALIB
)) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
399 if (!AccInflightCalibrationActive
&& !AccInflightCalibrationMeasurementDone
)
400 InflightcalibratingA
= 50;
401 AccInflightCalibrationActive
= true;
402 } else if (AccInflightCalibrationMeasurementDone
&& !ARMING_FLAG(ARMED
)) {
403 AccInflightCalibrationMeasurementDone
= false;
404 AccInflightCalibrationSavetoEEProm
= true;
408 void updateMagHold(void)
410 if (ABS(rcCommand
[YAW
]) < 70 && FLIGHT_MODE(MAG_MODE
)) {
411 int16_t dif
= heading
- magHold
;
416 dif
*= -masterConfig
.yaw_control_direction
;
417 if (STATE(SMALL_ANGLE
))
418 rcCommand
[YAW
] -= dif
* currentProfile
->pidProfile
.P8
[PIDMAG
] / 30; // 18 deg
433 #if defined(BARO) || defined(SONAR)
434 CALCULATE_ALTITUDE_TASK
,
439 #define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
442 void executePeriodicTasks(void)
444 static int periodicTaskIndex
= 0;
446 switch (periodicTaskIndex
++) {
448 case UPDATE_COMPASS_TASK
:
449 if (sensors(SENSOR_MAG
)) {
450 updateCompass(&masterConfig
.magZero
);
456 case UPDATE_BARO_TASK
:
457 if (sensors(SENSOR_BARO
)) {
458 baroUpdate(currentTime
);
463 #if defined(BARO) || defined(SONAR)
464 case CALCULATE_ALTITUDE_TASK
:
466 #if defined(BARO) && !defined(SONAR)
467 if (sensors(SENSOR_BARO
) && isBaroReady()) {
469 #if defined(BARO) && defined(SONAR)
470 if ((sensors(SENSOR_BARO
) && isBaroReady()) || sensors(SENSOR_SONAR
)) {
472 #if !defined(BARO) && defined(SONAR)
473 if (sensors(SENSOR_SONAR
)) {
475 calculateEstimatedAltitude(currentTime
);
480 case UPDATE_SONAR_TASK
:
481 if (sensors(SENSOR_SONAR
)) {
487 case UPDATE_DISPLAY_TASK
:
488 if (feature(FEATURE_DISPLAY
)) {
495 if (periodicTaskIndex
>= PERIODIC_TASK_COUNT
) {
496 periodicTaskIndex
= 0;
502 calculateRxChannelsAndUpdateFailsafe(currentTime
);
504 // in 3D mode, we need to be able to disarm by switch at any time
505 if (feature(FEATURE_3D
)) {
506 if (!IS_RC_MODE_ACTIVE(BOXARM
))
510 updateRSSI(currentTime
);
512 if (feature(FEATURE_FAILSAFE
)) {
514 if (currentTime
> FAILSAFE_POWER_ON_DELAY_US
&& !failsafe
->vTable
->isEnabled()) {
515 failsafe
->vTable
->enable();
518 failsafe
->vTable
->updateState();
521 throttleStatus_e throttleStatus
= calculateThrottleStatus(&masterConfig
.rxConfig
, masterConfig
.flight3DConfig
.deadband3d_throttle
);
523 if (throttleStatus
== THROTTLE_LOW
) {
527 // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers.
528 // mixTable constrains motor commands, so checking throttleStatus is enough
531 && feature(FEATURE_MOTOR_STOP
) && !STATE(FIXED_WING
)
532 && masterConfig
.auto_disarm_delay
!= 0
533 && isUsingSticksForArming()
535 if (throttleStatus
== THROTTLE_LOW
) {
536 if ((int32_t)(disarmAt
- millis()) < 0) // delay is over
539 disarmAt
= millis() + masterConfig
.auto_disarm_delay
* 1000; // extend delay
543 processRcStickPositions(&masterConfig
.rxConfig
, throttleStatus
, masterConfig
.retarded_arm
, masterConfig
.disarm_kill_switch
);
545 if (feature(FEATURE_INFLIGHT_ACC_CAL
)) {
546 updateInflightCalibrationState();
549 updateActivatedModes(currentProfile
->modeActivationConditions
);
552 updateAdjustmentStates(currentProfile
->adjustmentRanges
);
553 processRcAdjustments(currentControlRateProfile
, &masterConfig
.rxConfig
);
556 bool canUseHorizonMode
= true;
558 if ((IS_RC_MODE_ACTIVE(BOXANGLE
) || (feature(FEATURE_FAILSAFE
) && failsafe
->vTable
->hasTimerElapsed())) && (sensors(SENSOR_ACC
))) {
559 // bumpless transfer to Level mode
560 canUseHorizonMode
= false;
562 if (!FLIGHT_MODE(ANGLE_MODE
)) {
564 ENABLE_FLIGHT_MODE(ANGLE_MODE
);
567 DISABLE_FLIGHT_MODE(ANGLE_MODE
); // failsafe support
570 if (IS_RC_MODE_ACTIVE(BOXHORIZON
) && canUseHorizonMode
) {
572 DISABLE_FLIGHT_MODE(ANGLE_MODE
);
574 if (!FLIGHT_MODE(HORIZON_MODE
)) {
576 ENABLE_FLIGHT_MODE(HORIZON_MODE
);
579 DISABLE_FLIGHT_MODE(HORIZON_MODE
);
582 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
589 if (sensors(SENSOR_ACC
) || sensors(SENSOR_MAG
)) {
590 if (IS_RC_MODE_ACTIVE(BOXMAG
)) {
591 if (!FLIGHT_MODE(MAG_MODE
)) {
592 ENABLE_FLIGHT_MODE(MAG_MODE
);
596 DISABLE_FLIGHT_MODE(MAG_MODE
);
598 if (IS_RC_MODE_ACTIVE(BOXHEADFREE
)) {
599 if (!FLIGHT_MODE(HEADFREE_MODE
)) {
600 ENABLE_FLIGHT_MODE(HEADFREE_MODE
);
603 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
605 if (IS_RC_MODE_ACTIVE(BOXHEADADJ
)) {
606 headFreeModeHold
= heading
; // acquire new heading
612 if (sensors(SENSOR_GPS
)) {
613 updateGpsWaypointsAndMode();
617 if (IS_RC_MODE_ACTIVE(BOXPASSTHRU
)) {
618 ENABLE_FLIGHT_MODE(PASSTHRU_MODE
);
620 DISABLE_FLIGHT_MODE(PASSTHRU_MODE
);
623 if (masterConfig
.mixerMode
== MIXER_FLYING_WING
|| masterConfig
.mixerMode
== MIXER_AIRPLANE
) {
624 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
630 static uint32_t loopTime
;
631 #if defined(BARO) || defined(SONAR)
632 static bool haveProcessedAnnexCodeOnce
= false;
637 if (shouldProcessRx(currentTime
)) {
641 // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
642 if (haveProcessedAnnexCodeOnce
) {
643 if (sensors(SENSOR_BARO
)) {
644 updateAltHoldState();
650 // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
651 if (haveProcessedAnnexCodeOnce
) {
652 if (sensors(SENSOR_SONAR
)) {
653 updateSonarAltHoldState();
659 // not processing rx this iteration
660 executePeriodicTasks();
662 // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
663 // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
664 // change this based on available hardware
666 if (feature(FEATURE_GPS
)) {
672 currentTime
= micros();
673 if (masterConfig
.looptime
== 0 || (int32_t)(currentTime
- loopTime
) >= 0) {
674 loopTime
= currentTime
+ masterConfig
.looptime
;
676 imuUpdate(¤tProfile
->accelerometerTrims
, masterConfig
.mixerMode
);
678 // Measure loop rate just after reading the sensors
679 currentTime
= micros();
680 cycleTime
= (int32_t)(currentTime
- previousTime
);
681 previousTime
= currentTime
;
684 #if defined(BARO) || defined(SONAR)
685 haveProcessedAnnexCodeOnce
= true;
689 updateAutotuneState();
693 if (sensors(SENSOR_MAG
)) {
698 #if defined(BARO) || defined(SONAR)
699 if (sensors(SENSOR_BARO
) || sensors(SENSOR_SONAR
)) {
700 if (FLIGHT_MODE(BARO_MODE
) || FLIGHT_MODE(SONAR_MODE
)) {
701 applyAltHold(&masterConfig
.airplaneConfig
);
706 if (currentProfile
->throttle_correction_value
&& (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
))) {
707 rcCommand
[THROTTLE
] += calculateThrottleAngleCorrection(currentProfile
->throttle_correction_value
);
711 if (sensors(SENSOR_GPS
)) {
712 if ((FLIGHT_MODE(GPS_HOME_MODE
) || FLIGHT_MODE(GPS_HOLD_MODE
)) && STATE(GPS_FIX_HOME
)) {
713 updateGpsStateForHomeAndHoldMode();
718 // PID - note this is function pointer set by setPIDController()
720 ¤tProfile
->pidProfile
,
721 currentControlRateProfile
,
722 masterConfig
.max_angle_inclination
,
723 ¤tProfile
->accelerometerTrims
,
724 &masterConfig
.rxConfig
732 if (!cliMode
&& feature(FEATURE_BLACKBOX
)) {
739 if (!cliMode
&& feature(FEATURE_TELEMETRY
)) {
745 if (feature(FEATURE_LED_STRIP
)) {