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();//nothing much; monitering = true } 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; }*/ //drona led #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 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); 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()) || 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); } #if defined(MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); updateMagHoldHeading(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 // 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(); } // Enable low-throttle PID attenuation on multicopters if (!STATE(FIXED_WING)) { ENABLE_STATE(PID_ATTENUATE); } else { DISABLE_STATE(PID_ATTENUATE); } } else { DISABLE_STATE(PID_ATTENUATE); 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(); mspAllocateSerialPorts(); } } #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 { #ifndef SKIP_PID_MW23 pidResetITermAngle(); #endif 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)) { #ifndef SKIP_PID_MW23 pidResetITermAngle(); #endif ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (rcModeIsActive(BOXMOTORTEST)) { if (!FLIGHT_MODE(MOTORTEST_MODE) && !ARMING_FLAG(ARMED)) ENABLE_FLIGHT_MODE(MOTORTEST_MODE); } else { if (FLIGHT_MODE(MOTORTEST_MODE)) { DISABLE_FLIGHT_MODE(MOTORTEST_MODE); mwDisarm(); } } if (rcModeIsActive(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { #ifndef SKIP_PID_MW23 pidResetITermAngle(); #endif ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else if (FLIGHT_MODE(MOTORTEST_MODE)) { if (millis() & (1 << 7)) LED1_ON; else LED1_OFF; } 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. telemetryCheckState(); mspSerialAllocatePorts(); } } #endif }
void failsafeUpdateState(void) { if (!failsafeIsMonitoring()) { return; } bool receivingRxData = failsafeIsReceivingRxData(); bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; if (!receivingRxData) { beeperMode = BEEPER_RX_LOST; } bool reprocessState; do { reprocessState = false; switch (failsafeState.phase) { case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData reprocessState = true; } else if (!receivingRxData) { if (millis() > failsafeState.throttleLowPeriod) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData } else { failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; } reprocessState = true; } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (failsafeSwitchIsOn) { ENABLE_FLIGHT_MODE(FAILSAFE_MODE); } else { DISABLE_FLIGHT_MODE(FAILSAFE_MODE); } // Throttle low period expired (= low long enough for JustDisarm) failsafeState.throttleLowPeriod = 0; } break; case FAILSAFE_RX_LOSS_DETECTED: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { // Stabilize, and set Throttle to specified level failsafeActivate(); } reprocessState = true; break; case FAILSAFE_LANDING: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } if (armed) { failsafeApplyControlInput(); beeperMode = BEEPER_RX_LOST_LANDING; } if (failsafeShouldHaveCausedLandingByNow() || !armed) { failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.phase = FAILSAFE_LANDED; reprocessState = true; } break; case FAILSAFE_LANDED: ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link mwDisarm(); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; reprocessState = true; break; case FAILSAFE_RX_LOSS_MONITORING: // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. if (receivingRxData) { if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { DISABLE_ARMING_FLAG(PREVENT_ARMING); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } } } else { failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; } break; case FAILSAFE_RX_LOSS_RECOVERED: // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // This is to prevent that JustDisarm is activated on the next iteration. // Because that would have the effect of shutting down failsafe handling on intermittent connections. failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); reprocessState = true; break; default: break; } } while (reprocessState); if (beeperMode != BEEPER_SILENCE) { beeper(beeperMode); } }
void updateBattery(void) { updateBatteryVoltage(); /* battery has just been connected*/ if (batteryState == BATTERY_NOT_PRESENT && vbat > batteryConfig->batterynotpresentlevel ) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; /* wait for VBatt to stabilise then we can calc number of cells (using the filtered value takes a long time to ramp up) We only do this on the ground so don't care if we do block, not worse than original code anyway*/ delay(VBATTERY_STABLE_DELAY); updateBatteryVoltage(); unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ else if (batteryState != BATTERY_NOT_PRESENT && vbat <= batteryConfig->batterynotpresentlevel && !ARMING_FLAG(ARMED)) { batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } switch(batteryState) { case BATTERY_OK: if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } break; case BATTERY_WARNING: if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_CRITICAL; beeper(BEEPER_BAT_CRIT_LOW); } else if (vbat > batteryWarningVoltage) { batteryState = BATTERY_OK; } else { beeper(BEEPER_BAT_LOW); } break; case BATTERY_CRITICAL: if (vbat > batteryCriticalVoltage) { batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } else { beeper(BEEPER_BAT_CRIT_LOW); } break; case BATTERY_NOT_PRESENT: break; } }
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 loop(void) { static uint32_t loopTime; #if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(currentTime); if (shouldProcessRx(currentTime)) { processRx(); #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); } } annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef AUTOTUNE updateAutotuneState(); #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #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.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(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 applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate if (launchState.launchDetected) { bool applyLaunchIdleLogic = true; if (launchState.motorControlAllowed) { // If launch detected we are in launch procedure - control airplane const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); // If user moves the stick - finish the launch if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) { launchState.launchFinished = true; } // Abort launch after a pre-set time if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) { launchState.launchFinished = true; } // Motor control enabled if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { // Don't apply idle logic anymore applyLaunchIdleLogic = false; // Increase throttle gradually over `launch_motor_spinup_time` milliseconds if (navConfig()->fw.launch_motor_spinup_time > 0) { const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, 0.0f, navConfig()->fw.launch_motor_spinup_time, minIdleThrottle, navConfig()->fw.launch_throttle); } else { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; } } } if (applyLaunchIdleLogic) { // Launch idle logic - low throttle and zero out PIDs applyFixedWingLaunchIdleLogic(); } } else { // We are waiting for launch - update launch detector updateFixedWingLaunchDetector(currentTimeUs); // Launch idle logic - low throttle and zero out PIDs applyFixedWingLaunchIdleLogic(); } // Control beeper if (!launchState.launchFinished) { beeper(BEEPER_LAUNCH_MODE_ENABLED); } // Lock out controls rcCommand[ROLL] = 0; rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = 0; }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static batteryState_e batteryState = BATTERY_OK; static uint8_t vbatTimer = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } 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); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(heading - headFreeModeHold); float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryState = calculateBatteryState(); //handle beepers for battery levels if (batteryState == BATTERY_CRITICAL) beeper(BEEPER_BAT_CRIT_LOW); //critically low battery else if (batteryState == BATTERY_WARNING) beeper(BEEPER_BAT_LOW); //low battery } if (feature(FEATURE_CURRENT_METER)) { updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } vbatCycleTime = 0; } } beeperUpdate(); //call periodic beeper handler if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } #ifdef TELEMETRY telemetryCheckState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void processRcStickPositions(throttleStatus_e throttleStatus) { 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 timeMs_t rcDisarmTimeMs; // 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 (!isUsingSticksForArming()) { if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTimeMs = currentTimeMs; tryArm(); } 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()) { const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; if (disarmDelay > armingConfig()->switchDisarmDelayMs) { if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { disarm(DISARM_SWITCH); } } } else { rcDisarmTimeMs = currentTimeMs; } } } // KILLSWITCH disarms instantly if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { disarm(DISARM_KILLSWITCH); } if (rcDelayCommand != 20) { return; } if (isUsingSticksForArming()) { // 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) && armingConfig()->fixed_wing_auto_arm) { return; } else if (ARMING_FLAG(ARMED)) { disarm(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 // 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 (feature(FEATURE_TX_PROF_SEL)) { uint8_t i = 0; 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; } i = 0; // Multiple battery configuration profiles if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_HI + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { setConfigBatteryProfileAndWriteEEPROM(i - 1); batteryDisableProfileAutoswitch(); activateBatteryProfile(); return; } } // Save config if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } // Arming by sticks if (isUsingSticksForArming()) { if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (throttleStatus != THROTTLE_LOW) { tryArm(); return; } } else { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW tryArm(); 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; } }
/* * processRx called from taskUpdateRxMain */ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } updateRSSI(currentTimeUs); if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent Iterm from being reset } } else { airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetITerm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } #ifdef USE_RUNAWAY_TAKEOFF // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low. // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable // prevention for the remainder of the battery. if (ARMING_FLAG(ARMED) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && !STATE(FIXED_WING)) { // Determine if we're in "flight" // - motors running // - throttle over runaway_takeoff_deactivate_throttle_percent // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } } } // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds if (inStableFlight) { if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; // at high throttle levels reduce deactivation delay by 50% if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { deactivateDelay = deactivateDelay / 2; } if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } } else { if (runawayTakeoffDeactivateUs != 0) { runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs); } runawayTakeoffDeactivateUs = 0; } if (runawayTakeoffDeactivateUs == 0) { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000); } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000); } } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); #ifdef USE_DSHOT /* Enable beep warning when the crash flip mode is active */ if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { beeper(BEEPER_CRASH_FLIP_MODE); } #endif if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } #ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } #endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500)); } else { LED1_OFF; rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100)); } if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } #if defined(USE_ACC) || defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(USE_GPS) || defined(USE_MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } #endif if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { if (imuQuaternionHeadfreeOffsetSet()){ beeper(BEEPER_RX_SET); } } } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; if (feature(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); telemetryCheckState(); sharedPortTelemetryEnabled = true; } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); sharedPortTelemetryEnabled = false; } } #endif #ifdef USE_VTX_CONTROL vtxUpdateActivatedChannel(); if (canUpdateVTX()) { handleVTXControlButton(); } #endif #ifdef USE_ACRO_TRAINER pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER #ifdef USE_RC_SMOOTHING_FILTER if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); } #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); return true; }
void tryArm(void) { if (armingConfig()->gyro_cal_on_first_arm) { gyroStartCalibration(true); } updateArmingStatus(); if (!isArmingDisabled()) { if (ARMING_FLAG(ARMED)) { return; } #ifdef USE_DSHOT if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { tryingToArm = ARMING_DELAYED_CRASHFLIP; } else { tryingToArm = ARMING_DELAYED_NORMAL; } } return; } if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashMode = false; if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { flipOverAfterCrashMode = true; #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffCheckDisabled = false; #endif if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } } #endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); resetTryingToArm(); #ifdef USE_ACRO_TRAINER pidAcroTrainerInit(); #endif // USE_ACRO_TRAINER if (isModeActivationConditionPresent(BOXPREARM)) { ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } imuQuaternionHeadfreeOffsetSet(); disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero lastArmingDisabledReason = 0; //beep to indicate arming #ifdef USE_GPS if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING); } #else beeper(BEEPER_ARMING); #endif #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffDeactivateUs = 0; runawayTakeoffAccumulatedUs = 0; runawayTakeoffTriggerUs = 0; #endif } else { resetTryingToArm(); if (!isFirstArmingGyroCalibrationRunning()) { int armingDisabledReason = ffs(getArmingDisableFlags()); if (lastArmingDisabledReason != armingDisabledReason) { lastArmingDisabledReason = armingDisabledReason; beeperWarningBeeps(armingDisabledReason); } } } }
void updateGpsWaypointsAndMode(void) { bool resetNavNow = false; static bool gpsReadyBeepDone = false; if (STATE(GPS_FIX) && GPS_numSat >= 5) { // // process HOME mode // // HOME mode takes priority over HOLD mode. if (rcModeIsActive(BOXGPSHOME)) { if (!FLIGHT_MODE(GPS_HOME_MODE)) { // Transition to HOME mode ENABLE_FLIGHT_MODE(GPS_HOME_MODE); DISABLE_FLIGHT_MODE(GPS_HOLD_MODE); GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]); nav_mode = NAV_MODE_WP; resetNavNow = true; } } else { if (FLIGHT_MODE(GPS_HOME_MODE)) { // Transition from HOME mode DISABLE_FLIGHT_MODE(GPS_HOME_MODE); nav_mode = NAV_MODE_NONE; resetNavNow = true; } // // process HOLD mode // if (rcModeIsActive(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) { if (!FLIGHT_MODE(GPS_HOLD_MODE)) { // Transition to HOLD mode ENABLE_FLIGHT_MODE(GPS_HOLD_MODE); GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LON] = GPS_coord[LON]; GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); nav_mode = NAV_MODE_POSHOLD; resetNavNow = true; } } else { if (FLIGHT_MODE(GPS_HOLD_MODE)) { // Transition from HOLD mode DISABLE_FLIGHT_MODE(GPS_HOLD_MODE); nav_mode = NAV_MODE_NONE; resetNavNow = true; } } } if (!gpsReadyBeepDone) { //if 'ready' beep not yet done beeper(BEEPER_READY_BEEP); //do ready beep now gpsReadyBeepDone = true; //only beep once } } else { if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) { // Transition from HOME or HOLD mode DISABLE_FLIGHT_MODE(GPS_HOME_MODE); DISABLE_FLIGHT_MODE(GPS_HOLD_MODE); nav_mode = NAV_MODE_NONE; resetNavNow = true; } } if (resetNavNow) { GPS_reset_nav(); } }
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 }