diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7fe5c036d1..a822d8f87f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1176,10 +1176,8 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rssi = getRSSI(); -#ifdef USE_SERVOS //Tail servo for tricopters blackboxCurrent->servo[5] = servo[5]; -#endif #ifdef NAV_BLACKBOX blackboxCurrent->navState = navCurrentState; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index eeb1e8c564..b7c39e4b6e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -28,10 +28,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" - -#ifdef USE_SERVOS #include "flight/servos.h" -#endif #include "io/osd.h" #include "io/ledstrip.h" diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index c005c16e0e..3197837c4b 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -179,7 +179,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) // Handle outputs - may override the PWM/PPM inputs if (init->flyingPlatformType == PLATFORM_MULTIROTOR) { // Multicopter -#ifdef USE_SERVOS if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) { type = MAP_TO_SERVO_OUTPUT; } @@ -187,13 +186,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; } else -#endif if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } - } -#ifdef USE_SERVOS - else if (init->flyingPlatformType == PLATFORM_AIRPLANE || init->flyingPlatformType == PLATFORM_HELICOPTER) { + } else if (init->flyingPlatformType == PLATFORM_AIRPLANE || init->flyingPlatformType == PLATFORM_HELICOPTER) { // Fixed wing or HELI (one/two motors and a lot of servos if (timerHardwarePtr->usageFlags & TIM_USE_FW_SERVO) { type = MAP_TO_SERVO_OUTPUT; @@ -202,37 +198,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_MOTOR_OUTPUT; } } -#endif // If timer not mapped - skip if (type == MAP_TO_NONE) continue; -/* -#ifdef USE_SERVOS - if (init->useServos && !init->airplane) { -#if defined(SPRACINGF3MINI) - // remap PWM6+7 as servos - if ((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15) - type = MAP_TO_SERVO_OUTPUT; -#endif - -#if defined(SINGULARITY) - // remap PWM6+7 as servos - if (timerIndex == PWM6 || timerIndex == PWM7) - type = MAP_TO_SERVO_OUTPUT; -#endif - } - -#if defined(SPRACINGF3MINI) - if (((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15) - || ((timerIndex == PWM8 || timerIndex == PWM9 || timerIndex == PWM10 || timerIndex == PWM11) && timerHardwarePtr->tim == TIM2)) { - type = MAP_TO_SERVO_OUTPUT; - } -#endif - } - -#endif // USE_SERVOS -*/ if (type == MAP_TO_PPM_INPUT) { #if defined(USE_RX_PPM) @@ -284,7 +253,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; } -#ifdef USE_SERVOS if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) { pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount; @@ -298,7 +266,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } -#endif } else { continue; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 3711ec12d0..da61629cb7 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -68,12 +68,10 @@ typedef struct drv_pwm_config_s { #ifdef USE_RANGEFINDER bool useTriggerRangefinder; #endif -#ifdef USE_SERVOS bool useServoOutputs; bool useChannelForwarding; // configure additional channels as servos uint16_t servoPwmRate; uint16_t servoCenterPulse; -#endif uint8_t pwmProtocolType; uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ede9280629..e21049f1ad 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -50,10 +50,7 @@ typedef struct { static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; - -#ifdef USE_SERVOS static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; -#endif #ifdef BEEPER_PWM static pwmOutputPort_t beeperPwmPort; @@ -290,7 +287,6 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui return false; } -#ifdef USE_SERVOS bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) { pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput); @@ -323,7 +319,6 @@ void pwmWriteServo(uint8_t index, uint16_t value) } #endif } -#endif #ifdef BEEPER_PWM void pwmWriteBeeper(bool onoffBeep) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6925976220..69728f27c1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1267,7 +1267,6 @@ static void cliModeColor(char *cmdline) } #endif -#ifdef USE_SERVOS static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam) { // print out servo settings @@ -1530,7 +1529,6 @@ static void cliServoMix(char *cmdline) } } } -#endif // USE_SERVOS #ifdef USE_SDCARD @@ -2575,7 +2573,6 @@ static void printConfig(const char *cmdline, bool doDiff) printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0)); -#ifdef USE_SERVOS // print custom servo mixer if exists cliPrintHashLine("servo mix"); cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n"); @@ -2584,7 +2581,6 @@ static void printConfig(const char *cmdline, bool doDiff) // print servo parameters cliPrintHashLine("servo"); printServo(dumpMask, servoParams_CopyArray, servoParams(0)); -#endif cliPrintHashLine("feature"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -2755,17 +2751,13 @@ const clicmd_t cmdTable[] = { #ifdef USE_SERIAL_PASSTHROUGH CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] : passthrough to serial", cliSerialPassthrough), #endif -#ifdef USE_SERVOS CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo), -#endif CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), -#ifdef USE_SERVOS CLI_COMMAND_DEF("smix", "servo mixer", " \r\n" "\treset\r\n" "\tload \r\n" "\treverse r|n", cliServoMix), -#endif #ifdef USE_SDCARD CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 92e7bccded..c4012c92c3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -524,7 +524,6 @@ void processRx(timeUs_t currentTimeUs) LED1_OFF; } -#ifdef USE_SERVOS /* Flaperon mode */ if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) { if (!FLIGHT_MODE(FLAPERON)) { @@ -533,7 +532,6 @@ void processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(FLAPERON); } -#endif #ifdef USE_FLM_TURN_ASSIST /* Turn assistant mode */ @@ -756,9 +754,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // 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 -#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 && mixerConfig()->mixerMode != MIXER_CUSTOM_AIRPLANE @@ -803,7 +799,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) mixTable(); -#ifdef USE_SERVOS if (isMixerUsingServos()) { servoMixer(dT); processServoAutotrim(); @@ -818,7 +813,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (isServoOutputEnabled()) { writeServos(); } -#endif if (motorControlEnable) { writeMotors(); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f85292ca1e..a8a12a78ba 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -269,10 +269,8 @@ void init(void) debugTraceInit(); #endif -#ifdef USE_SERVOS servosInit(); mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state -#endif drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); @@ -314,12 +312,10 @@ void init(void) pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM); pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL); -#ifdef USE_SERVOS pwm_params.useServoOutputs = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servoPwmRate; -#endif pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol; #ifndef BRUSHED_MOTORS diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 51a2eef424..ff2f092cb7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -439,7 +439,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; -#ifdef USE_SERVOS case MSP_SERVO: sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2); break; @@ -466,8 +465,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); } break; -#endif - case MSP2_COMMON_MOTOR_MIXER: for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000); @@ -1009,11 +1006,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 1); // BF: motorConfig()->useUnsyncedPwm sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); -#ifdef USE_SERVOS sbufWriteU16(dst, servoConfig()->servoPwmRate); -#else - sbufWriteU16(dst, 0); -#endif sbufWriteU8(dst, gyroConfig()->gyroSync); break; @@ -1651,7 +1644,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; -#ifdef USE_SERVOS case MSP_SET_SERVO_CONFIGURATION: if (dataSize != (1 + 14)) { return MSP_RESULT_ERROR; @@ -1671,9 +1663,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) servoComputeScalingFactors(tmp_u8); } break; -#endif -#ifdef USE_SERVOS case MSP_SET_SERVO_MIX_RULE: sbufReadU8Safe(&tmp_u8, src); if ((dataSize >= 8) && (tmp_u8 < MAX_SERVO_RULES)) { @@ -1687,7 +1677,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; -#endif case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); @@ -1743,11 +1732,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); motorConfigMutable()->motorPwmRate = sbufReadU16(src); -#ifdef USE_SERVOS servoConfigMutable()->servoPwmRate = sbufReadU16(src); -#else - sbufReadU16(src); -#endif gyroConfigMutable()->gyroSync = sbufReadU8(src); } else return MSP_RESULT_ERROR; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 265d53da54..27ff3006c7 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -199,7 +199,6 @@ void initActiveBoxIds(void) #endif } -#ifdef USE_SERVOS /* * FLAPERON mode active only in case of airplane and custom airplane. Activating on * flying wing can cause bad thing @@ -207,7 +206,6 @@ void initActiveBoxIds(void) if (STATE(FLAPERON_AVAILABLE)) { activeBoxIds[activeBoxIdCount++] = BOXFLAPERON; } -#endif activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1f8039b04b..c2a0291f8c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -487,7 +487,6 @@ groups: - name: PG_GIMBAL_CONFIG type: gimbalConfig_t headers: ["io/gimbal.h"] - condition: USE_SERVOS members: - name: gimbal_mode field: mode @@ -582,7 +581,6 @@ groups: - name: PG_SERVO_CONFIG type: servoConfig_t headers: ["flight/servos.h"] - condition: USE_SERVOS members: - name: servo_center_pulse field: servoCenterPulse @@ -880,17 +878,14 @@ groups: max: 2 - name: fw_iterm_throw_limit field: fixedWingItermThrowLimit - condition: USE_SERVOS min: FW_ITERM_THROW_LIMIT_MIN max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed field: fixedWingReferenceAirspeed - condition: USE_SERVOS min: 1 max: 5000 - name: fw_turn_assist_yaw_gain field: fixedWingCoordinatedYawGain - condition: USE_SERVOS min: 0 max: 2 - name: dterm_notch_hz diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index dc4c75447c..684dd3164b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -302,7 +302,6 @@ bool isMixerEnabled(mixerMode_e mixerMode) return (mixer != NULL) ? true : false; } -#ifdef USE_SERVOS void mixerUpdateStateFlags(void) { // set flag that we're on something with wings @@ -363,16 +362,6 @@ void mixerUsePWMIOConfiguration(void) mixerResetDisarmedMotors(); } -#else -void mixerUsePWMIOConfiguration(void) -{ - motorCount = 4; - for (int i = 0; i < motorCount; i++) { - currentMixer[i] = mixerQuadX[i]; - } - mixerResetDisarmedMotors(); -} -#endif void mixerLoadMix(int index, motorMixer_t *customMixers) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 010a1542b9..ee91d3fb9f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -443,7 +443,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i } } -#ifdef USE_SERVOS static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis) { const float rateError = pidState->rateTarget - pidState->gyroRate; @@ -485,7 +484,6 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic axisPID_Setpoint[axis] = pidState->rateTarget; #endif } -#endif static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis) { @@ -761,15 +759,11 @@ void pidController(void) // Step 4: Run gyro-driven control for (int axis = 0; axis < 3; axis++) { // Apply PID setpoint controller -#ifdef USE_SERVOS if (STATE(FIXED_WING)) { pidApplyFixedWingRateController(&pidState[axis], axis); } else { pidApplyMulticopterRateController(&pidState[axis], axis); } -#else - pidApplyMulticopterRateController(&pidState[axis], axis); -#endif } } diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 60d17e0c8b..2a2d77bfc0 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -22,8 +22,6 @@ #include "platform.h" -#ifdef USE_SERVOS - #include "build/debug.h" #include "build/build_config.h" @@ -568,5 +566,4 @@ bool isServoOutputEnabled(void) bool isMixerUsingServos(void) { return mixerUsesServos; -} -#endif +} \ No newline at end of file diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h index be5147b81c..443360f644 100755 --- a/src/main/target/KFC32F3_INAV/target.h +++ b/src/main/target/KFC32F3_INAV/target.h @@ -136,7 +136,6 @@ // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 10 #define TARGET_MOTOR_COUNT 10 -#define USE_SERVOS // IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 21a6dce307..72025d291e 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -67,7 +67,6 @@ #define USE_TELEMETRY #define USE_BLACKBOX #define USE_SERIAL_RX -#define USE_SERVOS #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC diff --git a/src/main/target/common.h b/src/main/target/common.h index da32a6e035..a4ab787b26 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -21,7 +21,6 @@ #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week -#define USE_SERVOS #define USE_CLI #define USE_RX_PWM diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b0df8b7ce0..23987c9aab 100755 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -34,7 +34,6 @@ #define USE_TELEMETRY_SMARTPORT #define USE_TELEMETRY_LTM #define USE_LED_STRIP -#define USE_SERVOS #define TRANSPONDER #define USE_FAKE_GYRO #define USE_VCP