diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index ed58b25291..90ad21c900 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -24,6 +24,7 @@
#include "io.h"
#include "timer.h"
#include "pwm_output.h"
+#include "system.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
@@ -280,3 +281,28 @@ void servoInit(const servoConfig_t *servoConfig)
}
#endif
+
+#ifdef BRUSHED_ESC_AUTODETECT
+uint8_t hardwareMotorType = MOTOR_UNKNOWN;
+
+void detectBrushedESC(void)
+{
+ int i = 0;
+ while (!(timerHardware[i].usageFlags & TIM_USE_MOTOR) && (i < USABLE_TIMER_CHANNEL_COUNT)) {
+ i++;
+ }
+
+ IO_t MotorDetectPin = IOGetByTag(timerHardware[i].tag);
+ 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;
+ }
+}
+#endif
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 9d94a1d074..8c6488298e 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -114,3 +114,15 @@ pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
void pwmDisableMotors(void);
void pwmEnableMotors(void);
+
+#ifdef BRUSHED_ESC_AUTODETECT
+typedef enum {
+ MOTOR_UNKNOWN = 0,
+ MOTOR_BRUSHED,
+ MOTOR_BRUSHLESS
+} HardwareMotorTypes_e;
+
+extern uint8_t hardwareMotorType;
+
+void detectBrushedESC(void);
+#endif
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 59a8ca8181..4359fcd2b4 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -274,9 +274,19 @@ void resetMotorConfig(motorConfig_t *motorConfig)
motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->useUnsyncedPwm = true;
#else
- motorConfig->minthrottle = 1070;
- motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
- motorConfig->motorPwmProtocol = PWM_TYPE_ONESHOT125;
+#ifdef BRUSHED_ESC_AUTODETECT
+ if (hardwareMotorType == MOTOR_BRUSHED) {
+ motorConfig->minthrottle = 1000;
+ motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
+ motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
+ motorConfig->useUnsyncedPwm = true;
+ } else
+#endif
+ {
+ motorConfig->minthrottle = 1070;
+ motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
+ motorConfig->motorPwmProtocol = PWM_TYPE_ONESHOT125;
+ }
#endif
motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000;
diff --git a/src/main/main.c b/src/main/main.c
index e8abde0892..2bf3e063ae 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -35,7 +35,6 @@
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/dma.h"
-#include "drivers/gpio.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
@@ -162,6 +161,10 @@ void init(void)
detectHardwareRevision();
#endif
+#ifdef BRUSHED_ESC_AUTODETECT
+ detectBrushedESC();
+#endif
+
initEEPROM();
ensureEEPROMContainsValidData();
@@ -184,28 +187,22 @@ void init(void)
#endif
#if defined(BUTTONS)
- gpio_config_t buttonAGpioConfig = {
- BUTTON_A_PIN,
- Mode_IPU,
- Speed_2MHz
- };
- gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
+ IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN));
+ IOInit(buttonAPin, OWNER_SYSTEM, 0);
+ IOConfigGPIO(buttonAPin, IOCFG_IPU);
- gpio_config_t buttonBGpioConfig = {
- BUTTON_B_PIN,
- Mode_IPU,
- Speed_2MHz
- };
- gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
+ IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN));
+ IOInit(buttonBPin, OWNER_SYSTEM, 0);
+ IOConfigGPIO(buttonBPin, IOCFG_IPU);
// Check status of bind plug and exit if not active
- delayMicroseconds(10); // allow GPIO configuration to settle
+ delayMicroseconds(10); // allow configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
- bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
+ bothButtonsHeld = !IORead(buttonAPin) && !IORead(buttonBPin);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c
index 049a06e882..89bf61ecbb 100644
--- a/src/main/target/ALIENFLIGHTF1/config.c
+++ b/src/main/target/ALIENFLIGHTF1/config.c
@@ -33,7 +33,7 @@
#include "config/config_profile.h"
#include "config/config_master.h"
-#include "hardware_revision.h"
+#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz
// alternative defaults settings for AlienFlight targets
void targetConfiguration(master_t *config)
@@ -42,10 +42,7 @@ void targetConfiguration(master_t *config)
config->rxConfig.spektrum_sat_bind_autoreset = 1;
if (hardwareMotorType == MOTOR_BRUSHED) {
- config->motorConfig.minthrottle = 1000;
- config->motorConfig.motorPwmRate = 32000;
- config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
- config->motorConfig.useUnsyncedPwm = true;
+ config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
}
config->profile[0].pidProfile.P8[ROLL] = 90;
diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.c b/src/main/target/ALIENFLIGHTF1/hardware_revision.c
deleted file mode 100644
index 4ea15ecded..0000000000
--- a/src/main/target/ALIENFLIGHTF1/hardware_revision.c
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it 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 is distributed in the hope that it 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 Cleanflight. If not, see .
- */
-
-#include
-#include
-#include
-
-#include "platform.h"
-
-#include "build/build_config.h"
-
-#include "drivers/system.h"
-#include "drivers/io.h"
-#include "drivers/exti.h"
-#include "hardware_revision.h"
-
-uint8_t hardwareRevision = AFF1_REV_1;
-uint8_t hardwareMotorType = MOTOR_UNKNOWN;
-
-static IO_t MotorDetectPin = IO_NONE;
-
-void detectHardwareRevision(void)
-{
- MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
- 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;
- }
-}
-
-void updateHardwareRevision(void)
-{
-}
-
-const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
-{
- return NULL;
-}
diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.h b/src/main/target/ALIENFLIGHTF1/hardware_revision.h
deleted file mode 100644
index 8fcb761fcc..0000000000
--- a/src/main/target/ALIENFLIGHTF1/hardware_revision.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it 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 is distributed in the hope that it 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 Cleanflight. If not, see .
- */
-#pragma once
-
-typedef enum awf1HardwareRevision_t {
- AFF1_UNKNOWN = 0,
- AFF1_REV_1, // MPU6050 (I2C)
-} awf1HardwareRevision_e;
-
-typedef enum awf4HardwareMotorType_t {
- MOTOR_UNKNOWN = 0,
- MOTOR_BRUSHED,
- MOTOR_BRUSHLESS
-} awf4HardwareMotorType_e;
-
-extern uint8_t hardwareRevision;
-extern uint8_t hardwareMotorType;
-
-void updateHardwareRevision(void);
-void detectHardwareRevision(void);
-
-struct extiConfig_s;
-const struct extiConfig_s *selectMPUIntExtiConfigByHardwareRevision(void);
diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h
index 5bafc911c8..02a730444e 100644
--- a/src/main/target/ALIENFLIGHTF1/target.h
+++ b/src/main/target/ALIENFLIGHTF1/target.h
@@ -20,8 +20,7 @@
#define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1.
#define TARGET_CONFIG
-#define USE_HARDWARE_REVISION_DETECTION
-#define MOTOR_PIN PA8
+#define BRUSHED_ESC_AUTODETECT
#define LED0 PB3
#define LED1 PB4
diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c
index 449bdaf6d2..c14ed47be5 100644
--- a/src/main/target/ALIENFLIGHTF3/config.c
+++ b/src/main/target/ALIENFLIGHTF3/config.c
@@ -42,6 +42,8 @@
#include "hardware_revision.h"
+#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz
+
// alternative defaults settings for AlienFlight targets
void targetConfiguration(master_t *config)
{
@@ -78,11 +80,8 @@ void targetConfiguration(master_t *config)
config->compassConfig.mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
- config->motorConfig.minthrottle = 1000;
- config->motorConfig.motorPwmRate = 32000;
- config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
+ config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
config->pid_process_denom = 2;
- config->motorConfig.useUnsyncedPwm = true;
}
config->profile[0].pidProfile.P8[ROLL] = 90;
diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c
index 0dd8ba2e4c..e3942a0c1b 100644
--- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c
+++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c
@@ -29,10 +29,8 @@
#include "hardware_revision.h"
uint8_t hardwareRevision = AFF3_UNKNOWN;
-uint8_t hardwareMotorType = MOTOR_UNKNOWN;
static IO_t HWDetectPin = IO_NONE;
-static IO_t MotorDetectPin = IO_NONE;
void detectHardwareRevision(void)
{
@@ -40,10 +38,6 @@ void detectHardwareRevision(void)
IOInit(HWDetectPin, OWNER_SYSTEM, 0);
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
- MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
- IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
- IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
-
delayMicroseconds(10); // allow configuration to settle
// Check hardware revision
@@ -52,13 +46,6 @@ void detectHardwareRevision(void)
} else {
hardwareRevision = AFF3_REV_2;
}
-
- // Check presence of brushed ESC's
- if (IORead(MotorDetectPin)) {
- hardwareMotorType = MOTOR_BRUSHLESS;
- } else {
- hardwareMotorType = MOTOR_BRUSHED;
- }
}
void updateHardwareRevision(void)
diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h
index 9b0618561f..67cdbc9315 100644
--- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h
+++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h
@@ -22,14 +22,7 @@ typedef enum awf3HardwareRevision_t {
AFF3_REV_2 // MPU6500 / MPU9250 (SPI)
} awf3HardwareRevision_e;
-typedef enum awf4HardwareMotorType_t {
- MOTOR_UNKNOWN = 0,
- MOTOR_BRUSHED,
- MOTOR_BRUSHLESS
-} awf4HardwareMotorType_e;
-
extern uint8_t hardwareRevision;
-extern uint8_t hardwareMotorType;
void updateHardwareRevision(void);
void detectHardwareRevision(void);
diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h
index 1627043e91..0f091017da 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/src/main/target/ALIENFLIGHTF3/target.h
@@ -25,7 +25,7 @@
#define USE_HARDWARE_REVISION_DETECTION
#define HW_PIN PB2
-#define MOTOR_PIN PB15
+#define BRUSHED_ESC_AUTODETECT
// LED's V1
#define LED0 PB4
diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c
index 363a9704d3..7a9b8bc54d 100644
--- a/src/main/target/ALIENFLIGHTF4/config.c
+++ b/src/main/target/ALIENFLIGHTF4/config.c
@@ -61,11 +61,8 @@ void targetConfiguration(master_t *config)
config->compassConfig.mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
- config->motorConfig.minthrottle = 1000;
config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
- config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
config->pid_process_denom = 1;
- config->motorConfig.useUnsyncedPwm = true;
}
if (hardwareRevision == AFF4_REV_1) {
diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.c b/src/main/target/ALIENFLIGHTF4/hardware_revision.c
index 918e897c88..37f5918280 100644
--- a/src/main/target/ALIENFLIGHTF4/hardware_revision.c
+++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.c
@@ -28,10 +28,8 @@
#include "hardware_revision.h"
uint8_t hardwareRevision = AFF4_UNKNOWN;
-uint8_t hardwareMotorType = MOTOR_UNKNOWN;
static IO_t HWDetectPin = IO_NONE;
-static IO_t MotorDetectPin = IO_NONE;
void detectHardwareRevision(void)
{
@@ -39,10 +37,6 @@ void detectHardwareRevision(void)
IOInit(HWDetectPin, OWNER_SYSTEM, 0);
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
- MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
- IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
- IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
-
delayMicroseconds(10); // allow configuration to settle
// Check hardware revision
@@ -51,13 +45,6 @@ void detectHardwareRevision(void)
} else {
hardwareRevision = AFF4_REV_2;
}
-
- // Check presence of brushed ESC's
- if (IORead(MotorDetectPin)) {
- hardwareMotorType = MOTOR_BRUSHLESS;
- } else {
- hardwareMotorType = MOTOR_BRUSHED;
- }
}
void updateHardwareRevision(void)
diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.h b/src/main/target/ALIENFLIGHTF4/hardware_revision.h
index e421b88960..410c8adcaa 100644
--- a/src/main/target/ALIENFLIGHTF4/hardware_revision.h
+++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.h
@@ -22,14 +22,7 @@ typedef enum awf4HardwareRevision_t {
AFF4_REV_2 // ICM-20602 (SPI), OpenSky RX (CC2510), Current Sensor (ACS712), SDCard Reader
} awf4HardwareRevision_e;
-typedef enum awf4HardwareMotorType_t {
- MOTOR_UNKNOWN = 0,
- MOTOR_BRUSHED,
- MOTOR_BRUSHLESS
-} awf4HardwareMotorType_e;
-
extern uint8_t hardwareRevision;
-extern uint8_t hardwareMotorType;
void updateHardwareRevision(void);
void detectHardwareRevision(void);
diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h
index 058759327e..b85db3d9a1 100644
--- a/src/main/target/ALIENFLIGHTF4/target.h
+++ b/src/main/target/ALIENFLIGHTF4/target.h
@@ -21,7 +21,7 @@
#define USE_HARDWARE_REVISION_DETECTION
#define HW_PIN PC13
-#define MOTOR_PIN PB8
+#define BRUSHED_ESC_AUTODETECT
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 560858c50a..efaeef6c99 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -172,10 +172,8 @@
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX)
#define BUTTONS
-#define BUTTON_A_PORT GPIOB
-#define BUTTON_A_PIN Pin_1
-#define BUTTON_B_PORT GPIOB
-#define BUTTON_B_PIN Pin_0
+#define BUTTON_A_PIN PB1
+#define BUTTON_B_PIN PB0
#define AVOID_UART3_FOR_PWM_PPM
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index fad8be61ce..40f8af637f 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -21,6 +21,8 @@
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
+#define BRUSHED_ESC_AUTODETECT
+
#define LED0 PB8
#define BEEPER PC15
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 451404d341..24bfcfc5e0 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -141,10 +141,8 @@
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define BUTTONS
-#define BUTTON_A_PORT GPIOB
-#define BUTTON_A_PIN Pin_1
-#define BUTTON_B_PORT GPIOB
-#define BUTTON_B_PIN Pin_0
+#define BUTTON_A_PIN PB1
+#define BUTTON_B_PIN PB0
#define SPEKTRUM_BIND
// USART3,