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

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();}

static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer){    static uint8_t rotationPhase;    int ledRingIndex = 0;    if (updateNow) {        rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1;        int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10;        *timer += LED_STRIP_HZ(5) * 10 / scale;  // 5 - 50Hz update rate    }    for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {        const ledConfig_t *ledConfig = &currentLedStripConfig->ledConfigs[ledIndex];        if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) {            bool applyColor;            if (ARMING_FLAG(ARMED)) {                applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH;            } else {                applyColor = !(ledRingIndex % 2); // alternating pattern            }            if (applyColor) {                const hsvColor_t *ringColor = &currentLedStripConfig->colors[ledGetColor(ledConfig)];                setLedHsv(ledIndex, ringColor);            }            ledRingIndex++;        }    }}

void taskUpdateBattery(void){    static uint32_t vbatLastServiced = 0;    static uint32_t ibatLastServiced = 0;    if (feature(FEATURE_VBAT)) {        if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {            vbatLastServiced = currentTime;            voltageMeterUpdate();            batteryUpdate();        }    }    if (feature(FEATURE_AMPERAGE_METER)) {        int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);        if (ibatTimeSinceLastServiced >= IBATINTERVAL) {            ibatLastServiced = currentTime;            if (batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC) {                amperageUpdateMeter(ibatTimeSinceLastServiced);            } else {                throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle);                bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));                int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;                amperageUpdateVirtualMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);            }        }    }}

void disarm(void){    if (ARMING_FLAG(ARMED)) {        DISABLE_ARMING_FLAG(ARMED);        lastDisarmTimeUs = micros();#ifdef USE_BLACKBOX        if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON            blackboxFinish();        }#endif        BEEP_OFF;#ifdef USE_DSHOT        if (isMotorProtocolDshot() && flipOverAfterCrashMode && !feature(FEATURE_3D)) {            pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);        }#endif        flipOverAfterCrashMode = false;        // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead        if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) {            beeper(BEEPER_DISARMING);      // emit disarm tone        }    }}

void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle){    static int32_t amperageRaw = 0;    static int64_t mAhdrawnRaw = 0;    int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;    int32_t throttleFactor = 0;    switch(batteryConfig->currentMeterType) {        case CURRENT_SENSOR_ADC:            amperageRaw -= amperageRaw / 8;            amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));            amperage = currentSensorToCentiamps(amperageRaw / 8);            break;        case CURRENT_SENSOR_VIRTUAL:            amperage = (int32_t)batteryConfig->currentMeterOffset;            if (ARMING_FLAG(ARMED)) {                throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);                if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))                    throttleOffset = 0;                throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);                amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale  / 1000;            }            break;        case CURRENT_SENSOR_NONE:            amperage = 0;            break;    }    mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;    mAhDrawn = mAhdrawnRaw / (3600 * 100);}

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();}

static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer){    static uint8_t rotationPhase;    int ledRingIndex = 0;    if (updateNow) {        rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1;        *timer += HZ_TO_US(5 + (45 * scaledThrottle) / 100);  // 5 - 50Hz update rate    }    for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {        const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];        if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) {            bool applyColor;            if (ARMING_FLAG(ARMED)) {                applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH;            } else {                applyColor = !(ledRingIndex % 2); // alternating pattern            }            if (applyColor) {                const hsvColor_t *ringColor = &ledStripConfig()->colors[ledGetColor(ledConfig)];                setLedHsv(ledIndex, ringColor);            }            ledRingIndex++;        }    }}

static void applyLedAnimationLayer(bool updateNow, uint32_t *timer){    static uint8_t frameCounter = 0;    const int animationFrames = ledGridHeight;    if(updateNow) {        frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;        *timer += LED_STRIP_HZ(20);    }    if (ARMING_FLAG(ARMED))        return;    int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1;    int currentRow = frameCounter;    int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;    for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {        const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex];        if (ledGetY(ledConfig) == previousRow) {            setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION));            scaleLedValue(ledIndex, 50);        } else if (ledGetY(ledConfig) == currentRow) {            setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION));        } else if (ledGetY(ledConfig) == nextRow) {            scaleLedValue(ledIndex, 50);        }    }}

static void taskCameraControl(uint32_t currentTime){    if (ARMING_FLAG(ARMED)) {        return;    }    cameraControlProcess(currentTime);}

void mwArm(void){    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {			//led0_op(true);            return;        }        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            headFreeModeHold = heading;           // led2_op(1);//drona led           // led1_op(1);//drona led           // led0_op(0);//drona led#ifdef BLACKBOX            if (feature(FEATURE_BLACKBOX)) {                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);                if (sharedBlackboxAndMspPort) {                    mspReleasePortIfAllocated(sharedBlackboxAndMspPort);                }                startBlackbox();            }#endif            disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero            //beep to indicate arming#ifdef GPS            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)                beeper(BEEPER_ARMING_GPS_FIX);            else                beeper(BEEPER_ARMING);#else            beeper(BEEPER_ARMING);#endif            return;        }    }    if (!ARMING_FLAG(ARMED)) {        beeperConfirmationBeeps(1);    }}

void mwArm(void){    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            headFreeModeHold = heading;            return;        }    }    if (!ARMING_FLAG(ARMED)) {        blinkLedAndSoundBeeper(2, 255, 1);    }}

static void applyLedWarningLayer(bool updateNow, uint32_t *timer){    static uint8_t warningFlashCounter = 0;    static uint8_t warningFlags = 0;          // non-zero during blinks    if (updateNow) {        // keep counter running, so it stays in sync with blink        warningFlashCounter++;        warningFlashCounter &= 0xF;        if (warningFlashCounter == 0) {      // update when old flags was processed            warningFlags = 0;            if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK)                warningFlags |= 1 << WARNING_LOW_BATTERY;            if (feature(FEATURE_FAILSAFE) && failsafeIsActive())                warningFlags |= 1 << WARNING_FAILSAFE;            if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))                warningFlags |= 1 << WARNING_ARMING_DISABLED;        }        *timer += LED_STRIP_HZ(10);    }    if (warningFlags) {        const hsvColor_t *warningColor = NULL;        bool colorOn = (warningFlashCounter % 2) == 0;   // w_w_        warningFlags_e warningId = warningFlashCounter / 4;        if (warningFlags & (1 << warningId)) {            switch (warningId) {                case WARNING_ARMING_DISABLED:                    warningColor = colorOn ? &HSV(GREEN)  : &HSV(BLACK);                    break;                case WARNING_LOW_BATTERY:                    warningColor = colorOn ? &HSV(RED)    : &HSV(BLACK);                    break;                case WARNING_FAILSAFE:                    warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE);                    break;                default:;            }        }        if (warningColor)            applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor);    }}

void mwArm(void){    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {            return;        }        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            ENABLE_ARMING_FLAG(WAS_EVER_ARMED);            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);            resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));#ifdef BLACKBOX            if (feature(FEATURE_BLACKBOX)) {                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);                if (sharedBlackboxAndMspPort) {                    mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);                }                startBlackbox();            }#endif            disarmAt = millis() + masterConfig.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);    }}

// Everything that listens to VTX devicesvoid taskVtxControl(uint32_t currentTime){    if (ARMING_FLAG(ARMED))        return;#ifdef VTX_COMMON    vtxCommonProcess(currentTime);#endif}

static void taskBatteryAlerts(timeUs_t currentTimeUs){    if (!ARMING_FLAG(ARMED)) {        // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup.        batteryUpdatePresence();    }    batteryUpdateStates(currentTimeUs);    batteryUpdateAlarms();}

void mwArm(void){    static bool firstArmingCalibrationWasCompleted;    if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {        gyroSetCalibrationCycles();        armingCalibrationWasInitialised = true;        firstArmingCalibrationWasCompleted = true;    }    if (!isGyroCalibrationComplete()) return;  // prevent arming before gyro is calibrated    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {            return;        }        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            ENABLE_ARMING_FLAG(WAS_EVER_ARMED);            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);            disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero            //beep to indicate arming#ifdef GPS            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)                beeper(BEEPER_ARMING_GPS_FIX);            else                beeper(BEEPER_ARMING);#else            beeper(BEEPER_ARMING);#endif            return;        }    }    if (!ARMING_FLAG(ARMED)) {        beeperConfirmationBeeps(1);    }}

static void sendThrottleOrBatterySizeAsRpm(void){    sendDataHead(ID_RPM);    if (ARMING_FLAG(ARMED)) {        serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);    } else {        serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));    }}

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    }}

