diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 5b580a14e1..cd44aa1b53 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -89,22 +89,22 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0; switch (filterType) { - case FILTER_LPF: - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; - break; - case FILTER_NOTCH: - b0 = 1; - b1 = -2 * cs; - b2 = 1; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; - break; + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; } // precompute the coefficients diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 0743b6cff8..00e8cddda1 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -212,16 +212,16 @@ void processRcCommand(void) if (isRXDataNew) { // Set RC refresh rate for sampling and channels to filter switch (rxConfig()->rcInterpolation) { - case(RC_SMOOTHING_AUTO): - rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps - break; - case(RC_SMOOTHING_MANUAL): - rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; - break; - case(RC_SMOOTHING_OFF): - case(RC_SMOOTHING_DEFAULT): - default: - rxRefreshRate = rxGetRefreshRate(); + case(RC_SMOOTHING_AUTO): + rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps + break; + case(RC_SMOOTHING_MANUAL): + rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; + break; + case(RC_SMOOTHING_OFF): + case(RC_SMOOTHING_DEFAULT): + default: + rxRefreshRate = rxGetRefreshRate(); } rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; @@ -696,71 +696,71 @@ void subTaskMainSubprocesses(void) gyro.dev.temperature(&telemTemperature1); } - #ifdef MAG - if (sensors(SENSOR_MAG)) { - updateMagHold(); - } - #endif - - #ifdef GTUNE - updateGtuneState(); - #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(&masterConfig.airplaneConfig); - } - } - #endif - - // If we're armed, at minimum throttle, and we do arming via the - // sticks, do not process yaw input from the rx. We do this so the - // motors do not spin up while we are trying to arm or disarm. - // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. - if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck - #ifndef USE_QUAD_MIXER_ONLY - #ifdef USE_SERVOS - && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo) - #endif - && mixerConfig()->mixerMode != MIXER_AIRPLANE - && mixerConfig()->mixerMode != MIXER_FLYING_WING - #endif - ) { - rcCommand[YAW] = 0; - setpointRate[YAW] = 0; +#ifdef MAG + if (sensors(SENSOR_MAG)) { + updateMagHold(); } +#endif - if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); - } +#ifdef GTUNE + updateGtuneState(); +#endif - processRcCommand(); - - #ifdef GPS - if (sensors(SENSOR_GPS)) { - if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { - updateGpsStateForHomeAndHoldMode(); +#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(&masterConfig.airplaneConfig); } } - #endif +#endif - #ifdef USE_SDCARD - afatfs_poll(); - #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) && servoMixerConfig()->tri_unarmed_servo) +#endif + && mixerConfig()->mixerMode != MIXER_AIRPLANE + && mixerConfig()->mixerMode != MIXER_FLYING_WING +#endif + ) { + rcCommand[YAW] = 0; + setpointRate[YAW] = 0; + } - #ifdef BLACKBOX - if (!cliMode && feature(FEATURE_BLACKBOX)) { - handleBlackbox(startTime); + 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 + } +#endif - #ifdef TRANSPONDER - transponderUpdate(startTime); - #endif - DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); +#ifdef USE_SDCARD + afatfs_poll(); +#endif + +#ifdef BLACKBOX + if (!cliMode && feature(FEATURE_BLACKBOX)) { + handleBlackbox(startTime); + } +#endif + +#ifdef TRANSPONDER + transponderUpdate(startTime); +#endif + DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); } void subTaskMotorUpdate(void) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 707957aa54..314857d427 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -269,50 +269,50 @@ void writeServos(void) uint8_t servoIndex = 0; switch (currentMixerMode) { - case MIXER_BICOPTER: - pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); - pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); - break; + case MIXER_BICOPTER: + pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); + pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); + break; - case MIXER_TRI: - case MIXER_CUSTOM_TRI: - if (servoMixerConfig->tri_unarmed_servo) { - // if unarmed flag set, we always move servo + case MIXER_TRI: + case MIXER_CUSTOM_TRI: + if (servoMixerConfig->tri_unarmed_servo) { + // if unarmed flag set, we always move servo + pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); + } else { + // otherwise, only move servo when copter is armed + if (ARMING_FLAG(ARMED)) pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); - } else { - // otherwise, only move servo when copter is armed - if (ARMING_FLAG(ARMED)) - pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); - else - pwmWriteServo(servoIndex++, 0); // kill servo signal completely. - } - break; + else + pwmWriteServo(servoIndex++, 0); // kill servo signal completely. + } + break; - case MIXER_FLYING_WING: - pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); - pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); - break; + case MIXER_FLYING_WING: + pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); + pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); + break; - case MIXER_DUALCOPTER: - pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); - pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); - break; + case MIXER_DUALCOPTER: + pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); + pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); + break; - case MIXER_CUSTOM_AIRPLANE: - case MIXER_AIRPLANE: - for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { - pwmWriteServo(servoIndex++, servo[i]); - } - break; + case MIXER_CUSTOM_AIRPLANE: + case MIXER_AIRPLANE: + for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { + pwmWriteServo(servoIndex++, servo[i]); + } + break; - case MIXER_SINGLECOPTER: - for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) { - pwmWriteServo(servoIndex++, servo[i]); - } - break; + case MIXER_SINGLECOPTER: + for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) { + pwmWriteServo(servoIndex++, servo[i]); + } + break; - default: - break; + default: + break; } // Two servos for SERVO_TILT, if enabled @@ -410,27 +410,27 @@ void servoTable(void) { // airplane / servo mixes switch (currentMixerMode) { - case MIXER_CUSTOM_AIRPLANE: - case MIXER_FLYING_WING: - case MIXER_AIRPLANE: - case MIXER_BICOPTER: - case MIXER_CUSTOM_TRI: - case MIXER_TRI: - case MIXER_DUALCOPTER: - case MIXER_SINGLECOPTER: - case MIXER_GIMBAL: - servoMixer(); - break; + case MIXER_CUSTOM_AIRPLANE: + case MIXER_FLYING_WING: + case MIXER_AIRPLANE: + case MIXER_BICOPTER: + case MIXER_CUSTOM_TRI: + case MIXER_TRI: + case MIXER_DUALCOPTER: + case MIXER_SINGLECOPTER: + case MIXER_GIMBAL: + servoMixer(); + break; - /* - case MIXER_GIMBAL: - servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); - break; - */ + /* + case MIXER_GIMBAL: + servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + break; + */ - default: - break; + default: + break; } // camera stabilization