自学教程：C++ FLIGHT_MODE函数代码示例

void updateAutotuneState(void){    static bool landedAfterAutoTuning = false;    static bool autoTuneWasUsed = false;    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {        if (!FLIGHT_MODE(AUTOTUNE_MODE)) {            if (ARMING_FLAG(ARMED)) {                if (isAutotuneIdle() || landedAfterAutoTuning) {                    autotuneReset();                    landedAfterAutoTuning = false;                }                autotuneBeginNextPhase(&currentProfile->pidProfile);                ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);                autoTuneWasUsed = true;            } else {                if (havePidsBeenUpdatedByAutotune()) {                    saveConfigAndNotify();                    autotuneReset();                }            }        }        return;    }    if (FLIGHT_MODE(AUTOTUNE_MODE)) {        autotuneEndPhase();        DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);    }    if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {        landedAfterAutoTuning = true;    }}

void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,        uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig){    float horizonLevelStrength = 0.0f;    if (FLIGHT_MODE(HORIZON_MODE)) {        // (convert 0-100 range to 0.0-1.0 range)        horizonLevelStrength = (float)calcHorizonLevelStrength(rxConfig->midrc, pidProfile->horizon_tilt_effect,                pidProfile->horizon_tilt_mode, pidProfile->D8[PIDLEVEL]) / 100.0f;    }    // ----------PID controller----------    for (int axis = 0; axis < 3; axis++) {        const uint8_t rate = controlRateConfig->rates[axis];        // -----Get the desired angle rate depending on flight mode        float angleRate;        if (axis == FD_YAW) {            // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate            angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;        } else {            // control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID            angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate            if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {                // calculate error angle and limit the angle to the max inclination                // multiplication of rcCommand corresponds to changing the sticks scaling here#ifdef GPS                const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)                        - attitude.raw[axis] + angleTrim->raw[axis];#else                const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)                        - attitude.raw[axis] + angleTrim->raw[axis];#endif                if (FLIGHT_MODE(ANGLE_MODE)) {                    // ANGLE mode                    angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;                } else {                    // HORIZON mode                    // mix in errorAngle to desired angleRate to add a little auto-level feel.                    // horizonLevelStrength has been scaled to the stick input                    angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;                }            }        }        // --------low-level gyro-based PID. ----------        const float gyroRate = luxGyroScale * gyroADCf[axis] * gyro.scale;        axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);        //axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);#ifdef GTUNE        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            calculate_Gtune(axis);        }#endif    }}

static void ltm_sframe(void){    uint8_t lt_flightmode;    uint8_t lt_statemode;    if (FLIGHT_MODE(PASSTHRU_MODE))        lt_flightmode = 0;    else if (FLIGHT_MODE(NAV_WP_MODE))        lt_flightmode = 10;    else if (FLIGHT_MODE(NAV_RTH_MODE))        lt_flightmode = 13;    else if (FLIGHT_MODE(NAV_POSHOLD_MODE))        lt_flightmode = 9;    else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE))        lt_flightmode = 11;    else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))        lt_flightmode = 8;    else if (FLIGHT_MODE(ANGLE_MODE))        lt_flightmode = 2;    else if (FLIGHT_MODE(HORIZON_MODE))        lt_flightmode = 3;    else        lt_flightmode = 1;      // Rate mode    lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;    if (failsafeIsActive())        lt_statemode |= 2;    ltm_initialise_packet('S');    ltm_serialise_16(vbat * 100);    //vbat converted to mv    ltm_serialise_16(0);             //  current, not implemented    ltm_serialise_8((uint8_t)((rssi * 254) / 1023));        // scaled RSSI (uchar)    ltm_serialise_8(0);              // no airspeed    ltm_serialise_8((lt_flightmode << 2) | lt_statemode);    ltm_finalise();}

