fix(flysky gimbals): mask first 4 channels when FS gimbals are detect…

…ed (#3778)

* fix(flysky gimbals): mask first 4 channels when FS gimbals are detected.
* re-adjust center value to 4096 (2^12).This prevents values higher than necessary being displayed.

radio/src/targets/common/arm/stm32/CMakeLists.txt

@@ -44,6 +44,7 @@ set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
   ../common/arm/stm32/stm32_switch_driver.cpp
   ../common/arm/stm32/stm32_adc.cpp
   ../common/arm/stm32/stm32_timer.cpp
+  ../common/arm/stm32/stm32_dma.cpp
   ../common/arm/stm32/stm32_gpio_driver.cpp
   ../common/arm/stm32/mixer_scheduler_driver.cpp
   )
@@ -69,8 +70,8 @@ if(AUX_SERIAL OR AUX2_SERIAL)
 endif()
 
 if(FLYSKY_GIMBAL)
-  set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
-    ../common/arm/stm32/flysky_gimbal_driver.cpp
+  set(FIRMWARE_SRC ${FIRMWARE_SRC}
+    targets/common/arm/stm32/flysky_gimbal_driver.cpp
   )
   add_definitions(-DFLYSKY_GIMBAL)
 endif()

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,6 +170,8 @@ 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;
     }
   }

radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h

@@ -23,7 +23,13 @@
 #define FLYSKY_HALL_BAUDRATE            ( 921600 )
 #define FLYSKY_HALL_CHANNEL_COUNT       ( 4 )
 
-#define FLYSKY_OFFSET_VALUE             ( 16384 )
+// This value has been chosen arbitrarily to allow
+// for 13-bit precision.
+//
+// Note: Flysky gimbals provide signed 16-bit values, whereby
+//       ADC sampling uses unsigned 16-bit values.
+//
+#define FLYSKY_OFFSET_VALUE             ( 1 << 12 )
 
 #define FLYSKY_HALL_PROTOLO_HEAD        0x55
 #define FLYSKY_HALL_RESP_TYPE_VALUES    0x0c

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

radio/src/targets/common/arm/stm32/stm32_adc.h

@@ -65,3 +65,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);

radio/src/targets/common/arm/stm32/stm32_dma.cpp

@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) EdgeTx
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "stm32_dma.h"
+
+void stm32_dma_enable_clock(DMA_TypeDef* DMAx)
+{
+  if (DMAx == DMA1) {
+    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA1);
+  } else if (DMAx == DMA2) {
+    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_DMA2);
+  }
+}

radio/src/targets/common/arm/stm32/stm32_dma.h

@@ -47,3 +47,5 @@ inline static bool stm32_dma_check_tc_flag(DMA_TypeDef* DMAx, uint32_t DMA_Strea
 
   return true;
 }
+
+void stm32_dma_enable_clock(DMA_TypeDef* DMAx);

radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp

@@ -20,6 +20,7 @@
  */
 
 #include "stm32_usart_driver.h"
+#include "stm32_gpio_driver.h"
 #include "stm32_dma.h"
 
 #include <string.h>
@@ -178,6 +179,7 @@ void stm32_usart_init_rx_dma(const stm32_usart_t* usart, const void* buffer, uin
     NVIC_DisableIRQ(usart->IRQn);
   }
 
+  stm32_dma_enable_clock(usart->rxDMA);
   LL_DMA_DeInit(usart->rxDMA, usart->rxDMA_Stream);
 
   LL_DMA_InitTypeDef dmaInit;
@@ -224,7 +226,6 @@ void stm32_usart_init(const stm32_usart_t* usart, const etx_serial_init* params)
   enable_usart_clock(usart->USARTx);
   LL_USART_DeInit(usart->USARTx);
 
-  // TODO: enable GPIO clock
   LL_GPIO_InitTypeDef pinInit;
   LL_GPIO_StructInit(&pinInit);
 
@@ -234,6 +235,8 @@ void stm32_usart_init(const stm32_usart_t* usart, const etx_serial_init* params)
   pinInit.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
   pinInit.Pull = LL_GPIO_PULL_UP;
   pinInit.Alternate = _get_usart_af(usart->USARTx);
+
+  stm32_gpio_enable_clock(usart->GPIOx);
   LL_GPIO_Init(usart->GPIOx, &pinInit);
 
   LL_USART_InitTypeDef usartInit;
