diff --git a/make/source.mk b/make/source.mk index a68efec119..d0115e13f3 100644 --- a/make/source.mk +++ b/make/source.mk @@ -75,7 +75,6 @@ COMMON_SRC = \ fc/controlrate_profile.c \ drivers/camera_control.c \ drivers/accgyro/gyro_sync.c \ - drivers/pwm_esc_detect.c \ drivers/rx/rx_spi.c \ drivers/rx/rx_xn297.c \ drivers/rx/rx_pwm.c \ diff --git a/src/main/drivers/pwm_esc_detect.c b/src/main/drivers/pwm_esc_detect.c deleted file mode 100644 index 961df06ec7..0000000000 --- a/src/main/drivers/pwm_esc_detect.c +++ /dev/null @@ -1,60 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include -#include -#include - -#include "platform.h" - -#ifdef USE_BRUSHED_ESC_AUTODETECT - -#include "build/build_config.h" - -#include "drivers/io.h" -#include "drivers/time.h" -#include "drivers/timer.h" - -#include "pwm_esc_detect.h" - -static uint8_t hardwareMotorType = MOTOR_UNKNOWN; - -void detectBrushedESC(ioTag_t motorIoTag) -{ - IO_t motorDetectPin = IOGetByTag(motorIoTag); - IOInit(motorDetectPin, OWNER_SYSTEM, 0); - IOConfigGPIO(motorDetectPin, IOCFG_IPU); - - delayMicroseconds(10); // allow configuration to settle - - // Check presence of brushed ESC's - if (IORead(motorDetectPin)) { - hardwareMotorType = MOTOR_BRUSHLESS; - } else { - hardwareMotorType = MOTOR_BRUSHED; - } - IORelease(motorDetectPin); -} - -uint8_t getDetectedMotorType(void) -{ - return hardwareMotorType; -} -#endif diff --git a/src/main/drivers/pwm_esc_detect.h b/src/main/drivers/pwm_esc_detect.h deleted file mode 100644 index ee1310986e..0000000000 --- a/src/main/drivers/pwm_esc_detect.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#pragma once - -#include "drivers/io_types.h" - -typedef enum { - MOTOR_UNKNOWN = 0, - MOTOR_BRUSHED, - MOTOR_BRUSHLESS -} HardwareMotorTypes_e; - -void detectBrushedESC(ioTag_t motorIoTag); -uint8_t getDetectedMotorType(void); diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 0de8a63e51..735f59c4cf 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -63,7 +63,6 @@ #include "drivers/nvic.h" #include "drivers/persistent.h" #include "drivers/pin_pull_up_down.h" -#include "drivers/pwm_esc_detect.h" #include "drivers/pwm_output.h" #include "drivers/rx/rx_pwm.h" #include "drivers/sensor.h" @@ -280,20 +279,6 @@ void init(void) targetConfiguration(); #endif -#if defined(USE_BRUSHED_ESC_AUTODETECT) - // Opportunistically use the first motor pin of the default configuration for detection. - // We are doing this as with some boards, timing seems to be important, and the later detection will fail. -#if defined(MOTOR1_PIN) - ioTag_t motorIoTag = IO_TAG(MOTOR1_PIN); -#else - ioTag_t motorIoTag = IO_TAG_NONE; -#endif - - if (motorIoTag) { - detectBrushedESC(motorIoTag); - } -#endif - enum { FLASH_INIT_ATTEMPTED = (1 << 0), SD_INIT_ATTEMPTED = (1 << 1), @@ -417,15 +402,6 @@ void init(void) dbgPinInit(); #endif -#if defined(USE_BRUSHED_ESC_AUTODETECT) - // Now detect again with the actually configured pin for motor 1, if it is not the default pin. - ioTag_t configuredMotorIoTag = motorConfig()->dev.ioTags[0]; - - if (configuredMotorIoTag && configuredMotorIoTag != motorIoTag) { - detectBrushedESC(configuredMotorIoTag); - } -#endif - debugMode = systemConfig()->debug_mode; #ifdef TARGET_PREINIT diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index 09931e5a20..a8e95d1a0b 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -26,7 +26,6 @@ #ifdef USE_MOTOR -#include "drivers/pwm_esc_detect.h" #include "drivers/pwm_output.h" #include "pg/pg.h" @@ -55,19 +54,9 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; motorConfig->dev.useUnsyncedPwm = true; #else -#ifdef USE_BRUSHED_ESC_AUTODETECT - if (getDetectedMotorType() == MOTOR_BRUSHED) { - motorConfig->minthrottle = 1000; - motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; - motorConfig->dev.useUnsyncedPwm = true; - } else -#endif // USE_BRUSHED_ESC_AUTODETECT - { - motorConfig->minthrottle = 1070; - motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED; - } + motorConfig->minthrottle = 1070; + motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED; #endif // BRUSHED_MOTORS motorConfig->maxthrottle = 2000; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 1790fe5c04..2178068937 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -140,7 +140,6 @@ #undef USE_VTX_TRAMP #undef USE_VTX_MSP #undef USE_CAMERA_CONTROL -#undef USE_BRUSHED_ESC_AUTODETECT #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 3e5a348ddd..3990a853b5 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -172,8 +172,6 @@ #define USE_RX_PPM #define USE_RX_PWM -#define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot - #define USE_PINIO #if !defined(USE_SERIAL_RX)