From 4ea77cd17232f5649096b3ff51f4832a93f8be46 Mon Sep 17 00:00:00 2001
From: Risto <rotorman@users.noreply.github.com>
Date: Wed, 15 Feb 2023 07:22:58 +0100
Subject: [PATCH] fix: Enable AUX1 power on TX16S with IMU_LSM6DS33 (#3156)

* fix: enable AUX1 power on TX16S with IMU_LSM6DS33

* Add some delay when powering I2C bus

---------

Co-authored-by: raphaelcoeffic <raphael.coeffic@frafos.com>
---
 .../common/arm/stm32/stm32_i2c_driver.cpp     | 38 +++++++++++++++++++
 radio/src/targets/horus/hal.h                 |  4 ++
 2 files changed, 42 insertions(+)

diff --git a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
index 03f932559..7edb6ad14 100644
--- a/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_i2c_driver.cpp
@@ -33,6 +33,7 @@ struct _i2c_defs {
   GPIO_TypeDef*      SDA_GPIOx;
   uint32_t           SDA_Pin;
   uint32_t           Alternate;
+  void (*set_pwr)(bool enable);
 };
 
 #if defined(I2C_B1)
@@ -46,6 +47,7 @@ static const _i2c_defs pins_hi2c1 = {
   .SDA_GPIOx = I2C_B1_GPIO,
   .SDA_Pin = I2C_B1_SDA_GPIO_PIN,
   .Alternate = I2C_B1_GPIO_AF,
+  .set_pwr = nullptr,
 };
 #endif
 
@@ -54,6 +56,26 @@ static I2C_HandleTypeDef hi2c2 = {
   .Instance = I2C_B2,
   .Init = { 0, 0, 0, 0, 0, 0, 0, 0 },
 };
+
+#if defined(I2C_B2_PWR_GPIO)
+void _i2c_b2_pwr(bool enable)
+{
+  LL_GPIO_InitTypeDef pinInit;
+  LL_GPIO_StructInit(&pinInit);
+  
+  pinInit.Pin = I2C_B2_PWR_GPIO_PIN;
+  pinInit.Mode = LL_GPIO_MODE_OUTPUT;
+  pinInit.Pull = LL_GPIO_PULL_UP;
+  LL_GPIO_Init(I2C_B2_PWR_GPIO, &pinInit);
+
+  if (enable) {
+    LL_GPIO_SetOutputPin(I2C_B2_PWR_GPIO, I2C_B2_PWR_GPIO_PIN);
+  } else {
+    LL_GPIO_ResetOutputPin(I2C_B2_PWR_GPIO, I2C_B2_PWR_GPIO_PIN);
+  }
+}
+#endif
+
 static const _i2c_defs pins_hi2c2 = {
 #if defined(I2C_B2_GPIO)
   .SCL_GPIOx = I2C_B2_GPIO,
@@ -68,6 +90,11 @@ static const _i2c_defs pins_hi2c2 = {
 #endif
   .SDA_Pin = I2C_B2_SDA_GPIO_PIN,
   .Alternate = I2C_B2_GPIO_AF,
+#if defined(I2C_B2_PWR_GPIO)
+  .set_pwr = _i2c_b2_pwr,
+#else
+  .set_pwr = nullptr,
+#endif
 };
 #endif
 
@@ -269,6 +296,13 @@ static int i2c_pins_init(const _i2c_defs* def)
   pinInit.Pin = def->SDA_Pin;
   LL_GPIO_Init(def->SDA_GPIOx, &pinInit);
 
+  if (def->set_pwr) {
+    def->set_pwr(true);
+    // Add some delay to leave enought time
+    // for devices to boot before querying them
+    HAL_Delay(20);
+  }
+  
   return 0;
 }
 
@@ -290,6 +324,10 @@ static int i2c_pins_deinit(const _i2c_defs* def)
   pinInit.Pin = def->SDA_Pin;
   LL_GPIO_Init(def->SDA_GPIOx, &pinInit);
 
+  if (def->set_pwr) {
+    def->set_pwr(false);
+  }
+
   return 0;
 }
 
diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h
index 95be0e9a5..34b08493b 100644
--- a/radio/src/targets/horus/hal.h
+++ b/radio/src/targets/horus/hal.h
@@ -752,6 +752,10 @@
   #define I2C_B2_SCL_GPIO_PIN             LL_GPIO_PIN_10  // PB.10
   #define I2C_B2_SDA_GPIO_PIN             LL_GPIO_PIN_11  // PB.11
   #define I2C_B2_GPIO_AF                  LL_GPIO_AF_4    // I2C2
+ #if defined(RADIO_TX16S)
+   #define I2C_B2_PWR_GPIO                GPIOA
+   #define I2C_B2_PWR_GPIO_PIN            LL_GPIO_PIN_15  // PA.15
+ #endif
 #endif
 
 #if defined(IMU)