From 09aada75a4c46a6d68cdfa58ddd2a7f210809ba3 Mon Sep 17 00:00:00 2001
From: raphaelcoeffic <raphael.coeffic@frafos.com>
Date: Sat, 8 Jul 2023 10:09:10 +0200
Subject: [PATCH] fix(flysky gimbals): mask first 4 channels when FS gimbals
 are detected

---
 .../common/arm/stm32/flysky_gimbal_driver.cpp        |  6 ++++--
 radio/src/targets/common/arm/stm32/stm32_adc.cpp     | 12 ++++++++++++
 radio/src/targets/common/arm/stm32/stm32_adc.h       |  2 ++
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp
index ceda29fbc53..a6aed8318eb 100644
--- a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp
@@ -21,6 +21,8 @@
 
 #include "flysky_gimbal_driver.h"
 #include "stm32_serial_driver.h"
+#include "stm32_adc.h"
+
 #include "delays_driver.h"
 #include "hal/adc_driver.h"
 
@@ -168,10 +170,10 @@ bool flysky_gimbal_init()
   for (uint8_t i = 0; i < 70; i++) {
     delay_ms(1);
     if (_fs_gimbal_detected) {
+      // Mask the first 4 inputs (sticks)
+      stm32_hal_mask_inputs(0xF);
       return true;
     }
   }
 
-  flysky_gimbal_deinit();
-  return false;
 }
diff --git a/radio/src/targets/common/arm/stm32/stm32_adc.cpp b/radio/src/targets/common/arm/stm32/stm32_adc.cpp
index 2b9dd1794a2..95c6f599829 100644
--- a/radio/src/targets/common/arm/stm32/stm32_adc.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_adc.cpp
@@ -33,6 +33,7 @@
 
 // Max 32 inputs supported
 static uint32_t _adc_input_mask;
+static uint32_t _adc_input_inhibt_mask = 0;
 static volatile uint32_t _adc_inhibit_mask;
 
 // DMA buffers
@@ -52,6 +53,11 @@ static uint8_t _adc_run;
 static uint8_t _adc_oversampling_disabled;
 static uint16_t _adc_oversampling[MAX_ADC_INPUTS];
 
+void stm32_hal_mask_inputs(uint32_t inputs)
+{
+  _adc_input_inhibt_mask |= inputs;
+}
+
 // STM32 uses a 25K+25K voltage divider bridge to measure the battery voltage
 // Measuring VBAT puts considerable drain (22 µA) on the battery instead of
 // normal drain (~10 nA)
@@ -212,6 +218,12 @@ static uint8_t adc_init_channels(const stm32_adc_t* adc,
     uint8_t input_idx = *chan;
     const stm32_adc_input_t* input = &inputs[input_idx];
 
+    if (_adc_input_inhibt_mask & (1 << input_idx)) {
+      // skip input
+      nconv--; chan++;
+      continue;      
+    }
+    
     // internal channel don't have a GPIO + pin defined
     uint32_t mask = (1 << (ADC_CHANNEL_ID_MASK & input->ADC_Channel));
     if (!__LL_ADC_IS_CHANNEL_INTERNAL(input->ADC_Channel)) {
diff --git a/radio/src/targets/common/arm/stm32/stm32_adc.h b/radio/src/targets/common/arm/stm32/stm32_adc.h
index 1e53f708bc7..f586e88270b 100644
--- a/radio/src/targets/common/arm/stm32/stm32_adc.h
+++ b/radio/src/targets/common/arm/stm32/stm32_adc.h
@@ -66,3 +66,5 @@ void stm32_hal_adc_disable_oversampling();
 
 void stm32_hal_adc_dma_isr(const stm32_adc_t* adc);
 void stm32_hal_adc_isr(const stm32_adc_t* adc);
+
+void stm32_hal_mask_inputs(uint32_t inputs);