original dT back
[betaflight.git] / src / main / mw.c
blobf3e656be41d3a61fe4b052026526646f279a1d86
1 /*
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/>.
18 #include <stdbool.h>
19 #include <stdlib.h>
20 #include <stdint.h>
21 #include <math.h>
23 #include "platform.h"
25 #include "common/maths.h"
26 #include "common/axis.h"
27 #include "common/color.h"
28 #include "common/filter.h"
30 #include "drivers/sensor.h"
31 #include "drivers/accgyro.h"
32 #include "drivers/compass.h"
33 #include "drivers/light_led.h"
35 #include "drivers/gpio.h"
36 #include "drivers/system.h"
37 #include "drivers/serial.h"
38 #include "drivers/timer.h"
39 #include "drivers/pwm_rx.h"
40 #include "drivers/gyro_sync.h"
42 #include "sensors/sensors.h"
43 #include "sensors/boardalignment.h"
44 #include "sensors/sonar.h"
45 #include "sensors/compass.h"
46 #include "sensors/acceleration.h"
47 #include "sensors/barometer.h"
48 #include "sensors/gyro.h"
49 #include "sensors/battery.h"
51 #include "io/beeper.h"
52 #include "io/display.h"
53 #include "io/escservo.h"
54 #include "io/rc_controls.h"
55 #include "io/rc_curves.h"
56 #include "io/gimbal.h"
57 #include "io/gps.h"
58 #include "io/ledstrip.h"
59 #include "io/serial.h"
60 #include "io/serial_cli.h"
61 #include "io/serial_msp.h"
62 #include "io/statusindicator.h"
64 #include "rx/rx.h"
65 #include "rx/msp.h"
67 #include "telemetry/telemetry.h"
68 #include "blackbox/blackbox.h"
70 #include "flight/mixer.h"
71 #include "flight/pid.h"
72 #include "flight/imu.h"
73 #include "flight/altitudehold.h"
74 #include "flight/failsafe.h"
75 #include "flight/autotune.h"
76 #include "flight/navigation.h"
78 #include "config/runtime_config.h"
79 #include "config/config.h"
80 #include "config/config_profile.h"
81 #include "config/config_master.h"
83 // June 2013 V2.2-dev
85 enum {
86 ALIGN_GYRO = 0,
87 ALIGN_ACCEL = 1,
88 ALIGN_MAG = 2
91 /* VBAT monitoring interval (in microseconds) - 1s*/
92 #define VBATINTERVAL (6 * 3500)
93 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
94 #define IBATINTERVAL (6 * 3500)
95 #define GYRO_WATCHDOG_DELAY 500 // Watchdog for boards without interrupt for gyro
96 #define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
98 uint32_t currentTime = 0;
99 uint32_t previousTime = 0;
100 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
101 float dT;
103 int16_t magHold;
104 int16_t headFreeModeHold;
106 uint8_t motorControlEnable = false;
108 int16_t telemTemperature1; // gyro sensor temperature
109 static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
111 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
113 static bool isRXdataNew;
115 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
116 uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
118 extern pidControllerFuncPtr pid_controller;
120 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
122 currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
123 currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
125 saveConfigAndNotify();
128 #ifdef AUTOTUNE
130 void updateAutotuneState(void)
132 static bool landedAfterAutoTuning = false;
133 static bool autoTuneWasUsed = false;
135 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
136 if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
137 if (ARMING_FLAG(ARMED)) {
138 if (isAutotuneIdle() || landedAfterAutoTuning) {
139 autotuneReset();
140 landedAfterAutoTuning = false;
142 autotuneBeginNextPhase(&currentProfile->pidProfile);
143 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
144 autoTuneWasUsed = true;
145 } else {
146 if (havePidsBeenUpdatedByAutotune()) {
147 saveConfigAndNotify();
148 autotuneReset();
152 return;
155 if (FLIGHT_MODE(AUTOTUNE_MODE)) {
156 autotuneEndPhase();
157 DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
160 if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
161 landedAfterAutoTuning = true;
164 #endif
166 bool isCalibrating()
168 #ifdef BARO
169 if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
170 return true;
172 #endif
174 // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
176 return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
179 void annexCode(void)
181 int32_t tmp, tmp2;
182 int32_t axis, prop1 = 0, prop2;
184 static uint32_t vbatLastServiced = 0;
185 static uint32_t ibatLastServiced = 0;
186 // PITCH & ROLL only dynamic PID adjustment, depending on throttle value
187 if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
188 prop2 = 100;
189 } else {
190 if (rcData[THROTTLE] < 2000) {
191 prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
192 } else {
193 prop2 = 100 - currentControlRateProfile->dynThrPID;
197 for (axis = 0; axis < 3; axis++) {
198 tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
199 if (axis == ROLL || axis == PITCH) {
200 if (currentProfile->rcControlsConfig.deadband) {
201 if (tmp > currentProfile->rcControlsConfig.deadband) {
202 tmp -= currentProfile->rcControlsConfig.deadband;
203 } else {
204 tmp = 0;
208 tmp2 = tmp / 100;
209 rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
210 prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
211 prop1 = (uint16_t)prop1 * prop2 / 100;
212 } else if (axis == YAW) {
213 if (currentProfile->rcControlsConfig.yaw_deadband) {
214 if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
215 tmp -= currentProfile->rcControlsConfig.yaw_deadband;
216 } else {
217 tmp = 0;
220 tmp2 = tmp / 100;
221 rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
222 prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
224 // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
225 dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
226 dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
227 dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
229 // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
230 if (axis == YAW) {
231 PIDweight[axis] = 100;
233 else {
234 PIDweight[axis] = prop2;
237 if (rcData[axis] < masterConfig.rxConfig.midrc)
238 rcCommand[axis] = -rcCommand[axis];
241 tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
242 tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
243 tmp2 = tmp / 100;
244 rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
246 if (FLIGHT_MODE(HEADFREE_MODE)) {
247 float radDiff = degreesToRadians(heading - headFreeModeHold);
248 float cosDiff = cos_approx(radDiff);
249 float sinDiff = sin_approx(radDiff);
250 int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
251 rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
252 rcCommand[PITCH] = rcCommand_PITCH;
255 if (feature(FEATURE_VBAT)) {
256 if ((int32_t)(currentTime - vbatLastServiced) >= VBATINTERVAL) {
257 vbatLastServiced = currentTime;
258 updateBattery();
262 if (feature(FEATURE_CURRENT_METER)) {
263 int32_t ibatTimeSinceLastServiced = (int32_t) (currentTime - ibatLastServiced);
265 if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
266 ibatLastServiced = currentTime;
267 updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
271 beeperUpdate(); //call periodic beeper handler
273 if (ARMING_FLAG(ARMED)) {
274 LED0_ON;
275 } else {
276 if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
277 ENABLE_ARMING_FLAG(OK_TO_ARM);
280 if (!STATE(SMALL_ANGLE)) {
281 DISABLE_ARMING_FLAG(OK_TO_ARM);
284 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
285 DISABLE_ARMING_FLAG(OK_TO_ARM);
288 if (isCalibrating()) {
289 warningLedFlash();
290 DISABLE_ARMING_FLAG(OK_TO_ARM);
291 } else {
292 if (ARMING_FLAG(OK_TO_ARM)) {
293 warningLedDisable();
294 } else {
295 warningLedFlash();
299 warningLedUpdate();
302 #ifdef TELEMETRY
303 telemetryCheckState();
304 #endif
306 handleSerial();
308 #ifdef GPS
309 if (sensors(SENSOR_GPS)) {
310 updateGpsIndicator(currentTime);
312 #endif
314 // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
315 if (gyro.temperature)
316 gyro.temperature(&telemTemperature1);
319 void mwDisarm(void)
321 if (ARMING_FLAG(ARMED)) {
322 DISABLE_ARMING_FLAG(ARMED);
324 #ifdef BLACKBOX
325 if (feature(FEATURE_BLACKBOX)) {
326 finishBlackbox();
328 #endif
330 beeper(BEEPER_DISARMING); // emit disarm tone
334 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_MSP | FUNCTION_TELEMETRY_SMARTPORT)
336 void releaseSharedTelemetryPorts(void) {
337 serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
338 while (sharedPort) {
339 mspReleasePortIfAllocated(sharedPort);
340 sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
344 void mwArm(void)
346 if (ARMING_FLAG(OK_TO_ARM)) {
347 if (ARMING_FLAG(ARMED)) {
348 return;
350 if (!ARMING_FLAG(PREVENT_ARMING)) {
351 ENABLE_ARMING_FLAG(ARMED);
352 headFreeModeHold = heading;
354 #ifdef BLACKBOX
355 if (feature(FEATURE_BLACKBOX)) {
356 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
357 if (sharedBlackboxAndMspPort) {
358 mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
360 startBlackbox();
362 #endif
363 disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
365 //beep to indicate arming
366 #ifdef GPS
367 if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
368 beeper(BEEPER_ARMING_GPS_FIX);
369 else
370 beeper(BEEPER_ARMING);
371 #else
372 beeper(BEEPER_ARMING);
373 #endif
375 return;
379 if (!ARMING_FLAG(ARMED)) {
380 beeperConfirmationBeeps(1);
384 // Automatic ACC Offset Calibration
385 bool AccInflightCalibrationArmed = false;
386 bool AccInflightCalibrationMeasurementDone = false;
387 bool AccInflightCalibrationSavetoEEProm = false;
388 bool AccInflightCalibrationActive = false;
389 uint16_t InflightcalibratingA = 0;
391 void handleInflightCalibrationStickPosition(void)
393 if (AccInflightCalibrationMeasurementDone) {
394 // trigger saving into eeprom after landing
395 AccInflightCalibrationMeasurementDone = false;
396 AccInflightCalibrationSavetoEEProm = true;
397 } else {
398 AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
399 if (AccInflightCalibrationArmed) {
400 beeper(BEEPER_ACC_CALIBRATION);
401 } else {
402 beeper(BEEPER_ACC_CALIBRATION_FAIL);
407 void updateInflightCalibrationState(void)
409 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
410 InflightcalibratingA = 50;
411 AccInflightCalibrationArmed = false;
413 if (IS_RC_MODE_ACTIVE(BOXCALIB)) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
414 if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
415 InflightcalibratingA = 50;
416 AccInflightCalibrationActive = true;
417 } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
418 AccInflightCalibrationMeasurementDone = false;
419 AccInflightCalibrationSavetoEEProm = true;
423 void updateMagHold(void)
425 if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
426 int16_t dif = heading - magHold;
427 if (dif <= -180)
428 dif += 360;
429 if (dif >= +180)
430 dif -= 360;
431 dif *= -masterConfig.yaw_control_direction;
432 if (STATE(SMALL_ANGLE))
433 rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg
434 } else
435 magHold = heading;
438 typedef enum {
439 #ifdef MAG
440 UPDATE_COMPASS_TASK,
441 #endif
442 #ifdef BARO
443 UPDATE_BARO_TASK,
444 #endif
445 #ifdef SONAR
446 UPDATE_SONAR_TASK,
447 #endif
448 #if defined(BARO) || defined(SONAR)
449 CALCULATE_ALTITUDE_TASK,
450 #endif
451 UPDATE_DISPLAY_TASK
452 } periodicTasks;
454 #define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
457 void executePeriodicTasks(void)
459 static int periodicTaskIndex = 0;
461 switch (periodicTaskIndex++) {
462 #ifdef MAG
463 case UPDATE_COMPASS_TASK:
464 if (sensors(SENSOR_MAG)) {
465 updateCompass(&masterConfig.magZero);
467 break;
468 #endif
470 #ifdef BARO
471 case UPDATE_BARO_TASK:
472 if (sensors(SENSOR_BARO)) {
473 baroUpdate(currentTime);
475 break;
476 #endif
478 #if defined(BARO) || defined(SONAR)
479 case CALCULATE_ALTITUDE_TASK:
481 #if defined(BARO) && !defined(SONAR)
482 if (sensors(SENSOR_BARO) && isBaroReady()) {
483 #endif
484 #if defined(BARO) && defined(SONAR)
485 if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) {
486 #endif
487 #if !defined(BARO) && defined(SONAR)
488 if (sensors(SENSOR_SONAR)) {
489 #endif
490 calculateEstimatedAltitude(currentTime);
492 break;
493 #endif
494 #ifdef SONAR
495 case UPDATE_SONAR_TASK:
496 if (sensors(SENSOR_SONAR)) {
497 sonarUpdate();
499 break;
500 #endif
501 #ifdef DISPLAY
502 case UPDATE_DISPLAY_TASK:
503 if (feature(FEATURE_DISPLAY)) {
504 updateDisplay();
506 break;
507 #endif
510 if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
511 periodicTaskIndex = 0;
515 void processRx(void)
517 static bool armedBeeperOn = false;
519 calculateRxChannelsAndUpdateFailsafe(currentTime);
521 // in 3D mode, we need to be able to disarm by switch at any time
522 if (feature(FEATURE_3D)) {
523 if (!IS_RC_MODE_ACTIVE(BOXARM))
524 mwDisarm();
527 updateRSSI(currentTime);
529 if (feature(FEATURE_FAILSAFE)) {
531 if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
532 failsafeStartMonitoring();
535 failsafeUpdateState();
538 throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
540 if (throttleStatus == THROTTLE_LOW) {
541 pidResetErrorAngle();
542 pidResetErrorGyro();
545 // When armed and motors aren't spinning, do beeps and then disarm
546 // board after delay so users without buzzer won't lose fingers.
547 // mixTable constrains motor commands, so checking throttleStatus is enough
548 if (ARMING_FLAG(ARMED)
549 && feature(FEATURE_MOTOR_STOP)
550 && !STATE(FIXED_WING)
552 if (isUsingSticksForArming()) {
553 if (throttleStatus == THROTTLE_LOW) {
554 if (masterConfig.auto_disarm_delay != 0
555 && (int32_t)(disarmAt - millis()) < 0
557 // auto-disarm configured and delay is over
558 mwDisarm();
559 armedBeeperOn = false;
560 } else {
561 // still armed; do warning beeps while armed
562 beeper(BEEPER_ARMED);
563 armedBeeperOn = true;
565 } else {
566 // throttle is not low
567 if (masterConfig.auto_disarm_delay != 0) {
568 // extend disarm time
569 disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
572 if (armedBeeperOn) {
573 beeperSilence();
574 armedBeeperOn = false;
577 } else {
578 // arming is via AUX switch; beep while throttle low
579 if (throttleStatus == THROTTLE_LOW) {
580 beeper(BEEPER_ARMED);
581 armedBeeperOn = true;
582 } else if (armedBeeperOn) {
583 beeperSilence();
584 armedBeeperOn = false;
589 processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
591 if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
592 updateInflightCalibrationState();
595 updateActivatedModes(currentProfile->modeActivationConditions);
597 if (!cliMode) {
598 updateAdjustmentStates(currentProfile->adjustmentRanges);
599 processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
602 bool canUseHorizonMode = true;
604 if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
605 // bumpless transfer to Level mode
606 canUseHorizonMode = false;
608 if (!FLIGHT_MODE(ANGLE_MODE)) {
609 pidResetErrorAngle();
610 ENABLE_FLIGHT_MODE(ANGLE_MODE);
612 } else {
613 DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
616 if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
618 DISABLE_FLIGHT_MODE(ANGLE_MODE);
620 if (!FLIGHT_MODE(HORIZON_MODE)) {
621 pidResetErrorAngle();
622 ENABLE_FLIGHT_MODE(HORIZON_MODE);
624 } else {
625 DISABLE_FLIGHT_MODE(HORIZON_MODE);
628 if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
629 LED1_ON;
630 } else {
631 LED1_OFF;
634 #ifdef MAG
635 if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
636 if (IS_RC_MODE_ACTIVE(BOXMAG)) {
637 if (!FLIGHT_MODE(MAG_MODE)) {
638 ENABLE_FLIGHT_MODE(MAG_MODE);
639 magHold = heading;
641 } else {
642 DISABLE_FLIGHT_MODE(MAG_MODE);
644 if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
645 if (!FLIGHT_MODE(HEADFREE_MODE)) {
646 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
648 } else {
649 DISABLE_FLIGHT_MODE(HEADFREE_MODE);
651 if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
652 headFreeModeHold = heading; // acquire new heading
655 #endif
657 #ifdef GPS
658 if (sensors(SENSOR_GPS)) {
659 updateGpsWaypointsAndMode();
661 #endif
663 if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
664 ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
665 } else {
666 DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
669 if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
670 DISABLE_FLIGHT_MODE(HEADFREE_MODE);
673 #ifdef TELEMETRY
674 if (feature(FEATURE_TELEMETRY)) {
675 if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
676 (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
678 releaseSharedTelemetryPorts();
679 } else {
680 // the telemetry state must be checked immediately so that shared serial ports are released.
681 telemetryCheckState();
682 mspAllocateSerialPorts(&masterConfig.serialConfig);
685 #endif
689 // Gyro Low Pass
690 void filterGyro(void) {
691 int axis;
692 static float dTGyro;
693 static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
695 if (!dTGyro) {
696 dTGyro = (float)targetLooptime * 0.000001f;
699 for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
700 gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dTGyro);
703 void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
704 for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
705 modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
706 if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
707 *armingChannel = modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT;
708 break;
713 void filterRc(void){
714 static int16_t lastCommand[4] = { 0, 0, 0, 0 };
715 static int16_t deltaRC[4] = { 0, 0, 0, 0 };
716 static int16_t loop[5] = { 0, 0, 0, 0, 0 };
717 static int16_t factor, rcInterpolationFactor, loopAvg;
718 static uint32_t rxRefreshRate;
719 static int16_t lastAux, deltaAux; // last arming AUX position and delta for arming AUX
720 static uint8_t auxChannelToFilter; // AUX channel used for arming needs filtering when used
721 static int loopCount;
723 // Set RC refresh rate for sampling and channels to filter
724 if (!rxRefreshRate) {
725 if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
726 rxRefreshRate = 20000;
728 // AUX Channels to filter to replace PPM/PWM averaging
729 getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
733 // TODO Are there more different refresh rates?
734 else {
735 switch (masterConfig.rxConfig.serialrx_provider) {
736 case SERIALRX_SPEKTRUM1024:
737 rxRefreshRate = 22000;
738 break;
739 case SERIALRX_SPEKTRUM2048:
740 rxRefreshRate = 11000;
741 break;
742 case SERIALRX_SBUS:
743 rxRefreshRate = 11000;
744 break;
745 default:
746 rxRefreshRate = 10000;
747 break;
751 rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
755 // Averaging of cycleTime for more precise sampling
756 loop[loopCount] = cycleTime;
757 loopCount++;
759 // Start recalculating new rcInterpolationFactor every 5 loop iterations
760 if (loopCount > 4) {
761 uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
763 // Jitter tolerance to prevent rcInterpolationFactor jump too much
764 if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND)) {
765 loopAvg = tmp;
766 rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
769 loopCount = 0;
772 if (isRXdataNew) {
773 for (int channel=0; channel < 4; channel++) {
774 deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
775 lastCommand[channel] = rcData[channel];
778 // Read AUX channel (arm/disarm guard enhancement)
779 if (auxChannelToFilter) {
780 deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
781 lastAux = rcData[auxChannelToFilter];
784 isRXdataNew = false;
785 factor = rcInterpolationFactor - 1;
788 else {
789 factor--;
792 // Interpolate steps of rcData
793 if (factor > 0) {
794 for (int channel=0; channel < 4; channel++) {
795 rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
798 // Interpolate steps of Aux
799 if (auxChannelToFilter) {
800 rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
804 else {
805 factor = 0;
809 // Function for loop trigger
810 bool runLoop(uint32_t loopTime) {
811 bool loopTrigger = false;
813 if (masterConfig.syncGyroToLoop) {
814 if (ARMING_FLAG(ARMED)) {
815 if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
816 loopTrigger = true;
819 // Blheli arming workaround (stable looptime prior to arming)
820 else if (!ARMING_FLAG(ARMED) && ((int32_t)(currentTime - loopTime) >= 0)) {
821 loopTrigger = true;
825 else if ((int32_t)(currentTime - loopTime) >= 0){
826 loopTrigger = true;
829 return loopTrigger;
832 void loop(void)
834 static uint32_t loopTime;
835 #if defined(BARO) || defined(SONAR)
836 static bool haveProcessedAnnexCodeOnce = false;
837 #endif
839 updateRx(currentTime);
841 if (shouldProcessRx(currentTime)) {
842 processRx();
843 isRXdataNew = true;
845 #ifdef BARO
846 // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
847 if (haveProcessedAnnexCodeOnce) {
848 if (sensors(SENSOR_BARO)) {
849 updateAltHoldState();
852 #endif
854 #ifdef SONAR
855 // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
856 if (haveProcessedAnnexCodeOnce) {
857 if (sensors(SENSOR_SONAR)) {
858 updateSonarAltHoldState();
861 #endif
863 } else {
864 // not processing rx this iteration
865 executePeriodicTasks();
867 // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
868 // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
869 // change this based on available hardware
870 #ifdef GPS
871 if (feature(FEATURE_GPS)) {
872 gpsThread();
874 #endif
877 currentTime = micros();
878 if (runLoop(loopTime)) {
880 loopTime = currentTime + targetLooptime;
882 imuUpdate(&currentProfile->accelerometerTrims);
884 // Measure loop rate just after reading the sensors
885 currentTime = micros();
886 cycleTime = (int32_t)(currentTime - previousTime);
887 previousTime = currentTime;
889 dT = (float)cycleTime * 0.000001f;
891 if (currentProfile->pidProfile.gyro_cut_hz) {
892 filterGyro();
895 if (masterConfig.rcSmoothing) {
896 filterRc();
899 annexCode();
900 #if defined(BARO) || defined(SONAR)
901 haveProcessedAnnexCodeOnce = true;
902 #endif
904 #ifdef AUTOTUNE
905 updateAutotuneState();
906 #endif
908 #ifdef MAG
909 if (sensors(SENSOR_MAG)) {
910 updateMagHold();
912 #endif
914 #if defined(BARO) || defined(SONAR)
915 if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
916 if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
917 applyAltHold(&masterConfig.airplaneConfig);
920 #endif
922 // If we're armed, at minimum throttle, and we do arming via the
923 // sticks, do not process yaw input from the rx. We do this so the
924 // motors do not spin up while we are trying to arm or disarm.
925 // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
926 if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
927 #ifndef USE_QUAD_MIXER_ONLY
928 && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
929 && masterConfig.mixerMode != MIXER_AIRPLANE
930 && masterConfig.mixerMode != MIXER_FLYING_WING
931 #endif
933 rcCommand[YAW] = 0;
937 if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
938 rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
941 #ifdef GPS
942 if (sensors(SENSOR_GPS)) {
943 if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
944 updateGpsStateForHomeAndHoldMode();
947 #endif
949 // PID - note this is function pointer set by setPIDController()
950 pid_controller(
951 &currentProfile->pidProfile,
952 currentControlRateProfile,
953 masterConfig.max_angle_inclination,
954 &currentProfile->accelerometerTrims,
955 &masterConfig.rxConfig
958 mixTable();
960 #ifdef USE_SERVOS
961 filterServos();
962 writeServos();
963 #endif
965 if (motorControlEnable) {
966 writeMotors();
969 #ifdef BLACKBOX
970 if (!cliMode && feature(FEATURE_BLACKBOX)) {
971 handleBlackbox();
973 #endif
976 #ifdef TELEMETRY
977 if (!cliMode && feature(FEATURE_TELEMETRY)) {
978 telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
980 #endif
982 #ifdef LED_STRIP
983 if (feature(FEATURE_LED_STRIP)) {
984 updateLedStrip();
986 #endif