Merge pull request #37 from 4712betaflight/1wire_pass_through_optimize

Runtime ESC count / gpio / pin detection

src/main/drivers/pwm_mapping.c

@@ -456,14 +456,18 @@ 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;
     const uint16_t *setup;
 
     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
         }
     }

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);

src/main/io/serial_1wire.c

@@ -19,15 +19,20 @@
  */
 
 #include <stdbool.h>
-
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
 #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,18 +165,18 @@ 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;
   }
 }
 #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;

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

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", "<esc index>", cliUSB1Wire),
 #endif
     CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
@@ -2196,25 +2196,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
 

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);

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

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

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

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

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

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