void ltm_sframe(sbuf_t *dst){    uint8_t lt_flightmode;    if (FLIGHT_MODE(PASSTHRU_MODE))        lt_flightmode = 0;    else if (FLIGHT_MODE(NAV_WP_MODE))        lt_flightmode = 10;    else if (FLIGHT_MODE(NAV_RTH_MODE))        lt_flightmode = 13;    else if (FLIGHT_MODE(NAV_POSHOLD_MODE))        lt_flightmode = 9;    else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE))        lt_flightmode = 11;    else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))        lt_flightmode = 8;    else if (FLIGHT_MODE(ANGLE_MODE))        lt_flightmode = 2;    else if (FLIGHT_MODE(HORIZON_MODE))        lt_flightmode = 3;    else        lt_flightmode = 1;      // Rate mode    uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;    if (failsafeIsActive())        lt_statemode |= 2;    sbufWriteU8(dst, 'S');    ltm_serialise_16(dst, vbat * 100);    //vbat converted to mv    ltm_serialise_16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF));    // current mAh (65535 mAh max)    ltm_serialise_8(dst, (uint8_t)((rssi * 254) / 1023));        // scaled RSSI (uchar)    ltm_serialise_8(dst, 0);              // no airspeed    ltm_serialise_8(dst, (lt_flightmode << 2) | lt_statemode);}

void updateAltHoldState(void){    // Baro alt hold activate    if (!IS_RC_MODE_ACTIVE(BOXBARO)) {        DISABLE_FLIGHT_MODE(BARO_MODE);        return;         }    if (!FLIGHT_MODE(BARO_MODE)) {        //led0_op(false);        ENABLE_FLIGHT_MODE(BARO_MODE);           AltHold = EstAlt;//+100;//drona pras                      //DRONA        //initialThrottleHold = rcData[THROTTLE];//Drona pras     //        initialThrottleHold = 1500;//Drona pras     //        errorVelocityI = 0;                               //DRONA        altHoldThrottleAdjustment = 0;                    //DRONA    }    else    {           //led0_op(true);//drona pras    }    initialThrottleHold_test=initialThrottleHold;           //DRONA    //debug_d0 = pidProfile->D8[PIDALT];    debug_e1 = rcCommand[THROTTLE];                      //DRONA}

uint8_t getMagHoldState(){    #ifndef MAG        return MAG_HOLD_DISABLED;    #endif    if (!sensors(SENSOR_MAG) || !STATE(SMALL_ANGLE)) {        return MAG_HOLD_DISABLED;    }#if defined(NAV)    int navHeadingState = naivationGetHeadingControlState();    // 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        if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {            return MAG_HOLD_ENABLED;        }    }    else#endif    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {        return MAG_HOLD_ENABLED;    } else {        return MAG_HOLD_UPDATE_HEADING;    }    return MAG_HOLD_UPDATE_HEADING;}

static void pidLevel(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude){    // This is ROLL/PITCH, run ANGLE/HORIZON controllers    const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile->max_angle_inclination[axis]);    const float angleError = angleTarget - attitude.raw[axis];    float angleRateTarget = constrainf(angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER), -controlRateConfig->rates[axis] * 10.0f, controlRateConfig->rates[axis] * 10.0f);    // Apply simple LPF to angleRateTarget to make response less jerky    // Ideas behind this:    //  1) Attitude is updated at gyro rate, rateTarget for ANGLE mode is calculated from attitude    //  2) If this rateTarget is passed directly into gyro-base PID controller this effectively doubles the rateError.    //     D-term that is calculated from error tend to amplify this even more. Moreover, this tend to respond to every    //     slightest change in attitude making self-leveling jittery    //  3) Lowering LEVEL P can make the effects of (2) less visible, but this also slows down self-leveling.    //  4) Human pilot response to attitude change in RATE mode is fairly slow and smooth, human pilot doesn't    //     compensate for each slightest change    //  5) (2) and (4) lead to a simple idea of adding a low-pass filter on rateTarget for ANGLE mode damping    //     response to rapid attitude changes and smoothing out self-leveling reaction    if (pidProfile->I8[PIDLEVEL]) {        // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz        angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidProfile->I8[PIDLEVEL], dT);    }    // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)    if (FLIGHT_MODE(HORIZON_MODE)) {        pidState->rateTarget = (1.0f - horizonRateMagnitude) * angleRateTarget + horizonRateMagnitude * pidState->rateTarget;    } else {        pidState->rateTarget = angleRateTarget;    }}

void applyLedModeLayer(void){    const ledConfig_t *ledConfig;    uint8_t ledIndex;    for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {        ledConfig = &ledConfigs[ledIndex];        if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) {            if (ledConfig->flags & LED_FUNCTION_COLOR) {                setLedHsv(ledIndex, &colors[ledConfig->color]);            } else {                setLedHsv(ledIndex, &hsv_black);            }        }        if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) {            if (ledConfig->flags & LED_FUNCTION_ARM_STATE) {                if (!ARMING_FLAG(ARMED)) {                    setLedHsv(ledIndex, &hsv_green);                } else {                    setLedHsv(ledIndex, &hsv_blue);                }            }            continue;        }        applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors);        if (FLIGHT_MODE(HEADFREE_MODE)) {            applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors);#ifdef MAG        } else if (FLIGHT_MODE(MAG_MODE)) {            applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors);#endif#ifdef BARO        } else if (FLIGHT_MODE(BARO_MODE)) {            applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors);#endif        } else if (FLIGHT_MODE(HORIZON_MODE)) {            applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors);        } else if (FLIGHT_MODE(ANGLE_MODE)) {            applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors);        }    }}

void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig){    uint8_t magHoldState = getMagHoldState();    if (magHoldState == MAG_HOLD_UPDATE_HEADING) {        updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));    }    for (int axis = 0; axis < 3; axis++) {        // Step 1: Calculate gyro rates        pidState[axis].gyroRate = gyroADC[axis] * gyro.scale;        // Step 2: Read target        float rateTarget;        if (axis == FD_YAW && magHoldState == MAG_HOLD_ENABLED) {            rateTarget = pidMagHold(pidProfile);        } else {            rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]);        }        // Limit desired rate to something gyro can measure reliably        pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);    }    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK    if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {        const float horizonRateMagnitude = calcHorizonRateMagnitude(pidProfile, rxConfig);        pidLevel(pidProfile, controlRateConfig, &pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude);        pidLevel(pidProfile, controlRateConfig, &pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude);    }    if (FLIGHT_MODE(HEADING_LOCK) && magHoldState != MAG_HOLD_ENABLED) {        pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);    }    if (FLIGHT_MODE(TURN_ASSISTANT)) {        pidTurnAssistant(pidState);    }    // Step 4: Run gyro-driven control    for (int axis = 0; axis < 3; axis++) {        // Apply PID setpoint controller        pidApplyRateController(pidProfile, &pidState[axis], axis);     // scale gyro rate to DPS    }}

void annexCode(void){    int32_t throttleValue;    // Compute ROLL PITCH and YAW command    rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);    rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);    rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband);    //Compute THROTTLE command    throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);    throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]    rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);    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) == 0) {            ENABLE_ARMING_FLAG(OK_TO_ARM);        }        if (!STATE(SMALL_ANGLE)) {            DISABLE_ARMING_FLAG(OK_TO_ARM);        }        if (isCalibrating() || isSystemOverloaded()) {            warningLedFlash();            DISABLE_ARMING_FLAG(OK_TO_ARM);        }#if defined(NAV)        if (naivationBlockArming()) {            DISABLE_ARMING_FLAG(OK_TO_ARM);        }#endif        if (ARMING_FLAG(OK_TO_ARM)) {            warningLedDisable();        } else {            warningLedFlash();        }        warningLedUpdate();    }    // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.    if (gyro.temperature)        gyro.temperature(&telemTemperature1);}

static void ltm_sframe(void){    uint8_t lt_flightmode;    uint8_t lt_statemode;    if (FLIGHT_MODE(PASSTHRU_MODE))        lt_flightmode = 0;    else if (FLIGHT_MODE(GPS_HOME_MODE))        lt_flightmode = 13;    else if (FLIGHT_MODE(GPS_HOLD_MODE))        lt_flightmode = 9;    else if (FLIGHT_MODE(HEADFREE_MODE))        lt_flightmode = 4;    else if (FLIGHT_MODE(BARO_MODE))        lt_flightmode = 8;    else if (FLIGHT_MODE(ANGLE_MODE))        lt_flightmode = 2;    else if (FLIGHT_MODE(HORIZON_MODE))        lt_flightmode = 3;    else        lt_flightmode = 1;      // Rate mode    lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;    if (failsafeIsActive())        lt_statemode |= 2;    ltm_initialise_packet('S');    ltm_serialise_16(getBatteryVoltage() * 100);    //vbat converted to mv    ltm_serialise_16(0);             //  current, not implemented    ltm_serialise_8((uint8_t)((getRssi() * 254) / 1023));        // scaled RSSI (uchar)    ltm_serialise_8(0);              // no airspeed    ltm_serialise_8((lt_flightmode << 2) | lt_statemode);    ltm_finalise();}

void autotuneUpdateState(void){    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && ARMING_FLAG(ARMED)) {        if (!FLIGHT_MODE(AUTO_TUNE)) {            autotuneStart();            ENABLE_FLIGHT_MODE(AUTO_TUNE);        }        else {            autotuneCheckUpdateGains();        }    } else {        if (FLIGHT_MODE(AUTO_TUNE)) {            autotuneUpdateGains(tuneSaved);        }        DISABLE_FLIGHT_MODE(AUTO_TUNE);    }}

void updateGtuneState(void){    static bool GTuneWasUsed = false;    if (rcModeIsActive(BOXGTUNE)) {        if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            ENABLE_FLIGHT_MODE(GTUNE_MODE);            init_Gtune();            GTuneWasUsed = true;        }        if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {            saveConfigAndNotify();            GTuneWasUsed = false;        }    } else {        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            DISABLE_FLIGHT_MODE(GTUNE_MODE);        }    }}

