From cbd824eb10ba3107b5764fef7d183af9eb363061 Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Sun, 1 Oct 2017 09:07:07 +0200 Subject: [PATCH] [X10S] PWM sticks support - with autodetection (#5251) [X10S] PWM sticks support - with autodetection --- radio/src/opentx.cpp | 4 + .../targets/common/arm/stm32/adc_driver.cpp | 41 +++- radio/src/targets/horus/CMakeLists.txt | 1 + radio/src/targets/horus/board.cpp | 1 + radio/src/targets/horus/board.h | 10 + radio/src/targets/horus/hal.h | 10 +- radio/src/targets/horus/pwm_driver.cpp | 196 ++++++++++++++++++ 7 files changed, 253 insertions(+), 10 deletions(-) create mode 100644 radio/src/targets/horus/pwm_driver.cpp diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index ac5b61ff9..f0ffa4073 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -2593,6 +2593,10 @@ void opentxInit(OPENTX_INIT_ARGS) backlightOn(); } +#if NUM_PWMANALOGS > 0 + pwmCheck(); +#endif + if (!unexpectedShutdown) { opentxStart(); } diff --git a/radio/src/targets/common/arm/stm32/adc_driver.cpp b/radio/src/targets/common/arm/stm32/adc_driver.cpp index 7633231ff..f28bca44d 100644 --- a/radio/src/targets/common/arm/stm32/adc_driver.cpp +++ b/radio/src/targets/common/arm/stm32/adc_driver.cpp @@ -44,11 +44,16 @@ const int8_t ana_direction[NUM_ANALOGS] = {1,-1,1,-1, -1,1,0, -1,1, 1}; #endif -#if defined(PCBX9E) - #define NUM_ANALOGS_ADC 10 - #define NUM_ANALOGS_ADC_EXT (NUM_ANALOGS - 10) +#if NUM_PWMANALOGS > 0 + #define FIRST_ANALOG_ADC (ANALOGS_PWM_ENABLED() ? NUM_PWMANALOGS : 0) + #define NUM_ANALOGS_ADC (ANALOGS_PWM_ENABLED() ? (NUM_ANALOGS - NUM_PWMANALOGS) : NUM_ANALOGS) +#elif defined(PCBX9E) + #define FIRST_ANALOG_ADC 0 + #define NUM_ANALOGS_ADC 10 + #define NUM_ANALOGS_ADC_EXT (NUM_ANALOGS - 10) #else - #define NUM_ANALOGS_ADC NUM_ANALOGS + #define FIRST_ANALOG_ADC 0 + #define NUM_ANALOGS_ADC NUM_ANALOGS #endif uint16_t adcValues[NUM_ANALOGS] __DMA; @@ -86,8 +91,14 @@ void adcInit() ADC_MAIN->SQR1 = (NUM_ANALOGS_ADC-1) << 20; // bits 23:20 = number of conversions #if defined(PCBX10) - ADC_MAIN->SQR2 = (ADC_CHANNEL_POT3<<0) + (ADC_CHANNEL_SLIDER1<<5) + (ADC_CHANNEL_SLIDER2<<10) + (ADC_CHANNEL_BATT<<15) + (ADC_CHANNEL_EXT1<<20) + (ADC_CHANNEL_EXT2<<25); // conversions 7 and more - ADC_MAIN->SQR3 = (ADC_CHANNEL_STICK_LH<<0) + (ADC_CHANNEL_STICK_LV<<5) + (ADC_CHANNEL_STICK_RV<<10) + (ADC_CHANNEL_STICK_RH<<15) + (ADC_CHANNEL_POT1<<20) + (ADC_CHANNEL_POT2<<25); // conversions 1 to 6 + if (ANALOGS_PWM_ENABLED()) { + ADC_MAIN->SQR2 = (ADC_CHANNEL_EXT1<<0) + (ADC_CHANNEL_EXT2<<5); // conversions 7 and more + ADC_MAIN->SQR3 = (ADC_CHANNEL_POT1<<0) + (ADC_CHANNEL_POT2<<5) + (ADC_CHANNEL_POT3<<10) + (ADC_CHANNEL_SLIDER1<<15) + (ADC_CHANNEL_SLIDER2<<20) + (ADC_CHANNEL_BATT<<25); // conversions 1 to 6 + } + else { + ADC_MAIN->SQR2 = (ADC_CHANNEL_POT3<<0) + (ADC_CHANNEL_SLIDER1<<5) + (ADC_CHANNEL_SLIDER2<<10) + (ADC_CHANNEL_BATT<<15) + (ADC_CHANNEL_EXT1<<20) + (ADC_CHANNEL_EXT2<<25); // conversions 7 and more + ADC_MAIN->SQR3 = (ADC_CHANNEL_STICK_LH<<0) + (ADC_CHANNEL_STICK_LV<<5) + (ADC_CHANNEL_STICK_RV<<10) + (ADC_CHANNEL_STICK_RH<<15) + (ADC_CHANNEL_POT1<<20) + (ADC_CHANNEL_POT2<<25); // conversions 1 to 6 + } #elif defined(PCBX9E) ADC_MAIN->SQR2 = (ADC_CHANNEL_POT4<<0) + (ADC_CHANNEL_SLIDER3<<5) + (ADC_CHANNEL_SLIDER4<<10) + (ADC_CHANNEL_BATT<<15); // conversions 7 and more ADC_MAIN->SQR3 = (ADC_CHANNEL_STICK_LH<<0) + (ADC_CHANNEL_STICK_LV<<5) + (ADC_CHANNEL_STICK_RV<<10) + (ADC_CHANNEL_STICK_RH<<15) + (ADC_CHANNEL_POT2<<20) + (ADC_CHANNEL_POT3<<25); // conversions 1 to 6 @@ -106,7 +117,7 @@ void adcInit() ADC_DMA_Stream->CR = DMA_SxCR_PL | ADC_DMA_SxCR_CHSEL | DMA_SxCR_MSIZE_0 | DMA_SxCR_PSIZE_0 | DMA_SxCR_MINC; ADC_DMA_Stream->PAR = CONVERT_PTR_UINT(&ADC_MAIN->DR); - ADC_DMA_Stream->M0AR = CONVERT_PTR_UINT(adcValues); + ADC_DMA_Stream->M0AR = CONVERT_PTR_UINT(&adcValues[FIRST_ANALOG_ADC]); ADC_DMA_Stream->NDTR = NUM_ANALOGS_ADC; ADC_DMA_Stream->FCR = DMA_SxFCR_DMDIS | DMA_SxFCR_FTH_0; @@ -125,6 +136,12 @@ void adcInit() ADC_EXT_DMA_Stream->NDTR = NUM_ANALOGS_ADC_EXT; ADC_EXT_DMA_Stream->FCR = DMA_SxFCR_DMDIS | DMA_SxFCR_FTH_0; #endif + +#if NUM_PWMANALOGS > 0 + if (ANALOGS_PWM_ENABLED()) { + pwmInit(); + } +#endif } void adcSingleRead() @@ -167,7 +184,7 @@ void adcRead() for (int i=0; i<4; i++) { adcSingleRead(); - for (uint8_t x=0; x> 2; } + +#if NUM_PWMANALOGS > 0 + if (ANALOGS_PWM_ENABLED()) { + pwmRead(adcValues); + } +#endif } // TODO diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 11b61f738..66cdf68ca 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -29,6 +29,7 @@ if (PCB STREQUAL X10) ${TARGET_SRC} ../common/arm/stm32/audio_dac_driver.cpp ../common/arm/stm32/adc_driver.cpp + pwm_driver.cpp ) set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} x10_bitmaps) set(LUA_EXPORT lua_export_x10) diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index f05b30be7..c91f4bbea 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -129,6 +129,7 @@ void boardInit() ENABLE); RCC_APB1PeriphClockCmd(INTERRUPT_1MS_RCC_APB1Periph | + ADC_RCC_APB1Periph | TIMER_2MHz_RCC_APB1Periph | AUDIO_RCC_APB1Periph | SERIAL_RCC_APB1Periph | diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 99d629c9d..f95652005 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -328,8 +328,10 @@ void watchdogInit(unsigned int duration); #define NUM_XPOTS NUM_POTS #if defined(PCBX10) #define NUM_SLIDERS 2 + #define NUM_PWMANALOGS 4 #else #define NUM_SLIDERS 4 + #define NUM_PWMANALOGS 0 #endif enum Analogs { STICK1, @@ -389,6 +391,14 @@ void adcInit(void); void adcRead(void); uint16_t getAnalogValue(uint8_t index); uint16_t getBatteryVoltage(); // returns current battery voltage in 10mV steps +#if NUM_PWMANALOGS > 0 +extern uint8_t analogs_pwm_disabled; +#define ANALOGS_PWM_ENABLED() (analogs_pwm_disabled == false) +void pwmInit(void); +void pwmRead(uint16_t * values); +void pwmCheck(); +extern volatile uint32_t pwm_interrupt_count; +#endif #if defined(__cplusplus) && !defined(SIMU) extern "C" { diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index fde29f87f..164d756b9 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -177,6 +177,7 @@ // ADC #if defined(PCBX12S) #define ADC_RCC_AHB1Periph (RCC_AHB1Periph_GPIOE | RCC_AHB1Periph_GPIOF | RCC_AHB1Periph_DMA2) + #define ADC_RCC_APB1Periph 0 #define ADC_RCC_APB2Periph (RCC_APB2Periph_SPI4 | RCC_APB2Periph_ADC3) #define ADC_SPI SPI4 #define ADC_GPIO_AF GPIO_AF_SPI4 @@ -198,6 +199,7 @@ #define ADC_SAMPTIME 3 #elif defined(PCBX10) #define ADC_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOF | RCC_AHB1Periph_DMA2) + #define ADC_RCC_APB1Periph (RCC_APB1Periph_TIM5) #define ADC_RCC_APB2Periph (RCC_APB2Periph_ADC3) #define ADC_GPIO_PIN_STICK_LH GPIO_Pin_0 // PA.00 #define ADC_GPIO_PIN_STICK_LV GPIO_Pin_1 // PA.01 @@ -211,7 +213,13 @@ #define ADC_GPIO_PIN_BATT GPIO_Pin_7 // PF.07 #define ADC_GPIO_PIN_EXT1 GPIO_Pin_8 // PF.08 #define ADC_GPIO_PIN_EXT2 GPIO_Pin_9 // PF.09 - #define ADC_GPIOA_PINS (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3) + #define PWM_TIMER TIM5 + #define PWM_GPIO GPIOA + #define PWM_GPIO_AF GPIO_AF_TIM5 + #define PWM_IRQHandler TIM5_IRQHandler + #define PWM_IRQn TIM5_IRQn + #define PWM_GPIOA_PINS (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3) + #define ADC_GPIOA_PINS (ANALOGS_PWM_ENABLED() ? 0 : (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3)) #define ADC_GPIOC_PINS (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3) #define ADC_GPIOF_PINS (GPIO_Pin_6 | GPIO_Pin_7) // | GPIO_Pin_8 | GPIO_Pin_9) #define ADC_CHANNEL_STICK_LH ADC_Channel_0 // ADC3_IN0 diff --git a/radio/src/targets/horus/pwm_driver.cpp b/radio/src/targets/horus/pwm_driver.cpp new file mode 100644 index 000000000..7f27cffb5 --- /dev/null +++ b/radio/src/targets/horus/pwm_driver.cpp @@ -0,0 +1,196 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * 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 "opentx.h" + +#define TIMESAMPLE_COUNT 6 + +uint8_t analogs_pwm_disabled = 0; +volatile uint32_t pwm_interrupt_count; +volatile uint8_t timer_capture_states[4]; +volatile uint32_t timer_capture_rising_time[4]; +volatile uint32_t timer_capture_values[4][TIMESAMPLE_COUNT]; +volatile uint8_t timer_capture_indexes[4]; + +void pwmInit() +{ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = PWM_GPIOA_PINS; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(PWM_GPIO, &GPIO_InitStructure); + + GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource0, PWM_GPIO_AF); + GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource1, PWM_GPIO_AF); + GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource2, PWM_GPIO_AF); + GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource3, PWM_GPIO_AF); + + PWM_TIMER->CR1 &= ~TIM_CR1_CEN; // Stop timer + PWM_TIMER->PSC = 80; + PWM_TIMER->ARR = 0xffff; + PWM_TIMER->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0; + PWM_TIMER->CCMR2 = TIM_CCMR2_CC3S_0 | TIM_CCMR2_CC4S_0; + PWM_TIMER->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E; + PWM_TIMER->DIER |= TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4; + PWM_TIMER->CR1 = TIM_CR1_CEN; // Start timer + + NVIC_EnableIRQ(PWM_IRQn); + NVIC_SetPriority(PWM_IRQn, 10); +} + +inline uint32_t TIM_GetCapture(uint8_t n) +{ + switch (n) { + case 0: + return PWM_TIMER->CCR1; + case 1: + return PWM_TIMER->CCR2; + case 2: + return PWM_TIMER->CCR3; + case 3: + return PWM_TIMER->CCR4; + default: + return 0; + } +} + +inline void TIM_SetPolarityRising(uint8_t n) +{ + switch (n) { + case 0: + PWM_TIMER->CCER &= ~TIM_CCER_CC1P; + break; + case 1: + PWM_TIMER->CCER &= ~TIM_CCER_CC2P; + break; + case 2: + PWM_TIMER->CCER &= ~TIM_CCER_CC3P; + break; + case 3: + PWM_TIMER->CCER &= ~TIM_CCER_CC4P; + break; + } +} + +inline void TIM_SetPolarityFalling(uint8_t n) +{ + switch (n) { + case 0: + PWM_TIMER->CCER |= TIM_CCER_CC1P; + break; + case 1: + PWM_TIMER->CCER |= TIM_CCER_CC2P; + break; + case 2: + PWM_TIMER->CCER |= TIM_CCER_CC3P; + break; + case 3: + PWM_TIMER->CCER |= TIM_CCER_CC4P; + break; + } +} + +inline void TIM_ClearITPendingBit(uint8_t n) +{ + switch (n) { + case 0: + PWM_TIMER->SR = ~TIM_IT_CC1; + break; + case 1: + PWM_TIMER->SR = ~TIM_IT_CC2; + break; + case 2: + PWM_TIMER->SR = ~TIM_IT_CC3; + break; + case 3: + PWM_TIMER->SR = ~TIM_IT_CC4; + break; + } +} + +inline uint32_t diff_with_16bits_overflow(uint32_t a, uint32_t b) +{ + if (b > a) + return b - a; + else + return b + 0xffff - a; +} + +extern "C" void PWM_IRQHandler(void) +{ + for (int i=0; i<4; i++) { + if (PWM_TIMER->SR & (TIM_DIER_CC1IE << i)) { + uint32_t capture = TIM_GetCapture(i); + if (timer_capture_states[i] != 0) { + uint32_t value = diff_with_16bits_overflow(timer_capture_rising_time[i], capture); + if (value < 10000) { + pwm_interrupt_count++; + timer_capture_values[i][timer_capture_indexes[i]++] = value; + timer_capture_indexes[i] %= TIMESAMPLE_COUNT; + } + TIM_SetPolarityRising(i); + timer_capture_states[i] = 0; + } + else { + timer_capture_rising_time[i] = capture; + TIM_SetPolarityFalling(i); + timer_capture_states[i] = 0x80; + } + TIM_ClearITPendingBit(i); + } + } +} + +void pwmRead(uint16_t * values) +{ + uint32_t tmp[4]; + + for (int i=0; i<4; i++) { + uint32_t mycount = 0; + uint32_t mymax = 0; + uint32_t mymin = 4095; + for (uint32_t j=0; j mymax) + mymax = value; + if (value < mymin) + mymin = value; + } + // from the 6 values, remove the min and max and divide by 4 + tmp[i] = (mycount - mymax - mymin) >> 2; + } + + values[0] = tmp[0]; + values[1] = tmp[1]; + values[2] = tmp[3]; + values[3] = tmp[2]; +} + +void pwmCheck() +{ + // I have ~1860 interrupts with only one stick + if (pwm_interrupt_count < 1000) { + analogs_pwm_disabled = true; + adcInit(); + } +}