void processRx(void) { int i; uint32_t auxState = 0; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!rcOptions[BOXARM]) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { failsafe->vTable->enable(); } failsafe->vTable->updateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { resetErrorAngle(); resetErrorGyro(); } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } // Check AUX switches // auxState is a bitmask, 3 bits per channel. aux1 is first. // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 // // the three bits are as follows: // bit 1 is SET when the stick is less than 1300 // bit 2 is SET when the stick is between 1300 and 1700 // bit 3 is SET when the stick is above 1700 // if the value is 1300 or 1700 NONE of the three bits are set. for (i = 0; i < 4; i++) { auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2); auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) | (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) | (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16; } for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) rcOptions[i] = (auxState & currentProfile.activate[i]) > 0; if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode if (!f.ANGLE_MODE) { resetErrorAngle(); f.ANGLE_MODE = 1; } } else { f.ANGLE_MODE = 0; // failsafe support } if (rcOptions[BOXHORIZON]) { f.ANGLE_MODE = 0; if (!f.HORIZON_MODE) { resetErrorAngle(); f.HORIZON_MODE = 1; } } else { f.HORIZON_MODE = 0; } if (f.ANGLE_MODE || f.HORIZON_MODE) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (rcOptions[BOXMAG]) { if (!f.MAG_MODE) { f.MAG_MODE = 1; magHold = heading; } } else { f.MAG_MODE = 0; } if (rcOptions[BOXHEADFREE]) { if (!f.HEADFREE_MODE) { f.HEADFREE_MODE = 1; } } else { f.HEADFREE_MODE = 0; } if (rcOptions[BOXHEADADJ]) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (rcOptions[BOXPASSTHRU]) { f.PASSTHRU_MODE = 1; } else { f.PASSTHRU_MODE = 0; } if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) { f.HEADFREE_MODE = 0; } }
void processRx(void) { calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!rcOptions[BOXARM]) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { failsafe->vTable->enable(); } failsafe->vTable->updateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { resetErrorAngle(); resetErrorGyro(); } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile->activate, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateRcOptions(currentProfile->activate); bool canUseHorizonMode = true; if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { resetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (rcOptions[BOXHORIZON] && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { resetErrorAngle(); 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 (rcOptions[BOXMAG]) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (rcOptions[BOXHEADFREE]) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (rcOptions[BOXHEADADJ]) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (rcOptions[BOXPASSTHRU]) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } }