From 6b0bca4ef5fae9466e2a3ea9fb391ec0f22c0fc9 Mon Sep 17 00:00:00 2001
From: Thorsten Laux <thorsten.laux@gmx.net>
Date: Tue, 24 Sep 2019 13:34:51 +0200
Subject: [PATCH] Fix dshot bidir on H7

---
 src/main/drivers/dshot_dpwm.h           |  1 +
 src/main/drivers/pwm_output_dshot_hal.c | 20 ++++++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h
index fdccfdde2..f78bdba04 100644
--- a/src/main/drivers/dshot_dpwm.h
+++ b/src/main/drivers/dshot_dpwm.h
@@ -116,6 +116,7 @@ typedef struct motorDmaOutput_s {
 #ifdef STM32H7
     TIM_HandleTypeDef TimHandle;
     DMA_HandleTypeDef hdma_tim;
+    IO_t io;
 #endif
     uint8_t output;
     uint8_t index;
diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c
index 5103e2313..f43066928 100644
--- a/src/main/drivers/pwm_output_dshot_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal.c
@@ -115,7 +115,17 @@ static void pwmDshotSetDirectionInput(
     }
     LL_TIM_EnableARRPreload(timer); // Only update the period once all channels are done
     timer->ARR = 0xffffffff;
+
+#ifdef STM32H7
+    IOConfigGPIO(motor->io, GPIO_MODE_OUTPUT_PP);
+#endif
+
     LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
+
+#ifdef STM32H7
+    IOConfigGPIOAF(motor->io, motor->iocfg, timerHardware->alternateFunction);
+#endif
+
     motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
     xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
 }
@@ -240,11 +250,21 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
         output ^= TIMER_OUTPUT_INVERTED;
+#ifdef STM32H7
+        if (output & TIMER_OUTPUT_INVERTED) {
+            IOHi(motorIO);
+        } else {
+            IOLo(motorIO);
+        }
+#endif
     }
 #endif
     motor->timerHardware = timerHardware;
 
     motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode);
+#ifdef STM32H7
+    motor->io = motorIO;
+#endif
     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 
     if (configureTimer) {