void updateInflightCalibrationState(void) { if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !rcModeIsActive(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = false; } if (rcModeIsActive(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; } }
armingPreventedReason_e getArmingPreventionBlinkMask(void) { if (isCalibrating()) { return ARM_PREV_CALIB; } if (rcModeIsActive(BOXFAILSAFE) || failsafePhase() == FAILSAFE_LANDED) { return ARM_PREV_FAILSAFE; } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { return ARM_PREV_ANGLE; } if (cliMode) { return ARM_PREV_CLI; } if (isSystemOverloaded()) { return ARM_PREV_OVERLOAD; } return ARM_PREV_NONE; }
void mwArm(void) { if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { return; } if (rcModeIsActive(BOXFAILSAFE)) { return; } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP_SERVER); if (sharedBlackboxAndMspPort) { mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); } startBlackbox(); } #endif disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero //beep to indicate arming #ifdef GPS if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) beeper(BEEPER_ARMING_GPS_FIX); else beeper(BEEPER_ARMING); #else beeper(BEEPER_ARMING); #endif return; } } if (!ARMING_FLAG(ARMED)) { beeperConfirmationBeeps(1); } }
void updateGtuneState(void) { static bool GTuneWasUsed = false; if (rcModeIsActive(BOXGTUNE)) { if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { ENABLE_FLIGHT_MODE(GTUNE_MODE); init_Gtune(); GTuneWasUsed = true; } if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) { saveConfigAndNotify(); GTuneWasUsed = false; } } else { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { DISABLE_FLIGHT_MODE(GTUNE_MODE); } } }
void annexCode(void) { 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 (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); }
static void updateLEDs(void) { 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()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } uint32_t nextBlinkMask = getArmingPreventionBlinkMask(); warningLedSetBlinkMask(nextBlinkMask); warningLedUpdate(); } }
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); }
STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate) { static float lastRateForDelta[3]; SET_PID_LUX_FLOAT_CORE_LOCALS(axis); const float rateError = angleRate - gyroRate; // -----calculate P component float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100; // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW) { if (pidProfile->yaw_lpf_hz) { PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); } if (pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } } // -----calculate I component float ITerm = lastITermf[axis] + luxITermScale * rateError * getdT() * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); // Anti windup protection if (rcModeIsActive(BOXAIRMODE)) { if (STATE(ANTI_WINDUP) || motorLimitReached) { ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]); } else { ITermLimitf[axis] = ABS(ITerm); } } lastITermf[axis] = ITerm; // -----calculate D component float DTerm; if (pidProfile->D8[axis] == 0) { // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { float delta; if (pidProfile->deltaMethod == PID_DELTA_FROM_MEASUREMENT) { delta = -(gyroRate - lastRateForDelta[axis]); lastRateForDelta[axis] = gyroRate; } else { delta = rateError - lastRateForDelta[axis]; lastRateForDelta[axis] = rateError; } // Divide delta by targetLooptime to get differential (ie dr/dt) delta /= getdT(); // Filter delta if (pidProfile->dterm_notch_hz) { delta = biquadFilterApply(&dtermFilterNotch[axis], delta); } if (pidProfile->dterm_lpf_hz) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { delta = biquadFilterApply(&dtermFilterLpf[axis], delta); } else { // DTerm delta low pass filter delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); } } DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif GET_PID_LUX_FLOAT_CORE_LOCALS(axis); // -----calculate total PID output return lrintf(PTerm + ITerm + DTerm); }
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_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate) { static float lastRateForDelta[3]; static float deltaState[3][DTERM_AVERAGE_COUNT]; SET_PID_LUX_FLOAT_CORE_LOCALS(axis); const float rateError = angleRate - gyroRate; // -----calculate P component float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100; // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW && pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } // -----calculate I component float ITerm = lastITermf[axis] + luxITermScale * rateError * dT * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); // Anti windup protection if (rcModeIsActive(BOXAIRMODE)) { if (STATE(ANTI_WINDUP) || motorLimitReached) { ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]); } else { ITermLimitf[axis] = ABS(ITerm); } } lastITermf[axis] = ITerm; // -----calculate D component float DTerm; if (pidProfile->D8[axis] == 0) { // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { // delta calculated from measurement float delta = -(gyroRate - lastRateForDelta[axis]); lastRateForDelta[axis] = gyroRate; // Divide delta by dT to get differential (ie dr/dt) delta *= (1.0f / dT); if (pidProfile->dterm_cut_hz) { // DTerm delta low pass filter delta = applyBiQuadFilter(delta, &deltaFilterState[axis]); } else { // When DTerm low pass filter disabled apply moving average to reduce noise delta = filterApplyAveragef(delta, DTERM_AVERAGE_COUNT, deltaState[axis]); } DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif GET_PID_LUX_FLOAT_CORE_LOCALS(axis); // -----calculate total PID output return lrintf(PTerm + ITerm + DTerm); }
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 }
static void detectAndApplySignalLossBehaviour(void) { int channel; uint16_t sample; bool useValueFromRx = true; bool rxIsDataDriven = isRxDataDriven(); uint32_t currentMilliTime = millis(); if (!rxIsDataDriven) { rxSignalReceived = rxSignalReceivedNotDataDriven; rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven; } if (!rxSignalReceived || rxIsInFailsafeMode) { useValueFromRx = false; } #ifdef DEBUG_RX_SIGNAL_LOSS debug[0] = rxSignalReceived; debug[1] = rxIsInFailsafeMode; debug[2] = rcReadRawFunc(&rxRuntimeConfig, 0); #endif rxResetFlightChannelStatus(); for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { sample = (useValueFromRx) ? rcRaw[channel] : PPM_RCVR_TIMEOUT; bool validPulse = isPulseValid(sample); if (!validPulse) { if (currentMilliTime < rcInvalidPulsPeriod[channel]) { sample = rcData[channel]; // hold channel for MAX_INVALID_PULS_TIME } else { sample = getRxfailValue(channel); // after that apply rxfail value rxUpdateFlightChannelStatus(channel, validPulse); } } else { rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME; } if (rxIsDataDriven) { rcData[channel] = sample; } else { rcData[channel] = calculateNonDataDrivenChannel(channel, sample); } } rxFlightChannelsValid = rxHaveValidFlightChannels(); if ((rxFlightChannelsValid) && !(rcModeIsActive(BOXFAILSAFE) && feature(FEATURE_FAILSAFE))) { failsafeOnValidDataReceived(); } else { rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true; failsafeOnValidDataFailed(); for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { rcData[channel] = getRxfailValue(channel); } } #ifdef DEBUG_RX_SIGNAL_LOSS debug[3] = rcData[THROTTLE]; #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 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(); } }