naivation to navigation typo fixed

src/main/fc/fc_core.c

@@ -167,7 +167,7 @@ static void updatePreArmingChecks(void)
     }
 
 #if defined(NAV)
-    if (naivationBlockArming()) {
+    if (navigationBlockArming()) {
         ENABLE_ARMING_FLAG(BLOCKED_NAVIGATION_SAFETY);
         DISABLE_ARMING_FLAG(OK_TO_ARM);
     }
@@ -397,7 +397,7 @@ void processRx(timeUs_t currentTimeUs)
 
     bool canUseHorizonMode = true;
 
-    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
+    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
         // bumpless transfer to Level mode
         canUseHorizonMode = false;
 
@@ -475,7 +475,7 @@ void processRx(timeUs_t currentTimeUs)
 
     // Handle passthrough mode
     if (STATE(FIXED_WING)) {
-        if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
+        if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
             (!ARMING_FLAG(ARMED) && isCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating        
             ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
         } else {

src/main/fc/fc_core.c.orig

@@ -0,0 +1,754 @@
+/*
+ * 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 "blackbox/blackbox.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/axis.h"
+#include "common/color.h"
+#include "common/utils.h"
+#include "common/filter.h"
+
+#include "drivers/light_led.h"
+#include "drivers/gyro_sync.h"
+#include "drivers/serial.h"
+#include "drivers/time.h"
+
+#include "sensors/sensors.h"
+#include "sensors/diagnostics.h"
+#include "sensors/boardalignment.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+
+#include "fc/fc_core.h"
+#include "fc/cli.h"
+#include "fc/config.h"
+#include "fc/controlrate_profile.h"
+#include "fc/rc_adjustments.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_curves.h"
+#include "fc/runtime_config.h"
+
+#include "io/beeper.h"
+#include "io/dashboard.h"
+#include "io/gimbal.h"
+#include "io/gps.h"
+#include "io/serial.h"
+#include "io/statusindicator.h"
+#include "io/asyncfatfs/asyncfatfs.h"
+
+#include "msp/msp_serial.h"
+
+#include "navigation/navigation.h"
+
+#include "rx/rx.h"
+#include "rx/msp.h"
+
+#include "scheduler/scheduler.h"
+
+#include "telemetry/telemetry.h"
+
+#include "flight/mixer.h"
+#include "flight/servos.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+
+#include "flight/failsafe.h"
+
+#include "config/feature.h"
+
+// June 2013     V2.2-dev
+
+enum {
+    ALIGN_GYRO = 0,
+    ALIGN_ACCEL = 1,
+    ALIGN_MAG = 2
+};
+
+#define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro
+
+timeDelta_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
+
+float dT;
+
+int16_t headFreeModeHold;
+
+uint8_t motorControlEnable = false;
+
+int16_t telemTemperature1;      // gyro sensor temperature
+static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
+
+static bool isRXDataNew;
+static disarmReason_t lastDisarmReason = DISARM_NONE;
+
+bool isCalibrating(void)
+{
+#ifdef BARO
+    if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) {
+        return true;
+    }
+#endif
+
+#ifdef PITOT
+    if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) {
+        return true;
+    }
+#endif
+
+#ifdef NAV
+    if (!navIsCalibrationComplete()) {
+        return true;
+    }
+#endif
+
+    if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) {
+        return true;
+    }
+
+    if (!gyroIsCalibrationComplete()) {
+        return true;
+    }
+
+    return false;
+}
+
+int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
+{
+    int16_t stickDeflection;
+
+    stickDeflection = constrain(rawData - rxConfig()->midrc, -500, 500);
+    stickDeflection = applyDeadband(stickDeflection, deadband);
+
+    return rcLookup(stickDeflection, rate);
+}
+
+static void updatePreArmingChecks(void)
+{
+    DISABLE_ARMING_FLAG(BLOCKED_ALL_FLAGS);
+
+    if (!STATE(SMALL_ANGLE)) {
+        ENABLE_ARMING_FLAG(BLOCKED_UAV_NOT_LEVEL);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+
+    if (isCalibrating()) {
+        ENABLE_ARMING_FLAG(BLOCKED_SENSORS_CALIBRATING);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+
+    if (isSystemOverloaded()) {
+        ENABLE_ARMING_FLAG(BLOCKED_SYSTEM_OVERLOADED);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+
+#if defined(NAV)
+    if (navigationBlockArming()) {
+        ENABLE_ARMING_FLAG(BLOCKED_NAVIGATION_SAFETY);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+#endif
+
+#if defined(MAG)
+    if (sensors(SENSOR_MAG) && !STATE(COMPASS_CALIBRATED)) {
+        ENABLE_ARMING_FLAG(BLOCKED_COMPASS_NOT_CALIBRATED);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+#endif
+
+    if (sensors(SENSOR_ACC) && !STATE(ACCELEROMETER_CALIBRATED)) {
+        ENABLE_ARMING_FLAG(BLOCKED_ACCELEROMETER_NOT_CALIBRATED);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+
+    if (!isHardwareHealthy()) {
+        ENABLE_ARMING_FLAG(BLOCKED_HARDWARE_FAILURE);
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+    }
+}
+
+void annexCode(void)
+{
+    int32_t throttleValue;
+
+    if (failsafeShouldApplyControlInput()) {
+        // Failsafe will apply rcCommand for us
+        failsafeApplyControlInput();
+    }
+    else {
+        // Compute ROLL PITCH and YAW command
+        rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
+        rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
+        rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband);
+
+        //Compute THROTTLE command
+        throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
+        throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);       // [MINCHECK;2000] -> [0;1000]
+        rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
+
+        // Signal updated rcCommand values to Failsafe system
+        failsafeUpdateRcCommandValues();
+
+        if (FLIGHT_MODE(HEADFREE_MODE)) {
+            const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
+            const float cosDiff = cos_approx(radDiff);
+            const float sinDiff = sin_approx(radDiff);
+            const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
+            rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
+            rcCommand[PITCH] = rcCommand_PITCH;
+        }
+    }
+
+    if (ARMING_FLAG(ARMED)) {
+        LED0_ON;
+    } else {
+        if (!IS_RC_MODE_ACTIVE(BOXARM) && failsafeIsReceivingRxData()) {
+            ENABLE_ARMING_FLAG(OK_TO_ARM);
+        }
+
+        updatePreArmingChecks();
+
+        if (ARMING_FLAG(OK_TO_ARM)) {
+            warningLedDisable();
+        } else {
+            warningLedFlash();
+        }
+
+        warningLedUpdate();
+    }
+}
+
+void mwDisarm(disarmReason_t disarmReason)
+{
+    if (ARMING_FLAG(ARMED)) {
+        lastDisarmReason = disarmReason;
+        DISABLE_ARMING_FLAG(ARMED);
+
+#ifdef BLACKBOX
+        if (feature(FEATURE_BLACKBOX)) {
+            blackboxFinish();
+        }
+#endif
+
+        beeper(BEEPER_DISARMING);      // emit disarm tone
+    }
+}
+
+disarmReason_t getDisarmReason(void)
+{
+    return lastDisarmReason;
+}
+
+#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
+
+void releaseSharedTelemetryPorts(void) {
+    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    while (sharedPort) {
+        mspSerialReleasePortIfAllocated(sharedPort);
+        sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    }
+}
+
+void mwArm(void)
+{
+    if (ARMING_FLAG(OK_TO_ARM)) {
+        if (ARMING_FLAG(ARMED)) {
+            return;
+        }
+        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
+            return;
+        }
+        if (!ARMING_FLAG(PREVENT_ARMING)) {
+            ENABLE_ARMING_FLAG(ARMED);
+            ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
+            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+
+            resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+
+#ifdef BLACKBOX
+            if (feature(FEATURE_BLACKBOX)) {
+                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
+                if (sharedBlackboxAndMspPort) {
+                    mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
+                }
+                blackboxStart();
+            }
+#endif
+            disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
+
+            //beep to indicate arming
+#ifdef NAV
+            if (navigationPositionEstimateIsHealthy())
+                beeper(BEEPER_ARMING_GPS_FIX);
+            else
+                beeper(BEEPER_ARMING);
+#else
+            beeper(BEEPER_ARMING);
+#endif
+
+            return;
+        }
+    }
+
+    if (!ARMING_FLAG(ARMED)) {
+        beeperConfirmationBeeps(1);
+    }
+}
+
+void processRx(timeUs_t currentTimeUs)
+{
+    static bool armedBeeperOn = false;
+
+    calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
+
+    // in 3D mode, we need to be able to disarm by switch at any time
+    if (feature(FEATURE_3D)) {
+        if (!IS_RC_MODE_ACTIVE(BOXARM))
+            mwDisarm(DISARM_SWITCH_3D);
+    }
+
+    updateRSSI(currentTimeUs);
+
+    // Update failsafe monitoring system
+    if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
+        failsafeStartMonitoring();
+    }
+
+    failsafeUpdateState();
+
+    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+
+    // When armed and motors aren't spinning, do beeps and then disarm
+    // board after delay so users without buzzer won't lose fingers.
+    // mixTable constrains motor commands, so checking  throttleStatus is enough
+    if (ARMING_FLAG(ARMED)
+        && feature(FEATURE_MOTOR_STOP)
+        && !STATE(FIXED_WING)
+    ) {
+        if (isUsingSticksForArming()) {
+            if (throttleStatus == THROTTLE_LOW) {
+                if (armingConfig()->auto_disarm_delay != 0
+                    && (int32_t)(disarmAt - millis()) < 0
+                ) {
+                    // auto-disarm configured and delay is over
+                    mwDisarm(DISARM_TIMEOUT);
+                    armedBeeperOn = false;
+                } else {
+                    // still armed; do warning beeps while armed
+                    beeper(BEEPER_ARMED);
+                    armedBeeperOn = true;
+                }
+            } else {
+                // throttle is not low
+                if (armingConfig()->auto_disarm_delay != 0) {
+                    // extend disarm time
+                    disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
+                }
+
+                if (armedBeeperOn) {
+                    beeperSilence();
+                    armedBeeperOn = false;
+                }
+            }
+        } else {
+            // arming is via AUX switch; beep while throttle low
+            if (throttleStatus == THROTTLE_LOW) {
+                beeper(BEEPER_ARMED);
+                armedBeeperOn = true;
+            } else if (armedBeeperOn) {
+                beeperSilence();
+                armedBeeperOn = false;
+            }
+        }
+    }
+
+    processRcStickPositions(throttleStatus, armingConfig()->disarm_kill_switch, armingConfig()->fixed_wing_auto_arm);
+
+    updateActivatedModes();
+
+    if (!cliMode) {
+        updateAdjustmentStates();
+        processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile));
+    }
+
+    bool canUseHorizonMode = true;
+
+    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
+        // bumpless transfer to Level mode
+        canUseHorizonMode = false;
+
+        if (!FLIGHT_MODE(ANGLE_MODE)) {
+            ENABLE_FLIGHT_MODE(ANGLE_MODE);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
+    }
+
+    if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
+
+        DISABLE_FLIGHT_MODE(ANGLE_MODE);
+
+        if (!FLIGHT_MODE(HORIZON_MODE)) {
+            ENABLE_FLIGHT_MODE(HORIZON_MODE);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(HORIZON_MODE);
+    }
+
+    if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+        LED1_ON;
+    } else {
+        LED1_OFF;
+    }
+
+#ifdef USE_SERVOS
+    /* Flaperon mode */
+    if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
+        if (!FLIGHT_MODE(FLAPERON)) {
+            ENABLE_FLIGHT_MODE(FLAPERON);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(FLAPERON);
+    }
+#endif
+
+#ifdef USE_FLM_TURN_ASSIST
+    /* Turn assistant mode */
+    if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
+        if (!FLIGHT_MODE(TURN_ASSISTANT)) {
+            ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
+    }
+#endif
+
+    if (sensors(SENSOR_ACC)) {
+        if (IS_RC_MODE_ACTIVE(BOXHEADINGHOLD)) {
+            if (!FLIGHT_MODE(HEADING_MODE)) {
+                resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+                ENABLE_FLIGHT_MODE(HEADING_MODE);
+            }
+        } else {
+            DISABLE_FLIGHT_MODE(HEADING_MODE);
+        }
+    }
+
+#if defined(MAG)
+    if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
+        if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
+            if (!FLIGHT_MODE(HEADFREE_MODE)) {
+                ENABLE_FLIGHT_MODE(HEADFREE_MODE);
+            }
+        } else {
+            DISABLE_FLIGHT_MODE(HEADFREE_MODE);
+        }
+        if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
+            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
+        }
+    }
+#endif
+
+<<<<<<< HEAD
+    // Handle passthrough mode
+    if (STATE(FIXED_WING)) {
+        if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
+            (!ARMING_FLAG(ARMED) && isCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating        
+            ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
+        } else {
+            DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
+        }
+=======
+    // Navigation may override PASSTHRU_MODE
+    if (IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) {
+        ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
+    } else {
+        DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
+>>>>>>> naivation to navigation typo fix
+    }
+
+    /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
+       This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
+       Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
+    if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) {
+        /* In PASSTHRU mode we reset integrators prevent I-term wind-up (PID output is not used in PASSTHRU) */
+        pidResetErrorAccumulators();
+    }
+    else {
+        if (throttleStatus == THROTTLE_LOW) {
+            if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
+                rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
+
+                // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
+                if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
+                    ENABLE_STATE(ANTI_WINDUP);
+                }
+                else {
+                    DISABLE_STATE(ANTI_WINDUP);
+                }
+            }
+            else {
+                DISABLE_STATE(ANTI_WINDUP);
+                pidResetErrorAccumulators();
+            }
+        }
+        else {
+            DISABLE_STATE(ANTI_WINDUP);
+        }
+    }
+
+    if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
+        DISABLE_FLIGHT_MODE(HEADFREE_MODE);
+    }
+
+#if defined(AUTOTUNE_FIXED_WING) || defined(AUTOTUNE_MULTIROTOR)
+    autotuneUpdateState();
+#endif
+
+#ifdef TELEMETRY
+    if (feature(FEATURE_TELEMETRY)) {
+        if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
+                (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
+
+            releaseSharedTelemetryPorts();
+        } else {
+            // the telemetry state must be checked immediately so that shared serial ports are released.
+            telemetryCheckState();
+            mspSerialAllocatePorts();
+        }
+    }
+#endif
+
+}
+
+void filterRc(bool isRXDataNew)
+{
+    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
+    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
+    static int16_t factor, rcInterpolationFactor;
+    static biquadFilter_t filteredCycleTimeState;
+    static bool filterInitialised;
+
+    // Calculate average cycle time (1Hz LPF on cycle time)
+    if (!filterInitialised) {
+        biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
+        filterInitialised = true;
+    }
+
+    const timeDelta_t filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
+    rcInterpolationFactor = rxGetRefreshRate() / filteredCycleTime + 1;
+
+    if (isRXDataNew) {
+        for (int channel=0; channel < 4; channel++) {
+            deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            lastCommand[channel] = rcCommand[channel];
+        }
+
+        factor = rcInterpolationFactor - 1;
+    } else {
+        factor--;
+    }
+
+    // Interpolate steps of rcCommand
+    if (factor > 0) {
+        for (int channel=0; channel < 4; channel++) {
+            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+         }
+    } else {
+        factor = 0;
+    }
+}
+
+// Function for loop trigger
+void taskGyro(timeUs_t currentTimeUs) {
+    // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
+    // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
+    const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+
+    if (gyroConfig()->gyroSync) {
+        while (true) {
+            if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(micros(), currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
+                break;
+            }
+        }
+    }
+
+    /* Update actual hardware readings */
+    gyroUpdate();
+
+#ifdef ASYNC_GYRO_PROCESSING
+    /* Update IMU for better accuracy */
+    imuUpdateGyroscope((timeUs_t)currentDeltaTime + (micros() - currentTimeUs));
+#endif
+}
+
+static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
+{
+    if (throttleTiltCompensationStrength) {
+        float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
+        return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
+    } else {
+        return 1.0f;
+    }
+}
+
+void taskMainPidLoop(timeUs_t currentTimeUs)
+{
+    cycleTime = getTaskDeltaTime(TASK_SELF);
+    dT = (float)cycleTime * 0.000001f;
+
+#ifdef ASYNC_GYRO_PROCESSING
+    if (getAsyncMode() == ASYNC_MODE_NONE) {
+        taskGyro(currentTimeUs);
+    }
+
+    if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
+        imuUpdateAccelerometer();
+        imuUpdateAttitude(currentTimeUs);
+    }
+#else
+    /* Update gyroscope */
+    taskGyro(currentTimeUs);
+    imuUpdateAccelerometer();
+    imuUpdateAttitude(currentTimeUs);
+#endif
+
+
+    annexCode();
+
+    if (rxConfig()->rcSmoothing) {
+        filterRc(isRXDataNew);
+    }
+
+#if defined(NAV)
+    if (isRXDataNew) {
+        updateWaypointsAndNavigationMode();
+    }
+#endif
+
+    isRXDataNew = false;
+
+#if defined(NAV)
+    updatePositionEstimator();
+    applyWaypointNavigationAndAltitudeHold();
+#endif
+
+    // If we're armed, at minimum throttle, and we do arming via the
+    // sticks, do not process yaw input from the rx.  We do this so the
+    // motors do not spin up while we are trying to arm or disarm.
+    // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
+    if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
+#ifndef USE_QUAD_MIXER_ONLY
+#ifdef USE_SERVOS
+            && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
+#endif
+            && mixerConfig()->mixerMode != MIXER_AIRPLANE
+            && mixerConfig()->mixerMode != MIXER_FLYING_WING
+            && mixerConfig()->mixerMode != MIXER_CUSTOM_AIRPLANE
+#endif
+    ) {
+        rcCommand[YAW] = 0;
+    }
+
+    // Apply throttle tilt compensation
+    if (!STATE(FIXED_WING)) {
+        int16_t thrTiltCompStrength = 0;
+
+        if (navigationRequiresThrottleTiltCompensation()) {
+            thrTiltCompStrength = 100;
+        }
+        else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+            thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
+        }
+
+        if (thrTiltCompStrength) {
+            rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle
+                                            + (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
+                                            motorConfig()->minthrottle,
+                                            motorConfig()->maxthrottle);
+        }
+    }
+    else {
+        // FIXME: throttle pitch comp for FW
+    }
+
+    // Update PID coefficients
+    updatePIDCoefficients();
+
+    // Calculate stabilisation
+    pidController();
+
+#ifdef HIL
+    if (hilActive) {
+        hilUpdateControlState();
+        motorControlEnable = false;
+    }
+#endif
+
+    mixTable();
+
+#ifdef USE_SERVOS
+    if (isMixerUsingServos()) {
+        servoMixer();
+    }
+    if (feature(FEATURE_SERVO_TILT)) {
+        processServoTilt();
+    }
+    processServoAutotrim();
+    //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
+    if (isServoOutputEnabled()) {
+        writeServos();
+    }
+#endif
+
+    if (motorControlEnable) {
+        writeMotors();
+    }
+
+#ifdef USE_SDCARD
+    afatfs_poll();
+#endif
+
+#ifdef BLACKBOX
+    if (!cliMode && feature(FEATURE_BLACKBOX)) {
+        blackboxUpdate(micros());
+    }
+#endif
+}
+
+bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
+{
+    UNUSED(currentDeltaTime);
+
+    return rxUpdateCheck(currentTimeUs, currentDeltaTime);
+}
+
+void taskUpdateRxMain(timeUs_t currentTimeUs)
+{
+    processRx(currentTimeUs);
+    isRXDataNew = true;
+}

src/main/flight/pid.c

@@ -549,7 +549,7 @@ static uint8_t getHeadingHoldState()
     }
 
 #if defined(NAV)
-    int navHeadingState = naivationGetHeadingControlState();
+    int navHeadingState = navigationGetHeadingControlState();
     // NAV will prevent MAG_MODE from activating, but require heading control
     if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
         // Apply maghold only if heading control is in auto mode
@@ -722,7 +722,7 @@ void pidController(void)
     }
 
 #ifdef USE_FLM_TURN_ASSIST
-    if (FLIGHT_MODE(TURN_ASSISTANT) || naivationRequiresTurnAssistance()) {
+    if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
         pidTurnAssistant(pidState);
     }
 #endif

src/main/navigation/navigation.c

@@ -2417,7 +2417,7 @@ bool navigationRequiresThrottleTiltCompensation(void)
 /*-----------------------------------------------------------
  * An indicator that ANGLE mode must be forced per NAV requirement
  *-----------------------------------------------------------*/
-bool naivationRequiresAngleMode(void)
+bool navigationRequiresAngleMode(void)
 {
     const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
     return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
@@ -2426,7 +2426,7 @@ bool naivationRequiresAngleMode(void)
 /*-----------------------------------------------------------
  * An indicator that TURN ASSISTANCE is required for navigation
  *-----------------------------------------------------------*/
-bool naivationRequiresTurnAssistance(void)
+bool navigationRequiresTurnAssistance(void)
 {
     const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
     if (STATE(FIXED_WING)) {
@@ -2441,7 +2441,7 @@ bool naivationRequiresTurnAssistance(void)
 /**
  * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
  */
-int8_t naivationGetHeadingControlState(void)
+int8_t navigationGetHeadingControlState(void)
 {
     // For airplanes report as manual heading control
     if (STATE(FIXED_WING)) {
@@ -2462,7 +2462,7 @@ int8_t naivationGetHeadingControlState(void)
     }
 }
 
-bool naivationBlockArming(void)
+bool navigationBlockArming(void)
 {
     const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD);
     const bool navLaunchComboModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP));

src/main/navigation/navigation.h

@@ -255,11 +255,11 @@ void updatePositionEstimator(void);
 void applyWaypointNavigationAndAltitudeHold(void);
 
 /* Functions to signal navigation requirements to main loop */
-bool naivationRequiresAngleMode(void);
+bool navigationRequiresAngleMode(void);
 bool navigationRequiresThrottleTiltCompensation(void);
-bool naivationRequiresTurnAssistance(void);
+bool navigationRequiresTurnAssistance(void);
-int8_t naivationGetHeadingControlState(void);
+int8_t navigationGetHeadingControlState(void);
-bool naivationBlockArming(void);
+bool navigationBlockArming(void);
 bool navigationPositionEstimateIsHealthy(void);
 bool navIsCalibrationComplete(void);
 
@@ -315,8 +315,8 @@ extern int16_t navAccNEU[3];
 
 #else
 
-#define naivationRequiresAngleMode() (0)
+#define navigationRequiresAngleMode() (0)
-#define naivationGetHeadingControlState() (0)
+#define navigationGetHeadingControlState() (0)
 #define navigationRequiresThrottleTiltCompensation() (0)
 #define getEstimatedActualVelocity(axis) (0)