From 590b01a77ea3efd369309ffc536339fc26cc6134 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 7 Dec 2016 22:33:47 +0100 Subject: [PATCH 1/2] Generalize brushed ESC auto detection Used for all AlienFlight and SPRACINGF3EVO targets --- src/main/drivers/pwm_output.c | 26 ++++++++ src/main/drivers/pwm_output.h | 12 ++++ src/main/fc/config.c | 16 ++++- src/main/main.c | 4 ++ src/main/target/ALIENFLIGHTF1/config.c | 7 +-- .../target/ALIENFLIGHTF1/hardware_revision.c | 59 ------------------- .../target/ALIENFLIGHTF1/hardware_revision.h | 37 ------------ src/main/target/ALIENFLIGHTF1/target.h | 3 +- src/main/target/ALIENFLIGHTF3/config.c | 7 +-- .../target/ALIENFLIGHTF3/hardware_revision.c | 13 ---- .../target/ALIENFLIGHTF3/hardware_revision.h | 7 --- src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 3 - .../target/ALIENFLIGHTF4/hardware_revision.c | 13 ---- .../target/ALIENFLIGHTF4/hardware_revision.h | 7 --- src/main/target/ALIENFLIGHTF4/target.h | 2 +- src/main/target/SPRACINGF3EVO/target.h | 2 + 17 files changed, 65 insertions(+), 155 deletions(-) delete mode 100644 src/main/target/ALIENFLIGHTF1/hardware_revision.c delete mode 100644 src/main/target/ALIENFLIGHTF1/hardware_revision.h 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 fa1cc67cdc..dd94e46962 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 cf1060a547..5df667d934 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -158,6 +158,10 @@ void init(void) detectHardwareRevision(); #endif +#ifdef BRUSHED_ESC_AUTODETECT + detectBrushedESC(); +#endif + initEEPROM(); ensureEEPROMContainsValidData(); 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 516b64a78a..f99ee32ee4 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) { @@ -50,11 +52,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 63a3a9bb08..f97ba9ac34 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -24,7 +24,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/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 88e7d09f42..4cc76d1543 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 From fcb3ebf066971027e12eac6615fbda5ba4cb87a6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 9 Dec 2016 19:56:28 +0100 Subject: [PATCH 2/2] Change button code For SPRACINGF3MINI and OMNIBUS targets to new IO --- src/main/main.c | 23 ++++++++--------------- src/main/target/OMNIBUS/target.h | 6 ++---- src/main/target/SPRACINGF3MINI/target.h | 6 ++---- 3 files changed, 12 insertions(+), 23 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 5df667d934..3c84bed7da 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" @@ -188,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/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 166a570352..e88afed520 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -171,10 +171,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/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 79c1508ac3..605e57a083 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -147,10 +147,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,