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(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); }
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 applyLedWarningLayer(uint8_t updateNow) { const ledConfig_t *ledConfig; uint8_t ledIndex; static uint8_t warningFlashCounter = 0; if (updateNow && warningFlashCounter == 0) { warningFlags = WARNING_FLAG_NONE; if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { warningFlags |= WARNING_FLAG_LOW_BATTERY; } if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { warningFlags |= WARNING_FLAG_FAILSAFE; } if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { warningFlags |= WARNING_FLAG_ARMING_DISABLED; } } if (warningFlags || warningFlashCounter > 0) { const hsvColor_t *warningColor = &hsv_black; if ((warningFlashCounter & 1) == 0) { if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { warningColor = &hsv_green; } if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { warningColor = &hsv_red; } if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { warningColor = &hsv_yellow; } } else { if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { warningColor = &hsv_blue; } } for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { continue; } setLedHsv(ledIndex, warningColor); } } if (updateNow && (warningFlags || warningFlashCounter)) { warningFlashCounter++; if (warningFlashCounter == 20) { warningFlashCounter = 0; } } }
static void applyLedWarningLayer(bool updateNow, timeUs_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 (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK) warningFlags |= 1 << WARNING_LOW_BATTERY; if (failsafeIsActive()) warningFlags |= 1 << WARNING_FAILSAFE; if (!ARMING_FLAG(ARMED) && isArmingDisabled()) warningFlags |= 1 << WARNING_ARMING_DISABLED; } *timer += HZ_TO_US(10); } const hsvColor_t *warningColor = NULL; if (warningFlags) { 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:; } } } else { if (isBeeperOn()) { warningColor = &HSV(ORANGE); } } if (warningColor) { applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } }
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); } }
static void updateRcCommands(void) { // PITCH & ROLL only dynamic PID adjustment, depending on throttle value int32_t prop; if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop = 100; } else { if (rcData[THROTTLE] < 2000) { prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop = 100 - currentControlRateProfile->dynThrPID; } } for (int axis = 0; axis < 3; axis++) { // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. PIDweight[axis] = prop; int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (tmp > masterConfig.rcControlsConfig.deadband) { tmp -= masterConfig.rcControlsConfig.deadband; } else { tmp = 0; } rcCommand[axis] = tmp; } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; } } int32_t tmp; if (feature(FEATURE_3D)) { tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - PWM_RANGE_MIN); } else { tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); } rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc); } 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; } }
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]; } } }
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); // 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.disarm_kill_switch, masterConfig.fixed_wing_auto_arm); updateActivatedModes(currentProfile->modeActivationConditions, currentProfile->modeActivationOperator); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive()) || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } /* Heading lock mode */ if (IS_RC_MODE_ACTIVE(BOXHEADINGLOCK)) { if (!FLIGHT_MODE(HEADING_LOCK)) { ENABLE_FLIGHT_MODE(HEADING_LOCK); } } else { DISABLE_FLIGHT_MODE(HEADING_LOCK); } /* Flaperon mode */ if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) { if (!FLIGHT_MODE(FLAPERON)) { ENABLE_FLIGHT_MODE(FLAPERON); } } else { DISABLE_FLIGHT_MODE(FLAPERON); } /* Turn assistant mode */ if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) { if (!FLIGHT_MODE(TURN_ASSISTANT)) { ENABLE_FLIGHT_MODE(TURN_ASSISTANT); } } else { DISABLE_FLIGHT_MODE(TURN_ASSISTANT); } #if defined(MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); ENABLE_FLIGHT_MODE(MAG_MODE); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif // Navigation may override PASSTHRU_MODE if (IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode()) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) { /* In PASSTHRU mode we reset integrators prevent I-term wind-up (PID output is not used in PASSTHRU) */ pidResetErrorAccumulators(); } else { if (throttleStatus == THROTTLE_LOW) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) { ENABLE_STATE(ANTI_WINDUP); } else { DISABLE_STATE(ANTI_WINDUP); } } else { DISABLE_STATE(ANTI_WINDUP); pidResetErrorAccumulators(); } } else { DISABLE_STATE(ANTI_WINDUP); } } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); } } #endif }
/* * 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 mixTable(void) { uint32_t i; if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } // motors for non-servo mixes for (i = 0; i < motorCount; i++) { motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; } if (ARMING_FLAG(ARMED)) { bool isFailsafeActive = failsafeIsActive(); // Find the maximum motor output. int16_t maxMotor = motor[0]; for (i = 1; i < motorCount; i++) { // If one motor is above the maxthrottle threshold, we reduce the value // of all motors by the amount of overshoot. That way, only one motor // is at max and the relative power of each motor is preserved. if (motor[i] > maxMotor) { maxMotor = motor[i]; } } int16_t maxThrottleDifference = 0; if (maxMotor > escAndServoConfig->maxthrottle) { maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle; } for (i = 0; i < motorCount; i++) { // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxThrottleDifference; if (feature(FEATURE_3D)) { if (mixerConfig->pid_at_min_throttle || rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle || rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) { if (rcData[THROTTLE] > rxConfig->midrc) { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } else { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); } } else { if (rcData[THROTTLE] > rxConfig->midrc) { motor[i] = flight3DConfig->deadband3d_high; } else { motor[i] = flight3DConfig->deadband3d_low; } } } else { if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else { // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); if ((rcData[THROTTLE]) < rxConfig->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; } else if (mixerConfig->pid_at_min_throttle == 0) { motor[i] = escAndServoConfig->minthrottle; } } } } } } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS) // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } #endif }
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 } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_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); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(&masterConfig.serialConfig); } } #endif }
void mixTable(void *pidProfilePtr) { uint32_t i = 0; fix12_t vbatCompensationFactor = 0; static fix12_t mixReduction; bool use_vbat_compensation = false; pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; if (batteryConfig && pidProfile->vbatPidCompensation) { use_vbat_compensation = true; vbatCompensationFactor = calculateVbatPidCompensation(); } bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS]; int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i]; } // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange, throttle; int16_t throttleMin, throttleMax; static int16_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 throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; } else { // Deadband handling from positive to negative throttleMax = escAndServoConfig->maxthrottle; throttle = throttleMin = flight3DConfig->deadband3d_high; } } else { throttle = rcCommand[THROTTLE]; throttleMin = escAndServoConfig->minthrottle; throttleMax = escAndServoConfig->maxthrottle; } throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } // 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] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } } else { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = escAndServoConfig->mincommand; } } } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } #endif }
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos uint8_t stTmp = 0; int i; // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; if (rcData[i] > rxConfig->mincheck) stTmp |= 0x80; // check for MIN if (rcData[i] < rxConfig->maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { if (rcDelayCommand < 250) rcDelayCommand++; } else rcDelayCommand = 0; rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { if (rcModeIsActive(BOXARM)) { // Arming via ARM BOX if (throttleStatus == THROTTLE_LOW) { if (ARMING_FLAG(OK_TO_ARM)) { mwArm(); } } } else { // Disarming via ARM BOX if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { if (disarm_kill_switch) { mwDisarm(); } else if (throttleStatus == THROTTLE_LOW) { mwDisarm(); } } } } if (rcDelayCommand != 20) { return; } if (isUsingSticksToArm) { // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (ARMING_FLAG(ARMED)) mwDisarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat } } // Disarm on roll (only when retarded_arm is enabled) if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) { if (ARMING_FLAG(ARMED)) mwDisarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat } } } if (ARMING_FLAG(ARMED)) { // actions during armed return; } // actions during not armed i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef GPS if (feature(FEATURE_GPS)) { GPS_reset_home_position(); } #endif #ifdef BARO if (sensors(SENSOR_BARO)) baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) #endif return; } if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { // Inflight ACC Calibration handleInflightCalibrationStickPosition(); return; } // Multiple configuration profiles if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { changeProfile(i - 1); beeperConfirmationBeeps(i); return; } if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } if (isUsingSticksToArm) { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW mwArm(); return; } if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) { // Arm via ROLL mwArm(); return; } } if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { // Calibrating Mag ENABLE_STATE(CALIBRATE_MAG); return; } // Accelerometer Trim rollAndPitchTrims_t accelerometerTrimsDelta; memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); bool shouldApplyRollAndPitchTrimDelta = false; if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { accelerometerTrimsDelta.values.pitch = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { accelerometerTrimsDelta.values.pitch = -2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { accelerometerTrimsDelta.values.roll = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { accelerometerTrimsDelta.values.roll = -2; shouldApplyRollAndPitchTrimDelta = true; } if (shouldApplyRollAndPitchTrimDelta) { applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); rcDelayCommand = 0; // allow autorepetition return; } #ifdef DISPLAY if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { displayDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { displayEnablePageCycling(); } #endif }
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm) { static timeMs_t lastTickTimeMs = 0; static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint32_t rcSticks; // this hold sticks position for command combos static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it const timeMs_t currentTimeMs = millis(); updateRcStickPositions(); uint32_t stTmp = getRcStickPositions(); if (stTmp == rcSticks) { if (rcDelayCommand < 250) { if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) { lastTickTimeMs = currentTimeMs; rcDelayCommand++; } } } else rcDelayCommand = 0; rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX if (throttleStatus == THROTTLE_LOW) { if (ARMING_FLAG(OK_TO_ARM)) { mwArm(); } } } else { // Disarming via ARM BOX // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver // and can't afford to risk disarming in the air if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { rcDisarmTicks++; if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX) if (disarm_kill_switch) { mwDisarm(DISARM_SWITCH); } else if (throttleStatus == THROTTLE_LOW) { mwDisarm(DISARM_SWITCH); } } } else { rcDisarmTicks = 0; } } } // KILLSWITCH disarms instantly if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { mwDisarm(DISARM_KILLSWITCH); } if (rcDelayCommand != 20) { return; } if (isUsingSticksToArm) { // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { // Dont disarm if fixedwing and motorstop if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { return; } else if (ARMING_FLAG(ARMED)) { mwDisarm(DISARM_STICKS); } else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat } } } if (ARMING_FLAG(ARMED)) { // actions during armed return; } // actions during not armed int i = 0; // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); return; } #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) // Save waypoint list if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { const bool success = saveNonVolatileWaypointList(); beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); } // Load waypoint list if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { const bool success = loadNonVolatileWaypointList(); beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); } #endif // Multiple configuration profiles if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { setConfigProfileAndWriteEEPROM(i - 1); return; } // Save config if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } // Arming by sticks if (isUsingSticksToArm) { if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (throttleStatus != THROTTLE_LOW) { mwArm(); return; } } else { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW mwArm(); return; } } } // Calibrating Acc if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } // Calibrating Mag if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { ENABLE_STATE(CALIBRATE_MAG); return; } // Accelerometer Trim if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { applyAndSaveBoardAlignmentDelta(0, -2); rcDelayCommand = 10; return; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { applyAndSaveBoardAlignmentDelta(0, 2); rcDelayCommand = 10; return; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { applyAndSaveBoardAlignmentDelta(-2, 0); rcDelayCommand = 10; return; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { applyAndSaveBoardAlignmentDelta(2, 0); rcDelayCommand = 10; return; } }
void mixTable(void) { uint32_t i; if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rpyMix[MAX_SUPPORTED_MOTORS]; int16_t rpyMixMax = 0; // assumption: symetrical about zero. int16_t rpyMixMin = 0; // motors for non-servo mixes for (i = 0; i < motorCount; i++) { rpyMix[i] = axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; } int16_t rpyMixRange = rpyMixMax - rpyMixMin; int16_t throttleRange, throttleCommand; int16_t throttleMin, throttleMax; static int16_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 throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttleCommand = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; } else { // Deadband handling from positive to negative throttleMax = escAndServoConfig->maxthrottle; throttleCommand = throttleMin = flight3DConfig->deadband3d_high; } } else { throttleCommand = rcCommand[THROTTLE]; throttleMin = escAndServoConfig->minthrottle; throttleMax = escAndServoConfig->maxthrottle; } throttleRange = throttleMax - throttleMin; #define THROTTLE_CLIPPING_FACTOR 0.33f if (rpyMixRange > throttleRange) { motorLimitReached = true; float mixReduction = (float)throttleRange / rpyMixRange; for (i = 0; i < motorCount; i++) { rpyMix[i] = mixReduction * rpyMix[i]; } // Allow some clipping on edges to soften correction response throttleMin = throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2); throttleMax = throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2); } else { motorLimitReached = false; throttleMin = MIN(throttleMin + (rpyMixRange / 2), throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2)); throttleMax = MAX(throttleMax - (rpyMixRange / 2), throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2)); } // 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. if (ARMING_FLAG(ARMED)) { bool isFailsafeActive = failsafeIsActive(); for (i = 0; i < motorCount; i++) { motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } } else { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = escAndServoConfig->mincommand; } } } } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } }
void mavlinkSendHUDAndHeartbeat(void) { uint16_t msgLength; float mavAltitude = 0; float mavGroundSpeed = 0; float mavAirSpeed = 0; float mavClimbRate = 0; #if defined(GPS) // use ground speed if source available if (sensors(SENSOR_GPS)) { mavGroundSpeed = GPS_speed / 100.0f; } #endif // select best source for altitude #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) { // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0; } #if defined(GPS) else if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS mavAltitude = GPS_altitude; } #endif #elif defined(GPS) if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS mavAltitude = GPS_altitude; } #endif mavlink_msg_vfr_hud_pack(0, 200, &mavMsg, // airspeed Current airspeed in m/s mavAirSpeed, // groundspeed Current ground speed in m/s mavGroundSpeed, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second mavClimbRate); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; if (ARMING_FLAG(ARMED)) mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t mavSystemType; switch(mixerConfig()->mixerMode) { case MIXER_TRI: mavSystemType = MAV_TYPE_TRICOPTER; break; case MIXER_QUADP: case MIXER_QUADX: case MIXER_Y4: case MIXER_VTAIL4: mavSystemType = MAV_TYPE_QUADROTOR; break; case MIXER_Y6: case MIXER_HEX6: case MIXER_HEX6X: mavSystemType = MAV_TYPE_HEXAROTOR; break; case MIXER_OCTOX8: case MIXER_OCTOFLATP: case MIXER_OCTOFLATX: mavSystemType = MAV_TYPE_OCTOROTOR; break; case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_CUSTOM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; case MIXER_HELI_120_CCPM: case MIXER_HELI_90_DEG: mavSystemType = MAV_TYPE_HELICOPTER; break; default: mavSystemType = MAV_TYPE_GENERIC; break; } // Custom mode for compatibility with APM OSDs uint8_t mavCustomMode = 1; // Acro by default if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { mavCustomMode = 0; //Stabilize mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; } if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) mavCustomMode = 2; //Alt Hold if (FLIGHT_MODE(GPS_HOME_MODE)) mavCustomMode = 6; //Return to Launch if (FLIGHT_MODE(GPS_HOLD_MODE)) mavCustomMode = 16; //Position Hold (Earlier called Hybrid) uint8_t mavSystemState = 0; if (ARMING_FLAG(ARMED)) { if (failsafeIsActive()) { mavSystemState = MAV_STATE_CRITICAL; } else { mavSystemState = MAV_STATE_ACTIVE; } } else { mavSystemState = MAV_STATE_STANDBY; } mavlink_msg_heartbeat_pack(0, 200, &mavMsg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) mavSystemType, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM MAV_AUTOPILOT_GENERIC, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h mavModes, // custom_mode A bitfield for use for autopilot-specific flags. mavCustomMode, // system_status System status flag, see MAV_STATE ENUM mavSystemState); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
void mixTable(void) { uint32_t i; bool isFailsafeActive = failsafeIsActive(); if (motorCount >= 4 && mixerConfig()->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction axisPID[FD_YAW] = constrain(axisPID[FD_YAW], -mixerConfig()->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig()->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } if (rcModeIsActive(BOXAIRMODE)) { // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS]; int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = axisPID[FD_PITCH] * currentMixer[i].pitch + axisPID[FD_ROLL] * currentMixer[i].roll + -mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw; if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; } // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange, throttle; int16_t throttleMin, throttleMax; static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions in 3D. // Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check 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 ((rcData[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling throttleMax = motor3DConfig()->deadband3d_low; throttleMin = motorAndServoConfig()->minthrottle; throttlePrevious = throttle = rcData[THROTTLE]; } else if (rcData[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling throttleMax = motorAndServoConfig()->maxthrottle; throttleMin = motor3DConfig()->deadband3d_high; throttlePrevious = throttle = rcData[THROTTLE]; } else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = motor3DConfig()->deadband3d_low; throttleMin = motorAndServoConfig()->minthrottle; } else { // Deadband handling from positive to negative throttleMax = motorAndServoConfig()->maxthrottle; throttle = throttleMin = motor3DConfig()->deadband3d_high; } } else { throttle = rcCommand[THROTTLE]; throttleMin = motorAndServoConfig()->minthrottle; throttleMax = motorAndServoConfig()->maxthrottle; } throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { motorLimitReached = true; float mixReduction = (float) throttleRange / rollPitchYawMixRange; for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = lrintf((float) rollPitchYawMix[i] * mixReduction); } // Get the maximum correction by setting throttle offset to center. throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { motorLimitReached = false; throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } // 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] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { motor[i] = mixConstrainMotorForFailsafeCondition(i); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) { motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motor3DConfig()->deadband3d_low); } else { motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorAndServoConfig()->maxthrottle); } } else { motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle); } } } else { // motors for non-servo mixes for (i = 0; i < motorCount; i++) { motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[FD_PITCH] * currentMixer[i].pitch + axisPID[FD_ROLL] * currentMixer[i].roll + -mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw; } // Find the maximum motor output. int16_t maxMotor = motor[0]; for (i = 1; i < motorCount; i++) { // If one motor is above the maxthrottle threshold, we reduce the value // of all motors by the amount of overshoot. That way, only one motor // is at max and the relative power of each motor is preserved. if (motor[i] > maxMotor) { maxMotor = motor[i]; } } int16_t maxThrottleDifference = 0; if (maxMotor > motorAndServoConfig()->maxthrottle) { maxThrottleDifference = maxMotor - motorAndServoConfig()->maxthrottle; } for (i = 0; i < motorCount; i++) { // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxThrottleDifference; if (feature(FEATURE_3D)) { if (mixerConfig()->pid_at_min_throttle || rcData[THROTTLE] <= rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle || rcData[THROTTLE] >= rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle) { if (rcData[THROTTLE] > rxConfig()->midrc) { motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorAndServoConfig()->maxthrottle); } else { motor[i] = constrain(motor[i], motorAndServoConfig()->mincommand, motor3DConfig()->deadband3d_low); } } else { if (rcData[THROTTLE] > rxConfig()->midrc) { motor[i] = motor3DConfig()->deadband3d_high; } else { motor[i] = motor3DConfig()->deadband3d_low; } } } else { if (isFailsafeActive) { motor[i] = mixConstrainMotorForFailsafeCondition(i); } else { // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. motor[i] = constrain(motor[i], motorAndServoConfig()->minthrottle, motorAndServoConfig()->maxthrottle); if ((rcData[THROTTLE]) < rxConfig()->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = motorAndServoConfig()->mincommand; } else if (mixerConfig()->pid_at_min_throttle == 0) { motor[i] = motorAndServoConfig()->minthrottle; } } } } } } /* Disarmed for all mixers */ if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS) servoMixTable(); #endif }
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 (!rcModeIsActive(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(rxConfig()); /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ if (throttleStatus == THROTTLE_LOW) { if (rcModeIsActive(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { if (rollPitchStatus == CENTERED) { ENABLE_STATE(ANTI_WINDUP); } else { DISABLE_STATE(ANTI_WINDUP); } } else { pidResetITerm(); } } else { DISABLE_STATE(ANTI_WINDUP); } // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); 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(rxConfig(), throttleStatus, armingConfig()->retarded_arm, armingConfig()->disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } rcModeUpdateActivated(modeActivationProfile()->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); processRcAdjustments(currentControlRateProfile, rxConfig()); } bool canUseHorizonMode = true; if ((rcModeIsActive(BOXANGLE) || (feature(FEATURE_FAILSAFE) && 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 (rcModeIsActive(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (rcModeIsActive(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (rcModeIsActive(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (rcModeIsActive(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (rcModeIsActive(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 TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) || (telemetryConfig()->telemetry_switch && rcModeIsActive(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. bool telemetryStateChanged = telemetryCheckState(); if (telemetryStateChanged) { mspSerialAllocatePorts(); } } } #endif #ifdef VTX if (canUpdateVTX()) { updateVTXState(); } #endif }
void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; static bool airmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTimeUs); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTimeUs); if (feature(FEATURE_FAILSAFE)) { if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= 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) { pidResetErrorGyroState(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } // 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 mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(throttleStatus); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } 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)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #if defined(ACC) || defined(MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(GPS) || defined(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)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #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 TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) || (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); } } #endif #ifdef VTX vtxUpdateActivatedChannel(); #endif }
void executePeriodicTasks(void) { static int periodicTaskIndex = 0; switch (periodicTaskIndex++) { #ifdef MAG case UPDATE_COMPASS_TASK: if (sensors(SENSOR_MAG)) { updateCompass(&masterConfig.magZero); } break; #endif #ifdef BARO case UPDATE_BARO_TASK: if (sensors(SENSOR_BARO)) { baroUpdate(currentTime); } break; #endif #if defined(BARO) || defined(SONAR) case CALCULATE_ALTITUDE_TASK: #if defined(BARO) && !defined(SONAR) if (sensors(SENSOR_BARO) && isBaroReady()) { #endif #if defined(BARO) && defined(SONAR) if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) { #endif #if !defined(BARO) && defined(SONAR) if (sensors(SENSOR_SONAR)) { #endif calculateEstimatedAltitude(currentTime); } break; #endif #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { sonarUpdate(); } break; #endif #ifdef DISPLAY case UPDATE_DISPLAY_TASK: if (feature(FEATURE_DISPLAY)) { updateDisplay(); } break; #endif } if (periodicTaskIndex >= PERIODIC_TASK_COUNT) { periodicTaskIndex = 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 } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(&masterConfig.serialConfig); } } #endif } void filterRc(void){ static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t factor, rcInterpolationFactor; static filterStatePt1_t filteredCycleTimeState; uint16_t rxRefreshRate, filteredCycleTime; // Set RC refresh rate for sampling and channels to filter initRxRefreshRate(&rxRefreshRate); filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1); rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1; if (isRXDataNew) { for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcData[channel]; } isRXDataNew = false; factor = rcInterpolationFactor - 1; } else { factor--; } // Interpolate steps of rcData if (factor > 0) { for (int channel=0; channel < 4; channel++) { rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; } } else { factor = 0; } } 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 (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; // Gyro Low Pass if (currentProfile->pidProfile.gyro_cut_hz) { int axis; static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT]; for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz); } } if (masterConfig.rxConfig.rcSmoothing) { filterRc(); } annexCode(); #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 && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, ¤tProfile->accelerometerTrims, &masterConfig.rxConfig ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
void processRcStickPositions(throttleStatus_e throttleStatus) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it uint8_t stTmp = 0; int i; // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; if (rcData[i] > rxConfig()->mincheck) stTmp |= 0x80; // check for MIN if (rcData[i] < rxConfig()->maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { if (rcDelayCommand < 250) rcDelayCommand++; } else rcDelayCommand = 0; rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX tryArm(); } else { // Disarming via ARM BOX resetArmingDisabled(); if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { if (armingConfig()->disarm_kill_switch) { disarm(); } else if (throttleStatus == THROTTLE_LOW) { disarm(); } } } } } if (rcDelayCommand != 20) { return; } if (isUsingSticksToArm) { // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (ARMING_FLAG(ARMED)) disarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat } } } if (ARMING_FLAG(ARMED)) { // actions during armed return; } // actions during not armed i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration gyroStartCalibration(false); #ifdef GPS if (feature(FEATURE_GPS)) { GPS_reset_home_position(); } #endif #ifdef BARO if (sensors(SENSOR_BARO)) baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) #endif return; } if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { // Inflight ACC Calibration handleInflightCalibrationStickPosition(); return; } // Multiple configuration profiles if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { changePidProfile(i - 1); return; } if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } if (isUsingSticksToArm) { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW tryArm(); return; } else { resetArmingDisabled(); } } if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { // Calibrating Mag ENABLE_STATE(CALIBRATE_MAG); return; } // Accelerometer Trim rollAndPitchTrims_t accelerometerTrimsDelta; memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); bool shouldApplyRollAndPitchTrimDelta = false; if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { accelerometerTrimsDelta.values.pitch = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { accelerometerTrimsDelta.values.pitch = -2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { accelerometerTrimsDelta.values.roll = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { accelerometerTrimsDelta.values.roll = -2; shouldApplyRollAndPitchTrimDelta = true; } if (shouldApplyRollAndPitchTrimDelta) { applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); rcDelayCommand = 0; // allow autorepetition return; } #ifdef USE_DASHBOARD if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { dashboardDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { dashboardEnablePageCycling(); } #endif #ifdef VTX_CONTROL if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) { vtxIncrementBand(); } if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) { vtxDecrementBand(); } if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) { vtxIncrementChannel(); } if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) { vtxDecrementChannel(); } #endif }
void processRcStickPositions(throttleStatus_e throttleStatus) { // time the sticks are maintained static int16_t rcDelayMs; // hold sticks position for command combos static uint8_t rcSticks; // an extra guard for disarming through switch to prevent that one frame can disarm it static uint8_t rcDisarmTicks; static bool doNotRepeat; #ifdef USE_CMS if (cmsInMenu) { return; } #endif // checking sticks positions uint8_t stTmp = 0; for (int i = 0; i < 4; i++) { stTmp >>= 2; if (rcData[i] > rxConfig()->mincheck) { stTmp |= 0x80; // check for MIN } if (rcData[i] < rxConfig()->maxcheck) { stTmp |= 0x40; // check for MAX } } if (stTmp == rcSticks) { if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) { rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000; } } else { rcDelayMs = 0; doNotRepeat = false; } rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX tryArm(); } else { // Disarming via ARM BOX resetArmingDisabled(); if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { if (armingConfig()->disarm_kill_switch) { disarm(); } else if (throttleStatus == THROTTLE_LOW) { disarm(); } } } } } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { doNotRepeat = true; // Disarm on throttle down + yaw if (ARMING_FLAG(ARMED)) disarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat } } return; } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { doNotRepeat = true; if (!ARMING_FLAG(ARMED)) { // Arm via YAW tryArm(); } else { resetArmingDisabled(); } } return; } if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) { return; } doNotRepeat = true; // actions during not armed if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration gyroStartCalibration(false); #ifdef USE_GPS if (feature(FEATURE_GPS)) { GPS_reset_home_position(); } #endif #ifdef USE_BARO if (sensors(SENSOR_BARO)) baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) #endif return; } if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { // Inflight ACC Calibration handleInflightCalibrationStickPosition(); return; } // Change PID profile int newPidProfile = 0; if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> PID profile 1 newPidProfile = 1; } else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> PID profile 2 newPidProfile = 2; } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> PID profile 3 newPidProfile = 3; } if (newPidProfile) { changePidProfile(newPidProfile - 1); return; } if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { // Calibrating Mag ENABLE_STATE(CALIBRATE_MAG); return; } if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) { // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims rollAndPitchTrims_t accelerometerTrimsDelta; memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); bool shouldApplyRollAndPitchTrimDelta = false; if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { accelerometerTrimsDelta.values.pitch = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { accelerometerTrimsDelta.values.pitch = -2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { accelerometerTrimsDelta.values.roll = 2; shouldApplyRollAndPitchTrimDelta = true; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { accelerometerTrimsDelta.values.roll = -2; shouldApplyRollAndPitchTrimDelta = true; } if (shouldApplyRollAndPitchTrimDelta) { applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); repeatAfter(STICK_AUTOREPEAT_MS); return; } } else { // in ACRO mode, so use sticks to change RATE profile switch (rcSticks) { case THR_HI + YAW_CE + PIT_HI + ROL_CE: changeControlRateProfile(0); return; case THR_HI + YAW_CE + PIT_LO + ROL_CE: changeControlRateProfile(1); return; case THR_HI + YAW_CE + PIT_CE + ROL_HI: changeControlRateProfile(2); return; case THR_HI + YAW_CE + PIT_CE + ROL_LO: changeControlRateProfile(3); return; } } #ifdef USE_DASHBOARD if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { dashboardDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { dashboardEnablePageCycling(); } #endif #ifdef USE_VTX_CONTROL if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) { vtxIncrementBand(); } if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) { vtxDecrementBand(); } if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) { vtxIncrementChannel(); } if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) { vtxDecrementChannel(); } #endif #ifdef USE_CAMERA_CONTROL if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0); repeatAfter(3 * STICK_DELAY_MS); } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) { cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0); repeatAfter(3 * STICK_DELAY_MS); } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0); repeatAfter(3 * STICK_DELAY_MS); } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) { cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0); repeatAfter(3 * STICK_DELAY_MS); } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0); repeatAfter(3 * STICK_DELAY_MS); } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000); } #endif }