@@ -344,6 +347,7 @@ void stm32_usart_send_buffer(const stm32_usart_t* usart, const uint8_t * data, u
   _half_duplex_output(usart);
 
   if (usart->txDMA) {
+    stm32_dma_enable_clock(usart->txDMA);
     LL_DMA_DeInit(usart->txDMA, usart->txDMA_Stream);
 
     LL_DMA_InitTypeDef dmaInit;

radio/src/targets/horus/board.cpp

@@ -24,8 +24,6 @@
 #include "hal/switch_driver.h"
 #include "hal/rotary_encoder.h"
 
-#include "sticks_pwm_driver.h"
-
 #include "board.h"
 #include "boards/generic_stm32/module_ports.h"
 #include "boards/generic_stm32/intmodule_heartbeat.h"
@@ -40,7 +38,7 @@
 
 #include <string.h>
 
-#if defined(RADIO_FAMILY_T16) || defined(PCBNV14)
+#if defined(FLYSKY_GIMBAL)
   #include "flysky_gimbal_driver.h"
 #endif
 
@@ -111,9 +109,6 @@ void boardInit()
                          INTERRUPT_xMS_RCC_APB1Periph |
                          TIMER_2MHz_RCC_APB1Periph |
                          AUDIO_RCC_APB1Periph |
-#if defined(RADIO_FAMILY_T16)
-                         FLYSKY_HALL_RCC_APB1Periph |
-#endif
                          TELEMETRY_RCC_APB1Periph |
                          AUDIO_RCC_APB1Periph |
                          MIXER_SCHEDULER_TIMER_RCC_APB1Periph |
@@ -170,7 +165,7 @@ void boardInit()
   sticksPwmDetect();
 #endif
 
-#if defined(RADIO_FAMILY_T16)
+#if defined(FLYSKY_GIMBAL)
   flysky_gimbal_init();
 #endif
 

radio/src/targets/horus/hal.h

@@ -816,10 +816,6 @@
   #define FLYSKY_HALL_SERIAL_TX_GPIO_PIN           LL_GPIO_PIN_0  // PA.00
   #define FLYSKY_HALL_SERIAL_RX_GPIO_PIN           LL_GPIO_PIN_1  // PA.01
   #define FLYSKY_HALL_SERIAL_GPIO_AF               LL_GPIO_AF_8
-
-  #define FLYSKY_HALL_RCC_AHB1Periph               RCC_AHB1Periph_DMA1
-  #define FLYSKY_HALL_RCC_APB1Periph               RCC_APB1Periph_UART4
-
   #define FLYSKY_HALL_SERIAL_USART_IRQHandler      UART4_IRQHandler
   #define FLYSKY_HALL_SERIAL_USART_IRQn            UART4_IRQn
   #define FLYSKY_HALL_SERIAL_DMA                   DMA1

radio/src/targets/nv14/board.cpp

@@ -122,7 +122,6 @@ void delay_self(int count)
                                AUDIO_RCC_AHB1Periph |\
                                HAPTIC_RCC_AHB1Periph |\
                                INTMODULE_RCC_AHB1Periph |\
-                               FLYSKY_HALL_RCC_AHB1Periph |\
                                EXTMODULE_RCC_AHB1Periph\
                               )
 #define RCC_AHB3PeriphMinimum (SDRAM_RCC_AHB3Periph)
@@ -133,7 +132,6 @@ void delay_self(int count)
                               )
 
 #define RCC_APB1PeriphOther   (TELEMETRY_RCC_APB1Periph |\
-                               FLYSKY_HALL_RCC_APB1Periph |\
                                MIXER_SCHEDULER_TIMER_RCC_APB1Periph \
                               )
 #define RCC_APB2PeriphMinimum (LCD_RCC_APB2Periph)

radio/src/targets/nv14/hal.h

@@ -399,10 +399,6 @@
 #define FLYSKY_HALL_SERIAL_TX_GPIO_PIN           LL_GPIO_PIN_0  // PA.00
 #define FLYSKY_HALL_SERIAL_RX_GPIO_PIN           LL_GPIO_PIN_1  // PA.01
 #define FLYSKY_HALL_SERIAL_GPIO_AF               LL_GPIO_AF_8
-
-#define FLYSKY_HALL_RCC_AHB1Periph               RCC_AHB1Periph_DMA1
-#define FLYSKY_HALL_RCC_APB1Periph               RCC_APB1Periph_UART4
-
 #define FLYSKY_HALL_SERIAL_USART_IRQHandler      UART4_IRQHandler
 #define FLYSKY_HALL_SERIAL_USART_IRQn            UART4_IRQn
 #define FLYSKY_HALL_SERIAL_DMA                   DMA1

radio/src/targets/taranis/board.cpp

@@ -31,10 +31,13 @@
 #include "debug.h"
 #include "rtc.h"
 
-#include "../common/arm/stm32/timers_driver.h"
-
+#include "timers_driver.h"
 #include "dataconstants.h"
 
+#if defined(FLYSKY_GIMBAL)
+  #include "flysky_gimbal_driver.h"
+#endif
+
 #if !defined(BOOT)
   #include "opentx.h"
   #if defined(PXX1)
@@ -206,19 +209,23 @@ void boardInit()
 #endif
 
   delaysInit();
+  __enable_irq();
 
 #if defined(PWM_STICKS)
   sticksPwmDetect();
 #endif
 
+#if defined(FLYSKY_GIMBAL)
+  flysky_gimbal_init();
+#endif
+
   if (!adcInit(&_adc_driver))
     TRACE("adcInit failed");
 
   lcdInit(); // delaysInit() must be called before
   audioInit();
   init2MhzTimer();
   init1msTimer();
-  __enable_irq();
   usbInit();
 
 #if defined(DEBUG)