diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index 09a04a9c6a..48b902028d 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -24,5 +24,6 @@ namespace ChibiOS { class Util; class Shared_DMA; class SoftSigReader; + class SoftSigReaderInt; class CANManager; } diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index e3d8a8b9a3..fee2eafb72 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -35,6 +35,12 @@ void RCInput::init() sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL); rcin_prot.init(); #endif + +#if HAL_USE_EICU == TRUE + sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL); + rcin_prot.init(); +#endif + _init = true; } @@ -166,7 +172,7 @@ void RCInput::_timer_tick(void) if (!_init) { return; } -#if HAL_USE_ICU == TRUE +#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE uint32_t width_s0, width_s1; while(sig_reader.read(width_s0, width_s1)) { diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index 36ce3188dc..9d7771312b 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -17,7 +17,6 @@ #pragma once #include "AP_HAL_ChibiOS.h" -#include "SoftSigReader.h" #include "Semaphores.h" #ifdef HAL_RCINPUT_WITH_AP_RADIO @@ -25,9 +24,16 @@ #endif #if HAL_USE_ICU == TRUE +#include "SoftSigReader.h" #include #endif +#if HAL_USE_EICU == TRUE +#include "SoftSigReaderInt.h" +#include +#endif + + #ifndef RC_INPUT_MAX_CHANNELS #define RC_INPUT_MAX_CHANNELS 18 #endif @@ -75,6 +81,11 @@ private: AP_RCProtocol rcin_prot; #endif +#if HAL_USE_EICU == TRUE + ChibiOS::SoftSigReaderInt sig_reader; + AP_RCProtocol rcin_prot; +#endif + #if HAL_WITH_IO_MCU uint32_t last_iomcu_us; #endif diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp new file mode 100644 index 0000000000..79d13ac84a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp @@ -0,0 +1,76 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file 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. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ + +#include "SoftSigReaderInt.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +using namespace ChibiOS; +extern const AP_HAL::HAL& hal; + +#if HAL_USE_EICU == TRUE + +#define eicu_lld_invert_polarity(eicup, channel) \ + (eicup)->tim->CCER ^= ((uint16_t)(STM32_TIM_CCER_CC1P << ((channel) * 4))) + +// singleton instance +SoftSigReaderInt *SoftSigReaderInt::_instance; + +SoftSigReaderInt::SoftSigReaderInt() +{ + _instance = this; +} + +void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan) +{ + last_value = 0; + _icu_drv = icu_drv; + icucfg.dier = 0; + icucfg.frequency = INPUT_CAPTURE_FREQUENCY; + for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) { + icucfg.iccfgp[i]=nullptr; + } + icucfg.iccfgp[chan] = &channel_config; + channel_config.alvl = EICU_INPUT_ACTIVE_HIGH; + channel_config.capture_cb = _irq_handler; + eicuStart(_icu_drv, &icucfg); + eicuEnable(_icu_drv); +} + +void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t channel) +{ + uint16_t value = eicup->tim->CCR[channel]; + _instance->sigbuf.push(value); + eicu_lld_invert_polarity(eicup, channel); +} + +bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1) +{ + uint16_t p0, p1; + if (sigbuf.available() >= 2) { + if (sigbuf.pop(p0)&&sigbuf.pop(p1)) { + widths0 = p0 - last_value; + widths1 = p1 - p0; + last_value = p1; + return true; + } + } + return false; +} + +#endif // HAL_USE_EICU + +#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h new file mode 100644 index 0000000000..242c3a25e5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h @@ -0,0 +1,57 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file 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. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once +#include +#include + +#include "AP_HAL_ChibiOS.h" + +#if HAL_USE_EICU == TRUE + +#define INPUT_CAPTURE_FREQUENCY 1000000 //capture unit in microseconds +#define MAX_SIGNAL_TRANSITIONS 256 + + +class ChibiOS::SoftSigReaderInt { +public: + SoftSigReaderInt(); + /* Do not allow copies */ + SoftSigReaderInt(const SoftSigReaderInt &other) = delete; + SoftSigReaderInt &operator=(const SoftSigReaderInt&) = delete; + + // get singleton + static SoftSigReaderInt *get_instance(void) + { + return _instance; + } + + void init(EICUDriver* icu_drv, eicuchannel_t chan); + bool read(uint32_t &widths0, uint32_t &widths1); +private: + // singleton + static SoftSigReaderInt *_instance; + + static void _irq_handler(EICUDriver *eicup, eicuchannel_t channel); + + ObjectBuffer sigbuf{MAX_SIGNAL_TRANSITIONS}; + EICUConfig icucfg; + EICUChannelConfig channel_config; + EICUDriver* _icu_drv = nullptr; + uint16_t last_value; +}; + +#endif // HAL_USE_EICU + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat index 2048b87260..267de97733 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro/hwdef.dat @@ -35,13 +35,11 @@ PC2 BAT_VOLT_SENS ADC1 SCALE(1) PA0 RSSI_IN ADC1 #pwm output -#PB0 TIM3_CH3 TIM3 PWM(1) GPIO(26) -#PB1 TIM3_CH4 TIM3 PWM(2) GPIO(27) PB0 TIM1_CH2N TIM1 PWM(1) GPIO(26) PB1 TIM1_CH3N TIM1 PWM(2) GPIO(27) -PA1 TIM2_CH2 TIM2 PWM(3) GPIO(15) +PA3 TIM2_CH4 TIM2 PWM(3) GPIO(17) PA2 TIM2_CH3 TIM2 PWM(4) GPIO(16) -PA3 TIM2_CH4 TIM2 PWM(5) GPIO(17) +PA1 TIM2_CH2 TIM2 PWM(5) GPIO(15) PA8 TIM1_CH1 TIM1 PWM(6) GPIO(41) PA4 MPU6000_CS CS @@ -79,8 +77,7 @@ PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 PC5 VBUS INPUT OPENDRAIN -#Native ppm_in ps is PB8, but it has not right timer. -#PB8 TIM4_CH3 TIM4 RCIN PULLDOWN LOW DMA_CH1 +PB8 TIM4_CH3 TIM4 RCININT PULLDOWN LOW # SPI Device table SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ @@ -113,4 +110,4 @@ define HAL_GPIO_A_LED_PIN 57 define STM32_PWM_USE_ADVANCED TRUE -define BOARD_PWM_COUNT_DEFAULT 6 \ No newline at end of file +define BOARD_PWM_COUNT_DEFAULT 6 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index baa9d113e4..c2a32bfa56 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -578,6 +578,7 @@ def write_I2C_config(f): def write_PWM_config(f): '''write PWM config defines''' rc_in = None + rc_in_int = None alarm = None pwm_out = [] pwm_timers = [] @@ -586,6 +587,8 @@ def write_PWM_config(f): if p.type.startswith('TIM'): if p.has_extra('RCIN'): rc_in = p + elif p.has_extra('RCININT'): + rc_in_int = p elif p.has_extra('ALARM'): alarm = p else: @@ -620,6 +623,21 @@ def write_PWM_config(f): f.write('#define STM32_RCIN_DMA_STREAM STM32_TIM_TIM%u_CH%u_DMA_STREAM\n' % (n, int(chan_str))) f.write('#define STM32_RCIN_DMA_CHANNEL STM32_TIM_TIM%u_CH%u_DMA_CHAN\n' % (n, int(chan_str))) f.write('\n') + + if rc_in_int is not None: + a = rc_in_int.label.split('_') + chan_str = a[1][2:] + timer_str = a[0][3:] + if not is_int(chan_str) or not is_int(timer_str): + error("Bad timer channel %s" % rc_in.label) + n = int(a[0][3:]) + f.write('// RC input config\n') + f.write('#define HAL_USE_EICU TRUE\n') + f.write('#define STM32_EICU_USE_TIM%u TRUE\n' % n) + f.write('#define RCININT_EICU_TIMER EICUD%u\n' % n) + f.write('#define RCININT_EICU_CHANNEL EICU_CHANNEL_%u\n' % int(chan_str)) + f.write('\n') + if alarm is not None: a = alarm.label.split('_') @@ -972,7 +990,7 @@ def build_peripheral_list(): if label[-1] == 'N': label = label[:-1] peripherals.append(label) - elif not p.has_extra('ALARM'): + elif not p.has_extra('ALARM') and not p.has_extra('RCININT'): # get the TIMn_UP DMA channels for DShot label = type + '_UP' if not label in peripherals: