From 02b297afa7f34db17963260b6dd58aaa14689d42 Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Fri, 13 Nov 2015 18:32:57 +0100 Subject: [PATCH] Runtime ESC count / gpio / pin detection Runtime ESC gpio/pin detection Runtime ESC count detection --- src/main/drivers/pwm_mapping.c | 15 +++- src/main/drivers/pwm_mapping.h | 22 ++++++ src/main/io/serial_1wire.c | 83 +++++++++++++---------- src/main/io/serial_1wire.h | 6 +- src/main/io/serial_cli.c | 19 +++--- src/main/io/serial_msp.c | 8 ++- src/main/target/CC3D/target.h | 3 +- src/main/target/MOTOLAB/target.h | 3 +- src/main/target/NAZE/target.h | 4 +- src/main/target/RMDO/target.h | 3 +- src/main/target/SPARKY/target.h | 3 +- src/main/target/SPRACINGF3/target.h | 3 +- src/main/target/STM32F3DISCOVERY/target.h | 4 +- 13 files changed, 120 insertions(+), 56 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1bbdf7f29..66987172e 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -456,6 +456,11 @@ static const uint16_t * const hardwareMaps[] = { airPPM, }; +static pwmOutputConfiguration_t pwmOutputConfiguration; + +pwmOutputConfiguration_t *pwmGetOutputConfiguration(void){ + return &pwmOutputConfiguration; +} pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) { int i = 0; @@ -463,7 +468,6 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) int channelIndex = 0; - static pwmOutputConfiguration_t pwmOutputConfiguration; memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); @@ -651,16 +655,25 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } else { pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount); } + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; } else if (isMotorBrushed(init->motorPwmRate)) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; } + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr; pwmOutputConfiguration.motorCount++; + pwmOutputConfiguration.outputCount++; } else if (type == MAP_TO_SERVO_OUTPUT) { #ifdef USE_SERVOS + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.servoCount; pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; pwmOutputConfiguration.servoCount++; + pwmOutputConfiguration.outputCount++; #endif } } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 47386f0e4..f605ff5b5 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -16,6 +16,8 @@ */ #pragma once +#include "gpio.h" +#include "timer.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -78,9 +80,27 @@ typedef struct drv_pwm_config_s { } drv_pwm_config_t; +typedef enum { + PWM_PF_NONE = 0, + PWM_PF_MOTOR = (1 << 0), + PWM_PF_SERVO = (1 << 1), + PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), + PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), + PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) +} pwmPortFlags_e; + + +typedef struct pwmPortConfiguration_s { + uint8_t index; + pwmPortFlags_e flags; + const timerHardware_t *timerHardware; +} pwmPortConfiguration_t; + typedef struct pwmOutputConfiguration_s { uint8_t servoCount; uint8_t motorCount; + uint8_t outputCount; + pwmPortConfiguration_t portConfigurations[MAX_PWM_OUTPUT_PORTS]; } pwmOutputConfiguration_t; // This indexes into the read-only hardware definition structure, timerHardware_t @@ -102,3 +122,5 @@ enum { PWM15, PWM16 }; + +pwmOutputConfiguration_t *pwmGetOutputConfiguration(void); diff --git a/src/main/io/serial_1wire.c b/src/main/io/serial_1wire.c index 175edcf03..2af08d2e0 100644 --- a/src/main/io/serial_1wire.c +++ b/src/main/io/serial_1wire.c @@ -19,15 +19,20 @@ */ #include - +#include +#include +#include #include "platform.h" #ifdef USE_SERIAL_1WIRE - #include "drivers/gpio.h" #include "drivers/light_led.h" +#include "drivers/system.h" #include "io/serial_1wire.h" +#include "drivers/pwm_mapping.h" +#include "flight/mixer.h" +/* const escHardware_t escHardware[ESC_COUNT] = { // Figure out esc clocks and pins, extrapolated from timer.c // Periphs could be pulled progmatically... but I'll leave that for another exercise @@ -79,6 +84,12 @@ const escHardware_t escHardware[ESC_COUNT] = { { GPIOA, 2 } #endif }; +*/ + +uint8_t escCount; // we detect the hardware dynamically + + +static escHardware_t escHardware[MAX_PWM_MOTORS]; static void gpio_set_mode(GPIO_TypeDef* gpio, uint16_t pin, GPIO_Mode mode) { gpio_config_t cfg; @@ -88,11 +99,36 @@ static void gpio_set_mode(GPIO_TypeDef* gpio, uint16_t pin, GPIO_Mode mode) { gpioInit(gpio, &cfg); } + +static uint32_t GetPinPos(uint32_t pin) { + uint32_t pinPos; + for (pinPos = 0; pinPos < 16; pinPos++) { + uint32_t pinMask = (0x1 << pinPos); + if (pin & pinMask) { + // pos found + return pinPos; + } + } + return 0; +} + + void usb1WireInitialize() { - for (volatile uint8_t i = 0; i < ESC_COUNT; i++) { - gpio_set_mode(escHardware[i].gpio, (1U << escHardware[i].pinpos), Mode_IPU); //GPIO_Mode_IPU - } + escCount = 0; + memset(&escHardware,0,sizeof(escHardware)); + pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); + for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { + if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { + if(motor[pwmOutputConfiguration->portConfigurations[i].index] >0 ) { + escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio; + escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin; + escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin); + gpio_set_mode(escHardware[escCount].gpio,escHardware[escCount].pin, Mode_IPU); //GPIO_Mode_IPU + escCount++; + } + } + } #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper off until reboot @@ -129,7 +165,7 @@ static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { // reference CRL or CRH, depending whether pin number is 0..7 or 8..15 if (mode == Mode_IPU) { *cr = in_cr_mask; - escHardware[escIndex].gpio->ODR |= (1U << escHardware[escIndex].pinpos); + escHardware[escIndex].gpio->ODR |= escHardware[escIndex].pin; } else { *cr = out_cr_mask; @@ -137,10 +173,10 @@ static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { } #endif -#define ESC_HI(escIndex) ((escHardware[escIndex].gpio->IDR & (1U << escHardware[escIndex].pinpos)) != (uint32_t)Bit_RESET) +#define ESC_HI(escIndex) ((escHardware[escIndex].gpio->IDR & escHardware[escIndex].pin) != (uint32_t)Bit_RESET) #define RX_HI ((S1W_RX_GPIO->IDR & S1W_RX_PIN) != (uint32_t)Bit_RESET) -#define ESC_SET_HI(escIndex) escHardware[escIndex].gpio->BSRR = (1U << escHardware[escIndex].pinpos) -#define ESC_SET_LO(escIndex) escHardware[escIndex].gpio->BRR = (1U << escHardware[escIndex].pinpos) +#define ESC_SET_HI(escIndex) escHardware[escIndex].gpio->BSRR = escHardware[escIndex].pin +#define ESC_SET_LO(escIndex) escHardware[escIndex].gpio->BRR = escHardware[escIndex].pin #define TX_SET_HIGH S1W_TX_GPIO->BSRR = S1W_TX_PIN #define TX_SET_LO S1W_TX_GPIO->BRR = S1W_TX_PIN @@ -154,39 +190,17 @@ static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { #define ESC_OUTPUT(escIndex) gpioSetOne(escIndex, Mode_Out_PP) #endif -#if defined(STM32F3DISCOVERY) -#define LED_PRGMR_RX GPIO_Pin_8 -#define LED_PRGMR_TX GPIO_Pin_10 -// Top Left LD4, PE8 (blue)-- from programmer (RX) -#define RX_LED_OFF GPIOE->BRR = LED_PRGMR_RX -#define RX_LED_ON GPIOE->BSRR = LED_PRGMR_RX -// Top Right LD5, PE10 (orange) -- to programmer (TX) -#define TX_LED_OFF GPIOE->BRR = LED_PRGMR_TX -#define TX_LED_ON GPIOE->BSRR = LED_PRGMR_TX -static void ledInitDebug(void) -{ - uint32_t pinmask = LED_PRGMR_RX|LED_PRGMR_TX; - GPIO_DeInit(GPIOE); - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE); - gpio_set_mode(GPIOE, pinmask, Mode_Out_PP); - GPIOE->BRR = pinmask; -} -#else + #define RX_LED_OFF LED0_OFF #define RX_LED_ON LED0_ON #define TX_LED_OFF LED1_OFF #define TX_LED_ON LED1_ON -#endif // This method translates 2 wires (a tx and rx line) to 1 wire, by letting the // RX line control when data should be read or written from the single line void usb1WirePassthrough(uint8_t escIndex) { -#ifdef STM32F3DISCOVERY - ledInitDebug(); -#endif - - //Disable all interrupts + // disable all interrupts __disable_irq(); // reset all the pins @@ -231,7 +245,7 @@ void usb1WirePassthrough(uint8_t escIndex) // At first Echo to the esc, which helps to charge input capacities at ESC ESC_SET_HI(escIndex); // Listen to the escIndex, input mode, pullup resistor is on - gpio_set_mode(escHardware[escIndex].gpio, (1U << escHardware[escIndex].pinpos), Mode_IPU); + gpio_set_mode(escHardware[escIndex].gpio, escHardware[escIndex].pin, Mode_IPU); TX_LED_OFF; if (ct==0) break; //we reached zero // Listen to the escIndex while there is no data from the programmer @@ -246,6 +260,7 @@ void usb1WirePassthrough(uint8_t escIndex) } } } + // we get here in case ct reached zero TX_SET_HIGH; RX_LED_OFF; diff --git a/src/main/io/serial_1wire.h b/src/main/io/serial_1wire.h index 2b18bd262..a5855d6ae 100644 --- a/src/main/io/serial_1wire.h +++ b/src/main/io/serial_1wire.h @@ -22,11 +22,15 @@ #ifdef USE_SERIAL_1WIRE +extern uint8_t escCount; + typedef struct { GPIO_TypeDef* gpio; - uint32_t pinpos; + uint16_t pinpos; + uint16_t pin; } escHardware_t; + void usb1WireInitialize(); void usb1WirePassthrough(uint8_t escIndex); #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 64824c6fd..a776cc22b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -149,7 +149,7 @@ static void cliFlashRead(char *cmdline); #endif #endif -#ifdef USE_SERIAL_1WIRE +#ifdef USE_SERIAL_1WIRE_CLI static void cliUSB1Wire(char *cmdline); #endif @@ -234,7 +234,7 @@ typedef struct { // should be sorted a..z for bsearch() const clicmd_t cmdTable[] = { -#ifdef USE_SERIAL_1WIRE +#ifdef USE_SERIAL_1WIRE_CLI CLI_COMMAND_DEF("1wire", "1-wire interface to escs", "", cliUSB1Wire), #endif CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange), @@ -2195,25 +2195,26 @@ static void cliStatus(char *cmdline) printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t)); } -#ifdef USE_SERIAL_1WIRE +#ifdef USE_SERIAL_1WIRE_CLI static void cliUSB1Wire(char *cmdline) { - int i; - if (isEmpty(cmdline)) { cliPrint("Please specify a ouput channel. e.g. `1wire 2` to connect to motor 2\r\n"); return; } else { + usb1WireInitialize(); // init ESC outputs and get escCount value + int i; i = atoi(cmdline); - if (i >= 0 && i <= ESC_COUNT) { + if (i >= 0 && i <= escCount) { printf("Switching to BlHeli mode on motor port %d\r\n", i); + // motor 1 => index 0 + usb1WirePassthrough(i-1); } else { - printf("Invalid motor port, valid range: 1 to %d\r\n", ESC_COUNT); + printf("Invalid motor port, valid range: 1 to %d\r\n", escCount); + // cliReboot(); } } - // motor 1 => index 0 - usb1WirePassthrough(i-1); } #endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 269706c88..222f2ac0c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1787,13 +1787,19 @@ static bool processInCommand(void) // 0xFF -> preinitialize the Passthrough // switch all motor lines HI usb1WireInitialize(); + // reply the count of ESC found + headSerialReply(1); + serialize8(escCount); + // and come back right afterwards // rem: App: Wait at least appx. 500 ms for BLHeli to jump into // bootloader mode before try to connect any ESC + + return true; } else { // Check for channel number 0..ESC_COUNT-1 - if (i < ESC_COUNT) { + if (i < escCount) { // because we do not come back after calling usb1WirePassthrough // proceed with a success reply first headSerialReply(0); diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index e0e9cc79f..e998d86d6 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -129,8 +129,7 @@ #define USE_CLI #define USE_SERIAL_1WIRE -// How many escs does this board support? -#define ESC_COUNT 6 +#define USE_SERIAL_1WIRE_CLI // FlexPort (pin 21/22, TX/RX respectively): // Note, FlexPort has 10k pullups on both TX and RX diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 40ca89852..b25491d2f 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -178,7 +178,8 @@ #define BIND_PIN Pin_4 #define USE_SERIAL_1WIRE -#define ESC_COUNT 8 +#define USE_SERIAL_1WIRE_CLI + #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_6 #define S1W_RX_GPIO GPIOB diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index ab07de17c..ead401f4c 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -187,8 +187,8 @@ #define BIND_PIN Pin_3 #define USE_SERIAL_1WIRE -// How many escs does this board support? -#define ESC_COUNT 6 +#define USE_SERIAL_1WIRE_CLI + // STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index da112034b..d877c6e05 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -164,7 +164,8 @@ #define BIND_PIN Pin_11 #define USE_SERIAL_1WIRE -#define ESC_COUNT 8 +#define USE_SERIAL_1WIRE_CLI + #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 #define S1W_RX_GPIO GPIOA diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index db652b8a1..471636a5f 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -162,7 +162,8 @@ #endif #define USE_SERIAL_1WIRE -#define ESC_COUNT 6 +#define USE_SERIAL_1WIRE_CLI + #define S1W_TX_GPIO GPIOB #define S1W_TX_PIN GPIO_Pin_6 #define S1W_RX_GPIO GPIOB diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index d6cca0026..840872495 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -172,7 +172,8 @@ #define BIND_PIN Pin_11 #define USE_SERIAL_1WIRE -#define ESC_COUNT 8 +#define USE_SERIAL_1WIRE_CLI + #define S1W_TX_GPIO GPIOA #define S1W_TX_PIN GPIO_Pin_9 #define S1W_RX_GPIO GPIOA diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 844949ed3..f1267b970 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -100,8 +100,8 @@ #define USE_CLI #define USE_SERIAL_1WIRE -// How many escs does this board support? -#define ESC_COUNT 6 +#define USE_SERIAL_1WIRE_CLI + // STM32F3DISCOVERY TX - PD5 connects to UART RX #define S1W_TX_GPIO GPIOD #define S1W_TX_PIN GPIO_Pin_5