rollPitchStatus_e calculateRollPitchCenterStatus(void) { if (((rcData[PITCH] < (rxConfig()->midrc + AIRMODE_DEADBAND)) && (rcData[PITCH] > (rxConfig()->midrc -AIRMODE_DEADBAND))) && ((rcData[ROLL] < (rxConfig()->midrc + AIRMODE_DEADBAND)) && (rcData[ROLL] > (rxConfig()->midrc -AIRMODE_DEADBAND)))) return CENTERED; return NOT_CENTERED; }
throttleStatus_e calculateThrottleStatus(void) { const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig()->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + deadband3d_throttle))) return THROTTLE_LOW; else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck)) return THROTTLE_LOW; return THROTTLE_HIGH; }
throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)) { if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) return THROTTLE_LOW; } else { if (rcData[THROTTLE] < rxConfig()->mincheck) return THROTTLE_LOW; } return THROTTLE_HIGH; }
throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D)) { if (IS_RC_MODE_ACTIVE(BOX3DDISABLE) || isModeActivationConditionPresent(BOX3DONASWITCH)) { if (rcData[THROTTLE] < rxConfig()->mincheck) { return THROTTLE_LOW; } } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) { return THROTTLE_LOW; } } else if (rcData[THROTTLE] < rxConfig()->mincheck) { return THROTTLE_LOW; } return THROTTLE_HIGH; }
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]; } } }
static void failsafeApplyControlInput(void) { for (int i = 0; i < 3; i++) { rcData[i] = rxConfig()->midrc; } rcData[THROTTLE] = failsafeConfig()->failsafe_throttle; }
void taskUpdateBattery(void) { static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; if (feature(FEATURE_VBAT)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; voltageMeterUpdate(); batteryUpdate(); } } if (feature(FEATURE_AMPERAGE_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; if (batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC) { amperageUpdateMeter(ibatTimeSinceLastServiced); } else { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; amperageUpdateVirtualMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset); } } } }
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet switch (motorConfig()->dev.motorPwmProtocol) { #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); } else { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); } motorOutputHigh = DSHOT_MAX_THROTTLE; deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; break; #endif default: if (feature(FEATURE_3D)) { disarmMotorOutput = flight3DConfig()->neutral3d; motorOutputLow = flight3DConfig()->limit3d_low; motorOutputHigh = flight3DConfig()->limit3d_high; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dLow = flight3DConfig()->deadband3d_low; } else { disarmMotorOutput = motorConfig()->mincommand; motorOutputLow = motorConfig()->minthrottle; motorOutputHigh = motorConfig()->maxthrottle; } break; } rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck; rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; }
// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here. bool areSticksActive(uint8_t stickPercentLimit) { for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) { const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband; uint8_t stickPercent = 0; if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) { stickPercent = 100; } else { if (rcData[axis] > (rxConfig()->midrc + deadband)) { stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband); } else if (rcData[axis] < (rxConfig()->midrc - deadband)) { stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN); } } if (stickPercent >= stickPercentLimit) { return true; } } return false; }
// calculate the throttle stick percent - integer math is good enough here. uint8_t calculateThrottlePercent(void) { uint8_t ret = 0; if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) { if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) { ret = 100; } else { if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle); } else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) { ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN); } } } else { ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100); } return ret; }
static void sendThrottleOrBatterySizeAsRpm(uint16_t deadband3d_throttle) { uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); } else { serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } }
void annexCode(void) { int32_t tmp, tmp2; tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - 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 (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void subTaskPidController(void) { const uint32_t startTime = micros(); // PID - note this is function pointer set by setPIDController() pidLuxFloat( pidProfile(), currentControlRateProfile, imuConfig()->max_angle_inclination, &accelerometerConfig()->accelerometerTrims, rxConfig() ); if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;} }
static void updateInflightCalibrationState(void) { if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = false; } if (IS_RC_MODE_ACTIVE(BOXCALIB)) { // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone) InflightcalibratingA = 50; AccInflightCalibrationActive = true; } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) { AccInflightCalibrationMeasurementDone = false; AccInflightCalibrationSavetoEEProm = true; } }
static void updateRcStickPositions(void) { stickPositions_e tmp = 0; tmp |= ((rcData[ROLL] > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2); tmp |= ((rcData[ROLL] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2); tmp |= ((rcData[PITCH] > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2); tmp |= ((rcData[PITCH] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2); tmp |= ((rcData[YAW] > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2); tmp |= ((rcData[YAW] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2); tmp |= ((rcData[THROTTLE] > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2); tmp |= ((rcData[THROTTLE] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2); rcStickPositions = tmp; }
void targetConfiguration(void) { gyroConfigMutable()->looptime = 1000; rxConfigMutable()->rcmap[0] = 1; rxConfigMutable()->rcmap[1] = 2; rxConfigMutable()->rcmap[2] = 3; rxConfigMutable()->rcmap[3] = 0; featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP); serialConfigMutable()->portConfigs[0].functionMask = FUNCTION_MSP; if (rxConfig()->receiverType == RX_TYPE_SERIAL) { serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } }
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig) { if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK && (rxConfig()->serialrx_provider == SERIALRX_SPEKTRUM1024 || rxConfig()->serialrx_provider == SERIALRX_SPEKTRUM2048 || rxConfig()->serialrx_provider == SERIALRX_SBUS || rxConfig()->serialrx_provider == SERIALRX_SUMD || rxConfig()->serialrx_provider == SERIALRX_SUMH || rxConfig()->serialrx_provider == SERIALRX_XBUS_MODE_B || rxConfig()->serialrx_provider == SERIALRX_XBUS_MODE_B_RJ01 || rxConfig()->serialrx_provider == SERIALRX_IBUS)) { return true; } #ifdef USE_TELEMETRY_IBUS if ( portConfig->functionMask & FUNCTION_TELEMETRY_IBUS && portConfig->functionMask & FUNCTION_RX_SERIAL && rxConfig()->serialrx_provider == SERIALRX_IBUS) { // IBUS serial RX & telemetry return true; } #endif return false; }
static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); // 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] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { resetYawAxis(); } processRcCommand(); }
void taskUpdateBattery(void) { static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; if (feature(FEATURE_VBAT)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; updateBattery(); } } if (feature(FEATURE_CURRENT_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); updateCurrentMeter(ibatTimeSinceLastServiced, throttleStatus); } } }
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 }
static void subTaskMainSubprocesses(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // Read out gyro temperature if used for telemmetry if (feature(FEATURE_TELEMETRY)) { gyroReadTemperature(); } #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #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] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { resetYawAxis(); } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } processRcCommand(); #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && blackboxConfig()->device) { handleBlackbox(currentTimeUs); } #else UNUSED(currentTimeUs); #endif #ifdef TRANSPONDER transponderUpdate(currentTimeUs); #endif DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); }
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 taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; // Calculate average cycle time and average jitter filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT); debug[0] = cycleTime; debug[1] = cycleTime - filteredCycleTime; imuUpdateGyroAndAttitude(); annexCode(); if (rxConfig()->rcSmoothing) { filterRc(); } #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #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(); } } #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] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && mixerConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->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( pidProfile(), currentControlRateProfile, imuConfig()->max_angle_inclination, &accelerometerConfig()->accelerometerTrims, rxConfig() ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; // 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] - rxConfig()->midrc), 500); if (axis == ROLL || axis == PITCH) { if (rcControlsConfig()->deadband) { if (tmp > rcControlsConfig()->deadband) { tmp -= 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 (rcControlsConfig()->yaw_deadband) { if (tmp > rcControlsConfig()->yaw_deadband) { tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->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)pidProfile()->P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)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] < rxConfig()->midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - 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(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); debug[3] = ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } debug[3] = ARMING_FLAG(OK_TO_ARM); warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void init(void) { drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(systemConfig()->emf_avoidance); #endif i2cSetOverclock(systemConfig()->i2c_highspeed); systemInit(); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = debugConfig()->debug_mode; #ifdef USE_EXTI EXTIInit(); #endif #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); } else { ledInit(true); } #else ledInit(false); #endif #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef BUTTONS buttonsInit(); if (!isMPUSoftReset()) { buttonsHandleColdBootButtonPresses(); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(rxConfig()); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); serialInit(feature(FEATURE_SOFTSERIAL)); mixerInit(customMotorMixer(0)); #ifdef USE_SERVOS mixerInitServos(customServoMixer(0)); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; sonarGPIOConfig_t sonarGPIOConfig; if (feature(FEATURE_SONAR)) { bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC); sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins); sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio; sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin; sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio; sonarGPIOConfig.echoPin = sonarHardware->echo_pin; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2); #endif #if defined(USE_UART3) pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3); #endif #if defined(USE_UART4) pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4); #endif #if defined(USE_UART5) pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = ( feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC ); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate; pwm_params.idlePulse = calculateMotorOff(); if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(); // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params); mixerUsePWMIOConfiguration(pwmIOConfiguration); #ifdef DEBUG_PWM_CONFIGURATION debug[2] = pwmIOConfiguration->pwmInputCount; debug[3] = pwmIOConfiguration->ppmInputCount; #endif if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #ifdef STM32F303xC #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPI3); } #else spiInit(SPI3); #endif #endif #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_UART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.channelMask = 0; #ifdef ADC_BATTERY adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0); #endif #ifdef ADC_RSSI adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0); #endif #ifdef ADC_AMPERAGE adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0); #endif #ifdef ADC_POWER_12V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V); #endif #ifdef ADC_POWER_5V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V); #endif #ifdef ADC_POWER_3V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V); #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0; #endif adcInit(&adc_params); #endif initBoardAlignment(); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(); } #endif #ifdef NAZE if (hardwareRevision < NAZE32_REV5) { gyroConfig()->gyro_sync = 0; } #endif if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); mspInit(); mspSerialInit(); const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom); pidInitFilters(pidProfile()); #ifdef USE_SERVOS mixerInitialiseServoFiltering(targetPidLooptime); #endif imuInit(); #ifdef USE_CLI cliInit(); #endif failsafeInit(); rxInit(modeActivationProfile()->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit(); navigationInit(pidProfile()); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(transponderConfig()->data); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif if (mixerConfig()->mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif if (feature(FEATURE_VBAT)) { // Now that everything has powered up the voltage and cell count be determined. voltageMeterInit(); batteryInit(); } if (feature(FEATURE_AMPERAGE_METER)) { amperageMeterInit(); } #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif void configureScheduler(void) { schedulerInit(); setTaskEnabled(TASK_SYSTEM, true); uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); rescheduleTask(TASK_GYRO, gyroPeriodUs); setTaskEnabled(TASK_GYRO, true); rescheduleTask(TASK_PID, gyroPeriodUs); setTaskEnabled(TASK_PID, true); if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif }
/* This function processes RX dependent coefficients when new RX commands are available Those are: TPA, throttle expo */ static void updateRcCommands(void) { int32_t prop2; // 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 (int axis = 0; axis < 3; axis++) { int32_t prop1; int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); if (axis == ROLL || axis == PITCH) { if (rcControlsConfig()->deadband) { if (tmp > rcControlsConfig()->deadband) { tmp -= rcControlsConfig()->deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupPitchRoll(tmp); prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. 100 means 100% of the pids PIDweight[axis] = prop2; } else { if (rcControlsConfig()->yaw_deadband) { if (tmp > rcControlsConfig()->yaw_deadband) { tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupYaw(tmp) * -rcControlsConfig()->yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; // YAW TPA disabled. PIDweight[axis] = 100; } #ifdef USE_PID_MW23 // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100; #endif if (rcData[axis] < rxConfig()->midrc) { rcCommand[axis] = -rcCommand[axis]; } } int32_t tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } }
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 processRcCommand(void) { if (rxConfig()->rcSmoothing) { filterRc(); } }
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 subTaskMainSubprocesses(void) { const uint32_t startTime = micros(); // 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); } #ifdef GTUNE updateGtuneState(); #endif #if defined(BARO) || defined(SONAR) // FIXME outdated comments? // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState // this must be called here since applyAltHold directly manipulates rcCommands[] updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #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] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && mixerConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } processRcCommand(); #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;} }