float convertExternalToMotor(uint16_t externalValue) { uint16_t motorValue; switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT case true: externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX); if (feature(FEATURE_3D)) { if (externalValue == PWM_RANGE_MID) { motorValue = DSHOT_DISARM_COMMAND; } else if (externalValue < PWM_RANGE_MID) { motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE); } else { motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); } } else { motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); } break; case false: #endif default: motorValue = externalValue; break; } return (float)motorValue; }
uint16_t convertMotorToExternal(float motorValue) { uint16_t externalValue; switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT case true: if (feature(FEATURE_3D)) { if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) { externalValue = PWM_RANGE_MID; } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) { externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN); } else { externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX); } } else { externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX); } break; case false: #endif default: externalValue = motorValue; break; } return externalValue; }
static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (int i = 0; i < motorCount; i++) { float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); #ifdef USE_SERVOS if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); } #endif if (failsafeIsActive()) { if (isMotorProtocolDshot()) { motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motorOutput = disarmMotorOutput; } } motor[i] = motorOutput; } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (int i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } }
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 } } }
uint16_t convertMotorToExternal(uint16_t motorValue) { uint16_t externalValue = motorValue; #ifdef USE_DSHOT if (isMotorProtocolDshot()) { externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE); } #endif return externalValue; }
uint16_t convertExternalToMotor(uint16_t externalValue) { uint16_t motorValue = externalValue; #ifdef USE_DSHOT if (isMotorProtocolDshot()) { motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); } #endif return motorValue; }
// Add here scaled ESC outputs for digital protol void initEscEndpoints(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; maxMotorOutputNormal = DSHOT_MAX_THROTTLE; deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes } else #endif { disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; minMotorOutputNormal = motorConfig->minthrottle; maxMotorOutputNormal = motorConfig->maxthrottle; deadbandMotor3dHigh = flight3DConfig->deadband3d_high; deadbandMotor3dLow = flight3DConfig->deadband3d_low; } rcCommandThrottleRange = (2000 - rxConfig->mincheck); }
void mixTable(pidProfile_t *pidProfile) { uint32_t i = 0; float vbatCompensationFactor = 1; // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0; float throttleRange = 0, throttle = 0; float motorOutputRange, motorMixRange; uint16_t motorOutputMin, motorOutputMax; static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling motorOutputMax = deadbandMotor3dLow; motorOutputMin = minMotorOutputNormal; throttlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling motorOutputMax = maxMotorOutputNormal; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = deadbandMotor3dLow; motorOutputMin = motorOutputMax = minMotorOutputNormal; } else { // Deadband handling from positive to negative motorOutputMax = maxMotorOutputNormal; throttle = motorOutputMin = deadbandMotor3dHigh; } } else { throttle = rcCommand[THROTTLE]; motorOutputMin = minMotorOutputNormal; motorOutputMax = maxMotorOutputNormal; } motorOutputRange = motorOutputMax - motorOutputMin; throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f); throttleRange = motorOutputMax - motorOutputMin; float scaledAxisPIDf[3]; // Limit the PIDsum for (int axis = 0; axis < 3; axis++) scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); // Calculate voltage compensation if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw; if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i]; if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i]; } motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (i = 0; i < motorCount; i++) { motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center throttle = 0.5f; } else { float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) ); if (failsafeIsActive()) { if (isMotorProtocolDshot()) motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); } else { motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = disarmMotorOutput; } } } // Anti Desync feature for ESC's. Limit rapid throttle changes if (motorConfig->maxEscThrottleJumpMs) { const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000); // Only makes sense when it's within the range if (maxThrottleStep < throttleRange) { static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation motorPrevious[i] = motor[i]; } } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } }
static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode float currentThrottleInputRange = 0; if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) { rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. } if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { // INVERTED motorRangeMin = motorOutputLow; motorRangeMax = deadbandMotor3dLow; if (isMotorProtocolDshot()) { motorOutputMin = motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow; } else { motorOutputMin = deadbandMotor3dLow; motorOutputRange = motorOutputLow - deadbandMotor3dLow; } if (motorOutputMixSign != -1) { reversalTimeUs = currentTimeUs; } motorOutputMixSign = -1; rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; currentThrottleInputRange = rcCommandThrottleRange3dLow; } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { // NORMAL motorRangeMin = deadbandMotor3dHigh; motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; if (motorOutputMixSign != 1) { reversalTimeUs = currentTimeUs; } motorOutputMixSign = 1; rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow && !flight3DConfigMutable()->switched_mode3d) || isMotorsReversed()) { // INVERTED_TO_DEADBAND motorRangeMin = motorOutputLow; motorRangeMax = deadbandMotor3dLow; if (isMotorProtocolDshot()) { motorOutputMin = motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow; } else { motorOutputMin = deadbandMotor3dLow; motorOutputRange = motorOutputLow - deadbandMotor3dLow; } if (motorOutputMixSign != -1) { reversalTimeUs = currentTimeUs; } motorOutputMixSign = -1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dLow; } else { // NORMAL_TO_DEADBAND motorRangeMin = deadbandMotor3dHigh; motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; if (motorOutputMixSign != 1) { reversalTimeUs = currentTimeUs; } motorOutputMixSign = 1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dHigh; } if (currentTimeUs - reversalTimeUs < 250000) { // keep ITerm zero for 250ms after motor reversal pidResetITerm(); } } else { throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection; currentThrottleInputRange = rcCommandThrottleRange; motorRangeMin = motorOutputLow; motorRangeMax = motorOutputHigh; motorOutputMin = motorOutputLow; motorOutputRange = motorOutputHigh - motorOutputLow; motorOutputMixSign = 1; } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); }
/* * processRx called from taskUpdateRxMain */ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // 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)) disarm(); } updateRSSI(currentTimeUs); if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent Iterm from being reset } } else { airmodeIsActivated = false; } /* 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 */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetITerm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } #ifdef USE_RUNAWAY_TAKEOFF // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low. // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable // prevention for the remainder of the battery. if (ARMING_FLAG(ARMED) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && !STATE(FIXED_WING)) { // Determine if we're in "flight" // - motors running // - throttle over runaway_takeoff_deactivate_throttle_percent // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } } } // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds if (inStableFlight) { if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; // at high throttle levels reduce deactivation delay by 50% if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { deactivateDelay = deactivateDelay / 2; } if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } } else { if (runawayTakeoffDeactivateUs != 0) { runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs); } runawayTakeoffDeactivateUs = 0; } if (runawayTakeoffDeactivateUs == 0) { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000); } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000); } } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif // 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) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over disarm(); 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(); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); #ifdef USE_DSHOT /* Enable beep warning when the crash flip mode is active */ if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { beeper(BEEPER_CRASH_FLIP_MODE); } #endif if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (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); } #ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } #endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500)); } else { LED1_OFF; rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100)); } if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } #if defined(USE_ACC) || defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(USE_GPS) || defined(USE_MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } #endif 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)) { if (imuQuaternionHeadfreeOffsetSet()){ beeper(BEEPER_RX_SET); } } } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; if (feature(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); telemetryCheckState(); sharedPortTelemetryEnabled = true; } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); sharedPortTelemetryEnabled = false; } } #endif #ifdef USE_VTX_CONTROL vtxUpdateActivatedChannel(); if (canUpdateVTX()) { handleVTXControlButton(); } #endif #ifdef USE_ACRO_TRAINER pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER #ifdef USE_RC_SMOOTHING_FILTER if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); } #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); return true; }
void tryArm(void) { if (armingConfig()->gyro_cal_on_first_arm) { gyroStartCalibration(true); } updateArmingStatus(); if (!isArmingDisabled()) { if (ARMING_FLAG(ARMED)) { return; } #ifdef USE_DSHOT if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { tryingToArm = ARMING_DELAYED_CRASHFLIP; } else { tryingToArm = ARMING_DELAYED_NORMAL; } } return; } if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashMode = false; if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { flipOverAfterCrashMode = true; #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffCheckDisabled = false; #endif if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } } #endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); resetTryingToArm(); #ifdef USE_ACRO_TRAINER pidAcroTrainerInit(); #endif // USE_ACRO_TRAINER if (isModeActivationConditionPresent(BOXPREARM)) { ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } imuQuaternionHeadfreeOffsetSet(); disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero lastArmingDisabledReason = 0; //beep to indicate arming #ifdef USE_GPS if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING); } #else beeper(BEEPER_ARMING); #endif #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffDeactivateUs = 0; runawayTakeoffAccumulatedUs = 0; runawayTakeoffTriggerUs = 0; #endif } else { resetTryingToArm(); if (!isFirstArmingGyroCalibrationRunning()) { int armingDisabledReason = ffs(getArmingDisableFlags()); if (lastArmingDisabledReason != armingDisabledReason) { lastArmingDisabledReason = armingDisabledReason; beeperWarningBeeps(armingDisabledReason); } } } }