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();
}
}