void mwArm(void){    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {            return;        }        if (rcModeIsActive(BOXFAILSAFE)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);#ifdef BLACKBOX            if (feature(FEATURE_BLACKBOX)) {                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP_SERVER);                if (sharedBlackboxAndMspPort) {                    mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);                }                startBlackbox();            }#endif            disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero            //beep to indicate arming#ifdef GPS            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)                beeper(BEEPER_ARMING_GPS_FIX);            else                beeper(BEEPER_ARMING);#else            beeper(BEEPER_ARMING);#endif            return;        }    }    if (!ARMING_FLAG(ARMED)) {        beeperConfirmationBeeps(1);    }}

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);        }    }}

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);        }    }}

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 mwArm(void){    if (ARMING_FLAG(OK_TO_ARM)) {        if (ARMING_FLAG(ARMED)) {            return;        }        if (!ARMING_FLAG(PREVENT_ARMING)) {            ENABLE_ARMING_FLAG(ARMED);            headFreeModeHold = heading;#ifdef TELEMETRY            if (feature(FEATURE_TELEMETRY)) {                serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);                while (sharedPort) {                    mspReleasePortIfAllocated(sharedPort);                    sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);                }            }#endif#ifdef BLACKBOX            if (feature(FEATURE_BLACKBOX)) {                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);                if (sharedBlackboxAndMspPort) {                    mspReleasePortIfAllocated(sharedBlackboxAndMspPort);                }                startBlackbox();            }#endif            disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero            return;        }    }    if (!ARMING_FLAG(ARMED)) {        blinkLedAndSoundBeeper(2, 255, 1);    }}

void taskHandleSerial(timeUs_t currentTimeUs){    UNUSED(currentTimeUs);#ifdef USE_CLI    // in cli mode, all serial stuff goes to here. enter cli mode by sending #    if (cliMode) {        cliProcess();        return;    }#endif    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);}

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);}

static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs){    uint32_t startTime = 0;    if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}    // PID - note this is function pointer set by setPIDController()    pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);    DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);#ifdef USE_RUNAWAY_TAKEOFF    // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,    // and gyro rate for any axis is above the limit for at least the activate delay period.    // If so, disarm for safety    if (ARMING_FLAG(ARMED)        && !STATE(FIXED_WING)        && pidConfig()->runaway_takeoff_prevention        && !runawayTakeoffCheckDisabled        && !flipOverAfterCrashMode        && !runawayTakeoffTemporarilyDisabled        && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {        if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)            || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)            || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))            && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)                || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)                || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {            if (runawayTakeoffTriggerUs == 0) {                runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;            } else if (currentTimeUs > runawayTakeoffTriggerUs) {                setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);                disarm();            }        } else {            runawayTakeoffTriggerUs = 0;        }        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);    } else {        runawayTakeoffTriggerUs = 0;        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);    }#endif#ifdef USE_PID_AUDIO    if (isModeActivationConditionPresent(BOXPIDAUDIO)) {        pidAudioUpdate();    }#endif}

bool telemetryDetermineEnabledState(portSharing_e portSharing){    bool enabled = portSharing == PORTSHARING_NOT_SHARED;    if (portSharing == PORTSHARING_SHARED) {        if (telemetryConfig->telemetry_switch)            enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);        else            enabled = ARMING_FLAG(ARMED);    }    return enabled;}

bool telemetryDetermineEnabledState(portSharing_e portSharing){    bool enabled = portSharing == PORTSHARING_NOT_SHARED;    if (portSharing == PORTSHARING_SHARED) {        if (isModeActivationConditionPresent(BOXTELEMETRY))            enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);        else            enabled = ARMING_FLAG(ARMED);    }    return enabled;}

void setupMulticopterAltitudeController(void){    // Use midrc as neutral point for throttle if explicitly selected or if not possible to ascend or descend due to deadband    if (posControl.navConfig->flags.use_midrc_for_althold ||        (((int16_t)rcCommand[THROTTLE] - (int16_t)posControl.rcControlsConfig->alt_hold_deadband) <= ((int16_t)posControl.rxConfig->mincheck)) ||        (((int16_t)rcCommand[THROTTLE] + (int16_t)posControl.rcControlsConfig->alt_hold_deadband) >= ((int16_t)posControl.rxConfig->maxcheck)) ||        !ARMING_FLAG(ARMED)) {        altholdInitialRCThrottle = posControl.rxConfig->midrc;    }    else {        altholdInitialRCThrottle = rcCommand[THROTTLE];    }}

void batteryUpdateCurrentMeter(timeUs_t currentTimeUs){    UNUSED(currentTimeUs);    if (batteryCellCount == 0) {        currentMeterReset(&currentMeter);        return;    }    static uint32_t ibatLastServiced = 0;    const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);    ibatLastServiced = currentTimeUs;    switch (batteryConfig()->currentMeterSource) {        case CURRENT_METER_ADC:            currentMeterADCRefresh(lastUpdateAt);            currentMeterADCRead(&currentMeter);            break;        case CURRENT_METER_VIRTUAL: {#ifdef USE_VIRTUAL_CURRENT_METER            throttleStatus_e throttleStatus = calculateThrottleStatus();            bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));            int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;            currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);            currentMeterVirtualRead(&currentMeter);#endif            break;        }        case CURRENT_METER_ESC:#ifdef USE_ESC_SENSOR            if (feature(FEATURE_ESC_SENSOR)) {                currentMeterESCRefresh(lastUpdateAt);                currentMeterESCReadCombined(&currentMeter);            }#endif            break;        case CURRENT_METER_MSP:#ifdef USE_MSP_CURRENT_METER            currentMeterMSPRefresh(currentTimeUs);            currentMeterMSPRead(&currentMeter);#endif            break;        default:        case CURRENT_METER_NONE:            currentMeterReset(&currentMeter);            break;    }}