void updateGtuneState(void){    static bool GTuneWasUsed = false;    if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {        if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            ENABLE_FLIGHT_MODE(GTUNE_MODE);            init_Gtune(&currentProfile->pidProfile);            GTuneWasUsed = true;        }        if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {            saveConfigAndNotify();            GTuneWasUsed = false;        }    } else {        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            DISABLE_FLIGHT_MODE(GTUNE_MODE);        }    }}

static void applyLedFixedLayers(){    for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {        const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];        hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);        int fn = ledGetFunction(ledConfig);        int hOffset = HSV_HUE_MAX;        switch (fn) {            case LED_FUNCTION_COLOR:                color = ledStripConfig()->colors[ledGetColor(ledConfig)];                break;            case LED_FUNCTION_FLIGHT_MODE:                for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)                    if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {                        const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);                        if (directionalColor) {                            color = *directionalColor;                        }                        break; // stop on first match                    }                break;            case LED_FUNCTION_ARM_STATE:                color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);                break;            case LED_FUNCTION_BATTERY:                color = HSV(RED);                hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120);                break;            case LED_FUNCTION_RSSI:                color = HSV(RED);                hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);                break;            default:                break;        }        if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {            hOffset += scaledAux;        }        color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);        setLedHsv(ledIndex, &color);    }}

void updateMagHold(void){    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {        int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;        if (dif <= -180)            dif += 360;        if (dif >= +180)            dif -= 360;        dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);        if (STATE(SMALL_ANGLE))            rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30;    // 18 deg    } else        magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);}

void updateMagHold(void){    if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {        int16_t dif = heading - magHold;        if (dif <= -180)            dif += 360;        if (dif >= +180)            dif -= 360;        dif *= -masterConfig.yaw_control_direction;        if (STATE(SMALL_ANGLE))            rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg    } else        magHold = heading;}

void updateMagHold(void){    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {        int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;        if (dif <= -180)            dif += 360;        if (dif >= +180)            dif -= 360;        dif *= -masterConfig.yaw_control_direction;        if (STATE(SMALL_ANGLE))            rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg    } else        magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);}

void updateSonarAltHoldState(void){    // Sonar alt hold activate    if (!IS_RC_MODE_ACTIVE(BOXSONAR)) {        DISABLE_FLIGHT_MODE(SONAR_MODE);        return;    }    if (!FLIGHT_MODE(SONAR_MODE)) {        ENABLE_FLIGHT_MODE(SONAR_MODE);        AltHold = EstAlt;        initialThrottleHold = rcData[THROTTLE];        errorVelocityI = 0;        altHoldThrottleAdjustment = 0;    }}

void updateSonarAltHoldState(void){    // Sonar alt hold activate    if (!rcOptions[BOXSONAR]) {        DISABLE_FLIGHT_MODE(SONAR_MODE);        return;    }    if (!FLIGHT_MODE(SONAR_MODE)) {        ENABLE_FLIGHT_MODE(SONAR_MODE);        AltHold = EstAlt;        initialThrottleHold = rcCommand[THROTTLE];        errorVelocityI = 0;        altHoldThrottleAdjustment = 0;    }}

void updateAltHoldState(void){    // Baro alt hold activate    if (!IS_RC_MODE_ACTIVE(BOXBARO)) {        DISABLE_FLIGHT_MODE(BARO_MODE);        return;    }    if (!FLIGHT_MODE(BARO_MODE)) {        ENABLE_FLIGHT_MODE(BARO_MODE);        AltHold = estimatedAltitude;        initialThrottleHold = rcData[THROTTLE];        errorVelocityI = 0;        altHoldThrottleAdjustment = 0;    }}

static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,        rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig){    UNUSED(rxConfig);    int axis;    int32_t PTerm, ITerm, DTerm, delta;    static int32_t lastError[3] = { 0, 0, 0 };    static int32_t previousErrorGyroI[3] = { 0, 0, 0 };    int32_t AngleRateTmp, RateError, gyroRate;    int8_t horizonLevelStrength = 100;    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);        deltaStateIsSet = true;    }    if (FLIGHT_MODE(HORIZON_MODE)) {        // Figure out the raw stick positions        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));        const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);        // Progressively turn off the horizon self level strength as the stick is banged over        horizonLevelStrength = (500 - mostDeflectedPos) / 5;  // 100 at centre stick, 0 = max stick deflection        // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.        // For more rate mode increase D and slower flips and rolls will be possible        horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);    }    // ----------PID controller----------    for (axis = 0; axis < 3; axis++) {        uint8_t rate = 10;        // -----Get the desired angle rate depending on flight mode        if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {            rate = controlRateConfig->rates[axis];        }        // -----Get the desired angle rate depending on flight mode        if (axis == FD_YAW) {            // YAW is always gyro-controlled (MAG correction is applied to rcCommand)            AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5;        } else {

void updateGpsWaypointsAndMode(void){    static uint8_t GPSNavReset = 1;    if (STATE(GPS_FIX) && GPS_numSat >= 5) {        // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority        if (rcOptions[BOXGPSHOME]) {            if (!STATE(GPS_HOME_MODE)) {                ENABLE_FLIGHT_MODE(GPS_HOME_MODE);                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);                GPSNavReset = 0;                GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);                nav_mode = NAV_MODE_WP;            }        } else {            DISABLE_STATE(GPS_HOME_MODE);            if (rcOptions[BOXGPSHOLD] && areSticksInApModePosition(gpsProfile->ap_mode)) {                if (!FLIGHT_MODE(GPS_HOLD_MODE)) {                    ENABLE_STATE(GPS_HOLD_MODE);                    GPSNavReset = 0;                    GPS_hold[LAT] = GPS_coord[LAT];                    GPS_hold[LON] = GPS_coord[LON];                    GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);                    nav_mode = NAV_MODE_POSHOLD;                }            } else {                DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);                // both boxes are unselected here, nav is reset if not already done                if (GPSNavReset == 0) {                    GPSNavReset = 1;                    GPS_reset_nav();                }            }        }    } else {        DISABLE_FLIGHT_MODE(GPS_HOME_MODE);        DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);        nav_mode = NAV_MODE_NONE;    }}

void annexCode(void){    if (FLIGHT_MODE(HEADFREE_MODE)) {        float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);        float cosDiff = cos_approx(radDiff);        float sinDiff = sin_approx(radDiff);        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 (rcModeIsActive(BOXARM) == 0) {            ENABLE_ARMING_FLAG(OK_TO_ARM);        }        if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {            DISABLE_ARMING_FLAG(OK_TO_ARM);        }        if (isCalibrating() || isSystemOverloaded()) {            warningLedFlash();            DISABLE_ARMING_FLAG(OK_TO_ARM);        } else {            if (ARMING_FLAG(OK_TO_ARM)) {                warningLedDisable();            } else {                warningLedFlash();            }        }        warningLedUpdate();    }    // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.    if (gyro.temperature)        gyro.temperature(&telemTemperature1);}

//.........这里部分代码省略.........                    smartPortHasRequest = 0;                }                break;            case FSSP_DATAID_HEADING    :                smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg                smartPortHasRequest = 0;                break;            case FSSP_DATAID_ACCX       :                smartPortSendPackage(id, accSmooth[X] / 44);                // unknown input and unknown output unit                // we can only show 00.00 format, another digit won't display right on Taranis                // dividing by roughly 44 will give acceleration in G units                smartPortHasRequest = 0;                break;            case FSSP_DATAID_ACCY       :                smartPortSendPackage(id, accSmooth[Y] / 44);                smartPortHasRequest = 0;                break;            case FSSP_DATAID_ACCZ       :                smartPortSendPackage(id, accSmooth[Z] / 44);                smartPortHasRequest = 0;                break;            case FSSP_DATAID_T1         :                // we send all the flags as decimal digits for easy reading                // the t1Cnt simply allows the telemetry view to show at least some changes                t1Cnt++;                if (t1Cnt >= 4) {                    t1Cnt = 1;                }                tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off                // the Taranis seems to be able to fit 5 digits on the screen                // the Taranis seems to consider this number a signed 16 bit integer                if (ARMING_FLAG(OK_TO_ARM))                    tmpi += 1;                if (ARMING_FLAG(PREVENT_ARMING))                    tmpi += 2;                if (ARMING_FLAG(ARMED))                    tmpi += 4;                if (FLIGHT_MODE(ANGLE_MODE))                    tmpi += 10;                if (FLIGHT_MODE(HORIZON_MODE))                    tmpi += 20;                if (FLIGHT_MODE(UNUSED_MODE))                    tmpi += 40;                if (FLIGHT_MODE(PASSTHRU_MODE))                    tmpi += 40;                if (FLIGHT_MODE(MAG_MODE))                    tmpi += 100;                if (FLIGHT_MODE(BARO_MODE))                    tmpi += 200;                if (FLIGHT_MODE(SONAR_MODE))                    tmpi += 400;                if (FLIGHT_MODE(GPS_HOLD_MODE))                    tmpi += 1000;                if (FLIGHT_MODE(GPS_HOME_MODE))                    tmpi += 2000;                if (FLIGHT_MODE(HEADFREE_MODE))                    tmpi += 4000;                smartPortSendPackage(id, (uint32_t)tmpi);                smartPortHasRequest = 0;                break;            case FSSP_DATAID_T2         :                if (sensors(SENSOR_GPS)) {#ifdef GPS                    // provide GPS lock status                    smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);                    smartPortHasRequest = 0;#endif                }                else if (feature(FEATURE_GPS)) {                    smartPortSendPackage(id, 0);                    smartPortHasRequest = 0;                }                break;#ifdef GPS            case FSSP_DATAID_GPS_ALT    :                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {                    smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)                    smartPortHasRequest = 0;                }                break;#endif            case FSSP_DATAID_A4         :                if (feature(FEATURE_VBAT)) {                    smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts                    smartPortHasRequest = 0;                }                break;            default:                break;                // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start        }    }}

// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.// Based on 2DOF reference design (matlab)void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig){    float errorRate = 0, rP = 0, rD = 0, PVRate = 0;    float ITerm,PTerm,DTerm;    static float lastRateError[2];    static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];    float delta;    int axis;    float horizonLevelStrength = 1;    float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float    initFilters(pidProfile);    if (FLIGHT_MODE(HORIZON_MODE)) {        // Figure out the raw stick positions        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));        const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);        // Progressively turn off the horizon self level strength as the stick is banged over        horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection        if(pidProfile->D8[PIDLEVEL] == 0){            horizonLevelStrength = 0;        } else {            horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);        }    }    // Yet Highly experimental and under test and development    // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)    static float kiThrottleGain = 1.0f;    if (pidProfile->itermThrottleGain) {        const uint16_t maxLoopCount = 20000 / targetPidLooptime;        const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;        static int16_t previousThrottle;        static uint16_t loopIncrement;        if (loopIncrement >= maxLoopCount) {            kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5            previousThrottle = rcCommand[THROTTLE];            loopIncrement = 0;        } else {            loopIncrement++;        }    }    // ----------PID controller----------    for (axis = 0; axis < 3; axis++) {        static uint8_t configP[3], configI[3], configD[3];        // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now        // Prepare all parameters for PID controller        if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {            Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];            Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];            Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];            b[axis] = pidProfile->ptermSetpointWeight / 100.0f;            c[axis] = pidProfile->dtermSetpointWeight / 100.0f;            yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();            configP[axis] = pidProfile->P8[axis];            configI[axis] = pidProfile->I8[axis];            configD[axis] = pidProfile->D8[axis];        }        // Limit abrupt yaw inputs / stops        float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;        if (maxVelocity) {            float currentVelocity = setpointRate[axis] - previousSetpoint[axis];            if (ABS(currentVelocity) > maxVelocity) {                setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;            }            previousSetpoint[axis] = setpointRate[axis];        }        // Yaw control is GYRO based, direct sticks control is applied to rate PID        if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {            // calculate error angle and limit the angle to the max inclination#ifdef GPS                const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here#else                const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here#endif            if (FLIGHT_MODE(ANGLE_MODE)) {                // ANGLE mode - control is angle based, so control loop is needed                setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;            } else {                // HORIZON mode - direct sticks control is applied to rate PID                // mix up angle error to desired AngleRate to add a little auto-level feel                setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);            }        }        PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec//.........这里部分代码省略.........

STATIC_UNIT_TESTED void servoMixer(void){    int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]    static int16_t currentOutput[MAX_SERVO_RULES];    uint8_t i;    if (FLIGHT_MODE(PASSTHRU_MODE)) {        // Direct passthru from RX        input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];        input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];        input[INPUT_STABILIZED_YAW] = rcCommand[YAW];    } else {        // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui        input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];        input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];        input[INPUT_STABILIZED_YAW] = axisPID[YAW];        // Reverse yaw servo when inverted in 3D mode        if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {            input[INPUT_STABILIZED_YAW] *= -1;        }    }    input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);    input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);    input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]    // center the RC input value around the RC middle value    // by subtracting the RC middle value from the RC input value, we get:    // data - middle = input    // 2000 - 1500 = +500    // 1500 - 1500 = 0    // 1000 - 1500 = -500    input[INPUT_RC_ROLL]     = rcData[ROLL]     - rxConfig->midrc;    input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig->midrc;    input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig->midrc;    input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;    input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig->midrc;    input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig->midrc;    input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig->midrc;    input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig->midrc;    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)        servo[i] = 0;    // mix servos according to rules    for (i = 0; i < servoRuleCount; i++) {        // consider rule if no box assigned or box is active        if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {            uint8_t target = currentServoMixer[i].targetChannel;            uint8_t from = currentServoMixer[i].inputSource;            uint16_t servo_width = servoConf[target].max - servoConf[target].min;            int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;            int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;            if (currentServoMixer[i].speed == 0)                currentOutput[i] = input[from];            else {                if (currentOutput[i] < input[from])                    currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);                else if (currentOutput[i] > input[from])                    currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);            }            servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);        } else {            currentOutput[i] = 0;        }    }    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {        servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;        servo[i] += determineServoMiddleOrForwardFromChannel(i);    }}

void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,        uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig){    pidFilterIsSetCheck(pidProfile);    float horizonLevelStrength;    if (FLIGHT_MODE(HORIZON_MODE)) {        // Figure out the most deflected stick position        const int32_t stickPosAil = ABS(getRcStickDeflection(ROLL, rxConfig->midrc));        const int32_t stickPosEle = ABS(getRcStickDeflection(PITCH, rxConfig->midrc));        const int32_t mostDeflectedPos =  MAX(stickPosAil, stickPosEle);        // Progressively turn off the horizon self level strength as the stick is banged over        horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection        if(pidProfile->D8[PIDLEVEL] == 0){            horizonLevelStrength = 0;        } else {            horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);        }    }    // ----------PID controller----------    for (int axis = 0; axis < 3; axis++) {        const uint8_t rate = controlRateConfig->rates[axis];        // -----Get the desired angle rate depending on flight mode        float angleRate;        if (axis == FD_YAW) {            // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate            angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;        } else {            // control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID            angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate            if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {                // calculate error angle and limit the angle to the max inclination                // multiplication of rcCommand corresponds to changing the sticks scaling here#ifdef GPS                const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)                        - attitude.raw[axis] + angleTrim->raw[axis];#else                const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)                        - attitude.raw[axis] + angleTrim->raw[axis];#endif                if (FLIGHT_MODE(ANGLE_MODE)) {                    // ANGLE mode                    angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;                } else {                    // HORIZON mode                    // mix in errorAngle to desired angleRate to add a little auto-level feel.                    // horizonLevelStrength has been scaled to the stick input                    angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;                }            }        }        // --------low-level gyro-based PID. ----------        const float gyroRate = luxGyroScale * gyroADC[axis] * gyro.scale;        axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);        //axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);#ifdef GTUNE        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {            calculate_Gtune(axis);        }#endif    }}

void loop(void){    static uint32_t loopTime;#if defined(BARO) || defined(SONAR)    static bool haveProcessedAnnexCodeOnce = false;#endif    updateRx(currentTime);    if (shouldProcessRx(currentTime)) {        processRx();        isRXDataNew = true;#ifdef BARO        // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.        if (haveProcessedAnnexCodeOnce) {            if (sensors(SENSOR_BARO)) {                updateAltHoldState();            }        }#endif#ifdef SONAR        // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.        if (haveProcessedAnnexCodeOnce) {            if (sensors(SENSOR_SONAR)) {                updateSonarAltHoldState();            }        }#endif    } else {        // not processing rx this iteration        executePeriodicTasks();        // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck        // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will        // change this based on available hardware#ifdef GPS        if (feature(FEATURE_GPS)) {            gpsThread();        }#endif    }    currentTime = micros();    if (shouldRunLoop(loopTime)) {        loopTime = currentTime + targetLooptime;        imuUpdate(&currentProfile->accelerometerTrims);        // Measure loop rate just after reading the sensors        currentTime = micros();        cycleTime = (int32_t)(currentTime - previousTime);        previousTime = currentTime;        dT = (float)cycleTime * 0.000001f;        annexCode();        if (masterConfig.rxConfig.rcSmoothing) {            filterRc();        }#if defined(BARO) || defined(SONAR)        haveProcessedAnnexCodeOnce = true;#endif#ifdef MAG        if (sensors(SENSOR_MAG)) {            updateMagHold();        }#endif#ifdef GTUNE        updateGtuneState();#endif#if defined(BARO) || defined(SONAR)        if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {            if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {                applyAltHold(&masterConfig.airplaneConfig);            }        }#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] <= masterConfig.rxConfig.mincheck#ifndef USE_QUAD_MIXER_ONLY#ifdef USE_SERVOS                && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)#endif                && masterConfig.mixerMode != MIXER_AIRPLANE                && masterConfig.mixerMode != MIXER_FLYING_WING#endif        ) {            rcCommand[YAW] = 0;//.........这里部分代码省略.........

void processRx(void){    static bool armedBeeperOn = false;    calculateRxChannelsAndUpdateFailsafe(currentTime);    // 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();    }    updateRSSI(currentTime);    if (feature(FEATURE_FAILSAFE)) {        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {            failsafeStartMonitoring();        }        failsafeUpdateState();    }    throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);    if (throttleStatus == THROTTLE_LOW) {        pidResetErrorAngle();        pidResetErrorGyro();    }    // 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 (masterConfig.auto_disarm_delay != 0                    && (int32_t)(disarmAt - millis()) < 0                ) {                    // auto-disarm configured and delay is over                    mwDisarm();                    armedBeeperOn = false;                } else {                    // still armed; do warning beeps while armed                    beeper(BEEPER_ARMED);                    armedBeeperOn = true;                }            } else {                // throttle is not low                if (masterConfig.auto_disarm_delay != 0) {                    // extend disarm time                    disarmAt = millis() + masterConfig.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(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {        updateInflightCalibrationState();    }    updateActivatedModes(currentProfile->modeActivationConditions);    if (!cliMode) {        updateAdjustmentStates(currentProfile->adjustmentRanges);        processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);    }    bool canUseHorizonMode = true;    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {        // bumpless transfer to Level mode        canUseHorizonMode = false;        if (!FLIGHT_MODE(ANGLE_MODE)) {            pidResetErrorAngle();            ENABLE_FLIGHT_MODE(ANGLE_MODE);        }    } else {        DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support    }//.........这里部分代码省略.........