From fa12b27438063cb46fdab86a91b7462e18e293e6 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 5 Jan 2024 02:48:46 +0100 Subject: [PATCH] feat(radio): Discover SBUS trainer availability (#4266) Co-authored-by: raphaelcoeffic <1050031+raphaelcoeffic@users.noreply.github.com> Co-authored-by: Peter Feerick --- radio/src/bluetooth.cpp | 3 +- radio/src/boards/generic_stm32/CMakeLists.txt | 1 + .../generic_stm32/intmodule_heartbeat.cpp | 35 ++ .../generic_stm32/intmodule_heartbeat.h | 2 +- .../src/boards/generic_stm32/module_ports.cpp | 66 +++- .../boards/generic_stm32/trainer_ports.cpp | 147 ++++++++ radio/src/gui/212x64/view_main.cpp | 2 +- radio/src/gui/gui_common.cpp | 67 ++-- radio/src/hal/module_port.cpp | 19 +- radio/src/hal/module_port.h | 7 +- radio/src/hal/trainer_driver.h | 34 +- radio/src/mixer.cpp | 8 +- radio/src/myeeprom.h | 6 - radio/src/opentx.cpp | 2 +- radio/src/sbus.cpp | 2 +- radio/src/serial.cpp | 2 - radio/src/targets/common/arm/CMakeLists.txt | 1 - .../common/arm/stm32/heartbeat_driver.h | 2 + .../common/arm/stm32/stm32_pulse_driver.cpp | 2 + .../common/arm/stm32/trainer_driver.cpp | 319 ++++-------------- .../targets/common/arm/stm32/trainer_driver.h | 41 +++ radio/src/targets/horus/board.cpp | 4 +- radio/src/targets/horus/hal.h | 2 - radio/src/targets/nv14/board.cpp | 3 +- radio/src/targets/nv14/board.h | 3 - radio/src/targets/nv14/hal.h | 2 - radio/src/targets/pl18/board.cpp | 3 +- radio/src/targets/pl18/board.h | 3 - radio/src/targets/pl18/hal.h | 1 - radio/src/targets/simu/module_drivers.cpp | 34 +- radio/src/targets/simu/opentxsimulator.cpp | 3 +- radio/src/targets/taranis/board.cpp | 6 +- radio/src/targets/taranis/hal.h | 11 +- radio/src/tasks/mixer_task.cpp | 2 - radio/src/telemetry/multi.cpp | 4 +- radio/src/tests/mixer.cpp | 2 +- radio/src/tests/module_ports.cpp | 4 +- radio/src/trainer.cpp | 132 +++++--- radio/src/trainer.h | 47 +-- 39 files changed, 571 insertions(+), 463 deletions(-) create mode 100644 radio/src/boards/generic_stm32/trainer_ports.cpp create mode 100644 radio/src/targets/common/arm/stm32/trainer_driver.h diff --git a/radio/src/bluetooth.cpp b/radio/src/bluetooth.cpp index 47c7ed34a..7f325c15f 100644 --- a/radio/src/bluetooth.cpp +++ b/radio/src/bluetooth.cpp @@ -23,6 +23,7 @@ #include "opentx.h" #include "io/frsky_firmware_update.h" #include "bluetooth_driver.h" +#include "trainer.h" #if defined(LIBOPENUI) #include "libopenui.h" @@ -145,7 +146,7 @@ void Bluetooth::processTrainerFrame(const uint8_t * buffer) trainerInput[channel+1] = ((buffer[i+1] & 0x0f) << 4) + ((buffer[i+2] & 0xf0) >> 4) + ((buffer[i+2] & 0x0f) << 8) - 1500; } - trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT; + trainerResetTimer(); } void Bluetooth::appendTrainerByte(uint8_t data) diff --git a/radio/src/boards/generic_stm32/CMakeLists.txt b/radio/src/boards/generic_stm32/CMakeLists.txt index d320f117d..b64fb2f1b 100644 --- a/radio/src/boards/generic_stm32/CMakeLists.txt +++ b/radio/src/boards/generic_stm32/CMakeLists.txt @@ -22,6 +22,7 @@ set(BOARD_LIB_SRC ${CMAKE_CURRENT_BINARY_DIR}/hal_adc_inputs.inc boards/generic_stm32/module_ports.cpp + boards/generic_stm32/trainer_ports.cpp boards/generic_stm32/aux_ports.cpp boards/generic_stm32/sport_update.cpp boards/generic_stm32/intmodule_heartbeat.cpp diff --git a/radio/src/boards/generic_stm32/intmodule_heartbeat.cpp b/radio/src/boards/generic_stm32/intmodule_heartbeat.cpp index 3def287cb..9069a97d5 100644 --- a/radio/src/boards/generic_stm32/intmodule_heartbeat.cpp +++ b/radio/src/boards/generic_stm32/intmodule_heartbeat.cpp @@ -24,7 +24,12 @@ #include "heartbeat_driver.h" #include "mixer_scheduler.h" #include "dataconstants.h" + #include "pulses/pxx.h" +#include "trainer.h" + +#include "opentx.h" +#include "pulses/pulses.h" #if defined(PXX1) #include "pulses/pxx1.h" @@ -40,9 +45,21 @@ #define isPxx2Driver(drv) (false) #endif +static bool _trainer_mode_uses_ext_module(uint8_t mode) +{ + return mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE || + mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE; +} + void _intmodule_heartbeat_init(uint8_t module, const etx_proto_driver_t* drv) { if (module == INTERNAL_MODULE && (isPxx1Driver(drv) || isPxx2Driver(drv))) { + + // heartbeat cannot be used if the trainer is using the module bay + if (_trainer_mode_uses_ext_module(currentTrainerMode)) { + return; + } + init_intmodule_heartbeat(); if (isPxx1Driver(drv)) { // XJT / iXJT @@ -61,3 +78,21 @@ void _intmodule_heartbeat_deinit(uint8_t module, const etx_proto_driver_t* drv) stop_intmodule_heartbeat(); } } + +void _intmodule_heartbeat_trainer_hook(uint8_t old_mode, uint8_t new_mode) +{ + bool restart_int_module = false; + + if (_trainer_mode_uses_ext_module(new_mode)) { + restart_int_module = true; + } else if (_trainer_mode_uses_ext_module(old_mode)) { + restart_int_module = true; + } + + if (restart_int_module) { + auto pdrv = pulsesGetModuleDriver(INTERNAL_MODULE); + if (pdrv && (isPxx2Driver(pdrv->drv) || isPxx1Driver(pdrv->drv))) { + restartModule(INTERNAL_MODULE); + } + } +} diff --git a/radio/src/boards/generic_stm32/intmodule_heartbeat.h b/radio/src/boards/generic_stm32/intmodule_heartbeat.h index b2f1ef911..10a73b4e0 100644 --- a/radio/src/boards/generic_stm32/intmodule_heartbeat.h +++ b/radio/src/boards/generic_stm32/intmodule_heartbeat.h @@ -25,4 +25,4 @@ void _intmodule_heartbeat_init(uint8_t module, const etx_proto_driver_t* drv); void _intmodule_heartbeat_deinit(uint8_t module, const etx_proto_driver_t* drv); - +void _intmodule_heartbeat_trainer_hook(uint8_t old_mode, uint8_t new_mode); diff --git a/radio/src/boards/generic_stm32/module_ports.cpp b/radio/src/boards/generic_stm32/module_ports.cpp index e7e07fd3d..104d07dca 100644 --- a/radio/src/boards/generic_stm32/module_ports.cpp +++ b/radio/src/boards/generic_stm32/module_ports.cpp @@ -23,6 +23,7 @@ #include "stm32_serial_driver.h" #include "stm32_softserial_driver.h" #include "stm32_dma.h" +#include "trainer_driver.h" #include "module_ports.h" #include "board.h" @@ -177,6 +178,24 @@ static const stm32_usart_t extmoduleUSART = { DEFINE_STM32_SERIAL_PORT(ExternalModule, extmoduleUSART, INTMODULE_FIFO_SIZE, 0); +#elif defined(TRAINER_MODULE_SBUS_USART) + +static const stm32_usart_t sbus_trainer_USART = { + .USARTx = TRAINER_MODULE_SBUS_USART, + .GPIOx = TRAINER_MODULE_SBUS_GPIO, + .GPIO_Pin = TRAINER_MODULE_SBUS_GPIO_PIN, + .IRQn = (IRQn_Type)-1, + .IRQ_Prio = 0, + .txDMA = nullptr, + .txDMA_Stream = 0, + .txDMA_Channel = 0, + .rxDMA = TRAINER_MODULE_SBUS_DMA, + .rxDMA_Stream = TRAINER_MODULE_SBUS_DMA_STREAM_LL, + .rxDMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL, +}; + +DEFINE_STM32_SERIAL_PORT(SbusTrainer, sbus_trainer_USART, 32, 0); + #endif static stm32_pulse_dma_tc_cb_t _ext_timer_DMA_TC_Callback; @@ -223,7 +242,30 @@ extern "C" void EXTMODULE_TIMER_IRQHandler() } DEFINE_STM32_SOFTSERIAL_PORT(ExternalModule, extmoduleTimer); -#endif + +#if defined(TRAINER_MODULE_CPPM_TIMER) + +static_assert(__IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(TRAINER_MODULE_CPPM_TIMER_Channel), + "Unsupported trainer timer input channel"); + +static const stm32_pulse_timer_t trainerModuleTimer = { + .GPIOx = TRAINER_MODULE_CPPM_GPIO, + .GPIO_Pin = TRAINER_MODULE_CPPM_GPIO_PIN, + .GPIO_Alternate = TRAINER_MODULE_CPPM_GPIO_AF, + .TIMx = TRAINER_MODULE_CPPM_TIMER, + .TIM_Freq = TRAINER_MODULE_CPPM_FREQ, + .TIM_Channel = TRAINER_MODULE_CPPM_TIMER_Channel, + .TIM_IRQn = TRAINER_MODULE_CPPM_TIMER_IRQn, + .DMAx = nullptr, + .DMA_Stream = 0, + .DMA_Channel = 0, + .DMA_IRQn = (IRQn_Type)0, + .DMA_TC_CallbackPtr = nullptr, +}; + +#endif // TRAINER_MODULE_CPPM_TIMER + +#endif // HARDWARE_EXTERNAL_MODULE #define TELEMETRY_USART_IRQ_PRIORITY 0 #define TELEMETRY_DMA_IRQ_PRIORITY 0 @@ -456,6 +498,28 @@ static const etx_module_port_t _external_ports[] = { .set_inverted = nullptr, #endif }, +#if !defined(EXTMODULE_USART) && defined(TRAINER_MODULE_SBUS_USART) + // RX on HEARTBEAT + { + .port = ETX_MOD_PORT_UART, + .type = ETX_MOD_TYPE_SERIAL, + .dir_flags = ETX_MOD_DIR_RX, + .drv = { .serial = &STM32SerialDriver }, + .hw_def = REF_STM32_SERIAL_PORT(SbusTrainer), + .set_inverted = nullptr, + }, +#endif +#if defined(TRAINER_MODULE_CPPM_TIMER) + // Timer input on HEARTBEAT + { + .port = ETX_MOD_PORT_TIMER, + .type = ETX_MOD_TYPE_TIMER, + .dir_flags = ETX_MOD_DIR_RX, + .drv = { .timer = nullptr }, + .hw_def = (void*)&trainerModuleTimer, + .set_inverted = nullptr, + }, +#endif // TX/RX half-duplex on S.PORT { .port = ETX_MOD_PORT_SPORT, diff --git a/radio/src/boards/generic_stm32/trainer_ports.cpp b/radio/src/boards/generic_stm32/trainer_ports.cpp new file mode 100644 index 000000000..b7c3fc127 --- /dev/null +++ b/radio/src/boards/generic_stm32/trainer_ports.cpp @@ -0,0 +1,147 @@ +/* + * 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_pulse_driver.h" +#include "stm32_gpio_driver.h" +#include "trainer_driver.h" + +#include "hal/module_port.h" +#include "dataconstants.h" + +#include "hal.h" + +void board_trainer_init() +{ +#if defined(TRAINER_DETECT_GPIO_PIN) + LL_GPIO_InitTypeDef pinInit; + LL_GPIO_StructInit(&pinInit); + + pinInit.Pin = TRAINER_DETECT_GPIO_PIN; + pinInit.Mode = LL_GPIO_MODE_INPUT; + pinInit.Pull = LL_GPIO_PULL_UP; + stm32_gpio_enable_clock(TRAINER_DETECT_GPIO); + LL_GPIO_Init(TRAINER_DETECT_GPIO, &pinInit); +#endif + + trainer_init(); +} + +#if defined(TRAINER_GPIO) + +bool trainer_dsc_available() { return true; } + +static_assert(__IS_TRAINER_TIMER_OUT_CHANNEL_SUPPORTED(TRAINER_OUT_TIMER_Channel), + "Unsupported trainer timer output channel"); + +static_assert(__IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(TRAINER_IN_TIMER_Channel), + "Unsupported trainer timer input channel"); + +static const stm32_pulse_timer_t trainerOutputTimer = { + .GPIOx = TRAINER_GPIO, + .GPIO_Pin = TRAINER_OUT_GPIO_PIN, + .GPIO_Alternate = TRAINER_GPIO_AF, + .TIMx = TRAINER_TIMER, + .TIM_Freq = TRAINER_TIMER_FREQ, + .TIM_Channel = TRAINER_OUT_TIMER_Channel, + .TIM_IRQn = TRAINER_TIMER_IRQn, + .DMAx = nullptr, + .DMA_Stream = 0, + .DMA_Channel = 0, + .DMA_IRQn = (IRQn_Type)0, + .DMA_TC_CallbackPtr = nullptr, +}; + +void trainer_init_dsc_out() +{ + trainer_init_output(&trainerOutputTimer); +} + +static const stm32_pulse_timer_t trainerInputTimer = { + .GPIOx = TRAINER_GPIO, + .GPIO_Pin = TRAINER_IN_GPIO_PIN, + .GPIO_Alternate = TRAINER_GPIO_AF, + .TIMx = TRAINER_TIMER, + .TIM_Freq = TRAINER_TIMER_FREQ, + .TIM_Channel = TRAINER_IN_TIMER_Channel, + .TIM_IRQn = TRAINER_TIMER_IRQn, + .DMAx = nullptr, + .DMA_Stream = 0, + .DMA_Channel = 0, + .DMA_IRQn = (IRQn_Type)0, + .DMA_TC_CallbackPtr = nullptr, +}; + +void trainer_init_dsc_in() +{ + trainer_init_capture(&trainerInputTimer); +} + +void trainer_stop_dsc() { trainer_stop(); } + +#else +bool trainer_dsc_available() { return false; } +void trainer_init_dsc_out() {} +void trainer_init_dsc_in() {} +void trainer_stop_dsc() {} +#endif + +bool is_trainer_dsc_connected() +{ +#if defined(TRAINER_DETECT_GPIO_PIN) + bool set = LL_GPIO_IsInputPinSet(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN); +#if defined(TRAINER_DETECT_INVERTED) + return !set; +#else + return set; +#endif +#else // TRAINER_DETECT_GPIO_PIN + return true; +#endif +} + +void trainer_init_module_cppm() +{ + auto port = modulePortFind(EXTERNAL_MODULE, ETX_MOD_TYPE_TIMER, + ETX_MOD_PORT_TIMER, ETX_Pol_Normal, + ETX_MOD_DIR_RX); + if (!port) return; + + auto tim = (const stm32_pulse_timer_t*)port->hw_def; + if (!tim) return; + + modulePortSetPower(EXTERNAL_MODULE,true); + trainer_init_capture(tim); +} + +void trainer_stop_module_cppm() +{ + trainer_stop(); + modulePortSetPower(EXTERNAL_MODULE,false); +} + +#if defined(TRAINER_TIMER_IRQHandler) +extern "C" void TRAINER_TIMER_IRQHandler() { trainer_timer_isr(); } +#endif + +#if defined(TRAINER_MODULE_CPPM_TIMER_IRQHandler) && \ + TRAINER_TIMER_IRQHandler != TRAINER_MODULE_CPPM_TIMER_IRQHandler +extern "C" void TRAINER_MODULE_CPPM_TIMER_IRQHandler() { trainer_timer_isr(); } +#endif diff --git a/radio/src/gui/212x64/view_main.cpp b/radio/src/gui/212x64/view_main.cpp index 5fb34a9cb..d0ed17d80 100644 --- a/radio/src/gui/212x64/view_main.cpp +++ b/radio/src/gui/212x64/view_main.cpp @@ -309,7 +309,7 @@ void displayTopBar() x -= 12; } } - else if (is_trainer_connected()) { + else if (isTrainerConnected()) { LCD_NOTIF_ICON(x, ICON_TRAINER); x -= 12; } diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 5c185b0db..df1e74412 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -19,11 +19,12 @@ * GNU General Public License for more details. */ -#include "opentx.h" - #include "hal/module_port.h" #include "hal/adc_driver.h" #include "hal/switch_driver.h" +#include "hal/trainer_driver.h" + +#include "opentx.h" #include "switches.h" #include "mixes.h" @@ -725,10 +726,11 @@ bool isPxx2IsrmChannelsCountAllowed(int channels) bool isTrainerUsingModuleBay() { -#if defined(PCBTARANIS) - if (TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE <= g_model.trainerData.mode && g_model.trainerData.mode <= TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) + if (g_model.trainerData.mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE || + g_model.trainerData.mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) { return true; -#endif + } + return false; } @@ -892,7 +894,8 @@ bool isExternalModuleAvailable(int moduleType) #if defined(PXX2) return modulePortFind(EXTERNAL_MODULE, ETX_MOD_TYPE_SERIAL, - ETX_MOD_PORT_UART, ETX_Pol_Normal); + ETX_MOD_PORT_UART, ETX_Pol_Normal, + ETX_MOD_DIR_TX_RX | ETX_MOD_FULL_DUPLEX); #else return false; #endif @@ -1020,38 +1023,46 @@ bool isTrainerModeAvailable(int mode) ) return false; -#if (defined(PCBXLITE) && !defined(PCBXLITES)) || defined(RADIO_COMMANDO8) - if (mode == TRAINER_MODE_MASTER_TRAINER_JACK || mode == TRAINER_MODE_SLAVE) + if ((mode == TRAINER_MODE_MASTER_TRAINER_JACK || + mode == TRAINER_MODE_SLAVE) && + !trainer_dsc_available()) return false; -#endif -#if !defined(TRAINER_MODULE_CPPM) - if (mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) - return false; -#endif + if (mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE || + mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) { -#if !defined(TRAINER_MODULE_SBUS) - if (mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) - return false; -#endif + // no external module or is enabled + if (!modulePortGetModuleDescription(EXTERNAL_MODULE) || + IS_EXTERNAL_MODULE_ENABLED()) { + return false; + } -#if defined(TRAINER_MODULE_CPPM) || defined(TRAINER_MODULE_SBUS) - if (IS_EXTERNAL_MODULE_ENABLED() && - (mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE || - mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE)) - return false; -#endif + if (mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) { + auto port = modulePortFind(EXTERNAL_MODULE, ETX_MOD_TYPE_TIMER, + ETX_MOD_PORT_TIMER, ETX_Pol_Normal, + ETX_MOD_DIR_RX); + return port != nullptr; + } + + if (mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) { + auto port = modulePortFind(EXTERNAL_MODULE, ETX_MOD_TYPE_SERIAL, + ETX_MOD_PORT_UART, ETX_Pol_Normal, + ETX_MOD_DIR_RX); + return port != nullptr; + } + } -#if !defined(MULTIMODULE) || !defined(HARDWARE_INTERNAL_MODULE) || !defined(HARDWARE_EXTERNAL_MODULE) - if (mode == TRAINER_MODE_MULTI) + if (mode == TRAINER_MODE_MULTI) { + +#if !defined(MULTIMODULE) return false; #else - if (mode == TRAINER_MODE_MULTI && - ((!IS_INTERNAL_MODULE_ENABLED() && !IS_EXTERNAL_MODULE_ENABLED()) || + if ((!IS_INTERNAL_MODULE_ENABLED() && !IS_EXTERNAL_MODULE_ENABLED()) || (!isModuleMultimodule(INTERNAL_MODULE) && - !isModuleMultimodule(EXTERNAL_MODULE)))) + !isModuleMultimodule(EXTERNAL_MODULE))) return false; #endif + } return true; } diff --git a/radio/src/hal/module_port.cpp b/radio/src/hal/module_port.cpp index 1e5fb957c..839aa1ab8 100644 --- a/radio/src/hal/module_port.cpp +++ b/radio/src/hal/module_port.cpp @@ -106,8 +106,12 @@ static bool _init_timer_driver(etx_module_driver_t* d, const etx_module_port_t* } static bool _match_port(const etx_module_port_t* p, uint8_t type, uint8_t port, - uint8_t polarity, bool softserial_fallback) + uint8_t polarity, uint8_t direction, bool softserial_fallback) { + if ((p->dir_flags & direction) != direction) { + return false; + } + if (p->type == type && p->port == port) { // either polarity matches or can be set if (polarity != ETX_Pol_Inverted || @@ -134,6 +138,7 @@ static bool _match_port(const etx_module_port_t* p, uint8_t type, uint8_t port, static const etx_module_port_t* _find_port(uint8_t module, uint8_t type, uint8_t port, uint8_t polarity, + uint8_t direction, bool softserial_fallback) { if (module >= MAX_MODULES || module >= _n_modules || !_modules[module]) @@ -145,7 +150,7 @@ static const etx_module_port_t* _find_port(uint8_t module, uint8_t type, while(n_ports > 0) { - if (_match_port(p, type, port, polarity, softserial_fallback)) { + if (_match_port(p, type, port, polarity, direction, softserial_fallback)) { found_port = p; break; } @@ -173,9 +178,10 @@ const etx_module_t* modulePortGetModuleDescription(uint8_t module) } const etx_module_port_t* modulePortFind(uint8_t module, uint8_t type, - uint8_t port, uint8_t polarity) + uint8_t port, uint8_t polarity, + uint8_t direction) { - return _find_port(module, type, port, polarity, false); + return _find_port(module, type, port, polarity, direction, false); } void modulePortSetPower(uint8_t module, uint8_t enable) @@ -204,6 +210,7 @@ etx_module_state_t* modulePortInitSerial(uint8_t module, uint8_t port, { const etx_module_port_t* found_port = _find_port(module, ETX_MOD_TYPE_SERIAL, port, params->polarity, + params->direction, softserial_fallback); if (!found_port) return nullptr; @@ -236,8 +243,8 @@ etx_module_state_t* modulePortInitSerial(uint8_t module, uint8_t port, etx_module_state_t* modulePortInitTimer(uint8_t module, uint8_t port, const etx_timer_config_t* cfg) { - const etx_module_port_t* found_port = _find_port(module, ETX_MOD_TYPE_TIMER, - port, ETX_Pol_Normal, false); + const etx_module_port_t* found_port = _find_port( + module, ETX_MOD_TYPE_TIMER, port, ETX_Pol_Normal, ETX_MOD_DIR_TX, false); if (!found_port) return nullptr; auto state = &(_module_states[module]); diff --git a/radio/src/hal/module_port.h b/radio/src/hal/module_port.h index f682cc8ec..39932ce60 100644 --- a/radio/src/hal/module_port.h +++ b/radio/src/hal/module_port.h @@ -30,8 +30,8 @@ // // flags -#define ETX_MOD_DIR_TX (1 << 0) -#define ETX_MOD_DIR_RX (1 << 1) +#define ETX_MOD_DIR_RX (1 << 0) +#define ETX_MOD_DIR_TX (1 << 1) #define ETX_MOD_DIR_TX_RX (ETX_MOD_DIR_TX | ETX_MOD_DIR_RX) #define ETX_MOD_FULL_DUPLEX (1 << 2) @@ -117,7 +117,8 @@ void modulePortConfigExtra(const etx_module_port_t* port); const etx_module_t* modulePortGetModuleDescription(uint8_t module); const etx_module_port_t* modulePortFind(uint8_t module, uint8_t type, - uint8_t port, uint8_t polarity); + uint8_t port, uint8_t polarity, + uint8_t direction); void modulePortSetPower(uint8_t module, uint8_t enabled); diff --git a/radio/src/hal/trainer_driver.h b/radio/src/hal/trainer_driver.h index 30f2fef6e..2bf1c5c84 100644 --- a/radio/src/hal/trainer_driver.h +++ b/radio/src/hal/trainer_driver.h @@ -22,33 +22,23 @@ #pragma once // Startup init -void init_trainer(); +void board_trainer_init(); +// DSC port available? +bool trainer_dsc_available(); + // Output mode -void init_trainer_ppm(); -void stop_trainer_ppm(); +void trainer_init_dsc_out(); // Input mode -void init_trainer_capture(); -void stop_trainer_capture(); +void trainer_init_dsc_in(); + +// Stop input/output +void trainer_stop_dsc(); // Cable inserted? bool is_trainer_dsc_connected(); -// Active signal received -extern uint8_t trainerInputValidityTimer; -inline bool is_trainer_connected() -{ - return (trainerInputValidityTimer != 0); -} - -#if defined(TRAINER_MODULE_CPPM) -void init_trainer_module_cppm(); -void stop_trainer_module_cppm(); -#endif - -#if defined(TRAINER_MODULE_SBUS) -void init_trainer_module_sbus(); -void stop_trainer_module_sbus(); -int trainerModuleSbusGetByte(uint8_t* byte); -#endif +// Start/stop CPPM from module bay +void trainer_init_module_cppm(); +void trainer_stop_module_cppm(); diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 90a22fb76..622c8f42f 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -167,7 +167,7 @@ void applyExpos(int16_t * anas, uint8_t mode, uint8_t ovwrIdx, int16_t ovwrValue continue; if (ed->flightModes & (1<srcRaw >= MIXSRC_FIRST_TRAINER && ed->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) + if (ed->srcRaw >= MIXSRC_FIRST_TRAINER && ed->srcRaw <= MIXSRC_LAST_TRAINER && !isTrainerValid()) continue; if (getSwitch(ed->swtch)) { int32_t v; @@ -240,7 +240,7 @@ int16_t applyLimits(uint8_t channel, int32_t value) } #endif - if (isFunctionActive(FUNCTION_TRAINER_CHANNELS) && is_trainer_connected()) { + if (isFunctionActive(FUNCTION_TRAINER_CHANNELS) && isTrainerValid()) { return trainerInput[channel] * 2; } @@ -546,7 +546,7 @@ void evalInputs(uint8_t mode) if (mode <= e_perout_mode_inactive_flight_mode && isFunctionActive(FUNCTION_TRAINER_STICK1 + ch) && - is_trainer_connected()) { + isTrainerValid()) { // trainer mode TrainerMix* td = &g_eeGeneral.trainer.mix[ch]; if (td->mode) { @@ -770,7 +770,7 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) if (mixLineActive) { // disable mixer using trainer channels if not connected if (md->srcRaw >= MIXSRC_FIRST_TRAINER && - md->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) { + md->srcRaw <= MIXSRC_LAST_TRAINER && !isTrainerValid()) { mixCondition = true; mixEnabled = 0; } diff --git a/radio/src/myeeprom.h b/radio/src/myeeprom.h index 69eafc19f..793fedc7b 100644 --- a/radio/src/myeeprom.h +++ b/radio/src/myeeprom.h @@ -40,12 +40,6 @@ #define GET_MODULE_PPM_DELAY(idx) (g_model.moduleData[idx].ppm.delay * 50 + 300) #define GET_TRAINER_PPM_DELAY() (g_model.trainerData.delay * 50 + 300) -#if defined(HARDWARE_TRAINER_EXTERNAL_MODULE) - #define IS_TRAINER_EXTERNAL_MODULE() (g_model.trainerData.mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE || g_model.trainerData.mode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE) -#else - #define IS_TRAINER_EXTERNAL_MODULE() false -#endif - #define IS_PLAY_FUNC(func) ((func) >= FUNC_PLAY_SOUND && func <= FUNC_PLAY_VALUE) #if defined(GVARS) diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 72a0ab906..324ce862a 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -166,7 +166,7 @@ void per10ms() #endif if (trimsCheckTimer) trimsCheckTimer--; - if (trainerInputValidityTimer) trainerInputValidityTimer--; + trainerDecTimer(); if (trimsDisplayTimer) trimsDisplayTimer--; diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 3ca5a5775..02ad9316b 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -91,7 +91,7 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size) inputbits >>= SBUS_CH_BITS; } - trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT; + trainerResetTimer(); } void processSbusInput() diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 0ae65a595..b4382af18 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -217,12 +217,10 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por break; #endif -#if defined(SBUS_TRAINER) case UART_MODE_SBUS_TRAINER: sbusSetAuxGetByte(ctx, getByte); // TODO: setRxCb (see MODE_LUA) break; -#endif case UART_MODE_TELEMETRY: // telemetrySetGetByte(ctx, getByte); diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index e7e353b02..57511fc93 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -278,7 +278,6 @@ add_definitions(-g) add_definitions(-DCPUARM) add_definitions(-DGPS) add_definitions(-DBATTGRAPH -DTHRTRACE) -add_definitions(-DSBUS_TRAINER) foreach(LANGUAGE ${TTS_LANGUAGES}) string(TOLOWER ${LANGUAGE} lang_lower) diff --git a/radio/src/targets/common/arm/stm32/heartbeat_driver.h b/radio/src/targets/common/arm/stm32/heartbeat_driver.h index 225f8934f..0a0665e34 100644 --- a/radio/src/targets/common/arm/stm32/heartbeat_driver.h +++ b/radio/src/targets/common/arm/stm32/heartbeat_driver.h @@ -21,6 +21,8 @@ #pragma once +#include + struct HeartbeatCapture { uint8_t valid; #if defined(DEBUG_LATENCY) diff --git a/radio/src/targets/common/arm/stm32/stm32_pulse_driver.cpp b/radio/src/targets/common/arm/stm32/stm32_pulse_driver.cpp index e310c1f40..55550ebd6 100644 --- a/radio/src/targets/common/arm/stm32/stm32_pulse_driver.cpp +++ b/radio/src/targets/common/arm/stm32/stm32_pulse_driver.cpp @@ -20,6 +20,7 @@ */ #include "stm32_pulse_driver.h" +#include "stm32_gpio_driver.h" #include "stm32_timer.h" #include "stm32_dma.h" @@ -87,6 +88,7 @@ int stm32_pulse_init(const stm32_pulse_timer_t* tim, uint32_t freq) pinInit.Pin = tim->GPIO_Pin; pinInit.Mode = LL_GPIO_MODE_ALTERNATE; pinInit.Alternate = tim->GPIO_Alternate; + stm32_gpio_enable_clock(tim->GPIOx); LL_GPIO_Init(tim->GPIOx, &pinInit); LL_TIM_InitTypeDef timInit; diff --git a/radio/src/targets/common/arm/stm32/trainer_driver.cpp b/radio/src/targets/common/arm/stm32/trainer_driver.cpp index c2ecd32cd..2b9e12beb 100644 --- a/radio/src/targets/common/arm/stm32/trainer_driver.cpp +++ b/radio/src/targets/common/arm/stm32/trainer_driver.cpp @@ -20,49 +20,21 @@ */ #include "stm32_pulse_driver.h" +#include "stm32_serial_driver.h" + #include "hal/trainer_driver.h" +#include "hal/module_port.h" + #include "hal.h" #include "opentx.h" - -#define __IS_TRAINER_TIMER_OUT_CHANNEL_SUPPORTED(ch) \ - (((ch) == LL_TIM_CHANNEL_CH1 || (ch) == LL_TIM_CHANNEL_CH2 || \ - (ch) == LL_TIM_CHANNEL_CH3 || (ch) == LL_TIM_CHANNEL_CH4) && \ - __STM32_PULSE_IS_TIMER_CHANNEL_SUPPORTED(ch)) - -#define __IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(ch) \ - ((ch) == LL_TIM_CHANNEL_CH1 || (ch) == LL_TIM_CHANNEL_CH2 || \ - (ch) == LL_TIM_CHANNEL_CH3 || (ch) == LL_TIM_CHANNEL_CH4) - -#if defined(TRAINER_GPIO) - -static_assert(__IS_TRAINER_TIMER_OUT_CHANNEL_SUPPORTED(TRAINER_OUT_TIMER_Channel), - "Unsupported trainer timer output channel"); - -static_assert(__IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(TRAINER_IN_TIMER_Channel), - "Unsupported trainer timer input channel"); -#else -void init_trainer_ppm() {} -void stop_trainer_ppm() {} -void init_trainer_capture() {} -void stop_trainer_capture() {} -#endif +#include "trainer.h" static void (*_trainer_timer_isr)(); static const stm32_pulse_timer_t* _trainer_timer; -void init_trainer() +void trainer_init() { -#if defined(TRAINER_DETECT_GPIO_PIN) - LL_GPIO_InitTypeDef pinInit; - LL_GPIO_StructInit(&pinInit); - - pinInit.Pin = TRAINER_DETECT_GPIO_PIN; - pinInit.Mode = LL_GPIO_MODE_INPUT; - pinInit.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(TRAINER_DETECT_GPIO, &pinInit); -#endif - _trainer_timer = nullptr; _trainer_timer_isr = nullptr; } @@ -102,6 +74,37 @@ static inline bool trainer_check_isr_flag(const stm32_pulse_timer_t* tim) return false; } +static inline void capture_pulse(uint16_t capture) +{ + static uint16_t lastCapt = 0; + static uint8_t channel = MAX_TRAINER_CHANNELS; + + uint16_t val = (uint16_t)(capture - lastCapt) / 2; + lastCapt = capture; + + if (val > 4000 && val < 19000) { + // blanking period in [4..19] milliseconds + channel = 0; + return; + } + + if (channel >= MAX_TRAINER_CHANNELS) { + return; + } + + if (val < 800 || val > 2200) { + // invalid pulse width + channel = MAX_TRAINER_CHANNELS; + return; + } + + trainerInput[channel++] = + // +-500 != 512, but close enough. + (int16_t)(val - 1500) * (g_eeGeneral.PPM_Multiplier + 10) / 10; + + trainerResetTimer(); +} + static void trainer_in_isr() { // proceed only if the channel flag was set @@ -127,17 +130,17 @@ static void trainer_in_isr() return; } - captureTrainerPulses(capture); + capture_pulse(capture); } -static void _stop_trainer(const stm32_pulse_timer_t* tim) +void trainer_stop() { - stm32_pulse_deinit(tim); + stm32_pulse_deinit(_trainer_timer); _trainer_timer_isr = nullptr; _trainer_timer = nullptr; } -static void _init_trainer_capture(const stm32_pulse_timer_t* tim) +void trainer_init_capture(const stm32_pulse_timer_t* tim) { // set proper ISR handler first _trainer_timer = tim; @@ -164,44 +167,27 @@ static void _init_trainer_capture(const stm32_pulse_timer_t* tim) LL_TIM_EnableCounter(tim->TIMx); } -#if defined(TRAINER_GPIO) - -static const stm32_pulse_timer_t trainerOutputTimer = { - .GPIOx = TRAINER_GPIO, - .GPIO_Pin = TRAINER_OUT_GPIO_PIN, - .GPIO_Alternate = TRAINER_GPIO_AF, - .TIMx = TRAINER_TIMER, - .TIM_Freq = TRAINER_TIMER_FREQ, - .TIM_Channel = TRAINER_OUT_TIMER_Channel, - .TIM_IRQn = TRAINER_TIMER_IRQn, - .DMAx = nullptr, - .DMA_Stream = 0, - .DMA_Channel = 0, - .DMA_IRQn = (IRQn_Type)0, - .DMA_TC_CallbackPtr = nullptr, -}; - -static void trainerSendNextFrame() +static void trainer_send_next_frame(const stm32_pulse_timer_t* tim) { - stm32_pulse_set_polarity(&trainerOutputTimer, GET_TRAINER_PPM_POLARITY()); - stm32_pulse_set_cmp_val(&trainerOutputTimer, GET_TRAINER_PPM_DELAY() * 2); + stm32_pulse_set_polarity(tim, GET_TRAINER_PPM_POLARITY()); + stm32_pulse_set_cmp_val(tim, GET_TRAINER_PPM_DELAY() * 2); // load the first period: next reload when CC compare event triggers trainerPulsesData.ppm.ptr = trainerPulsesData.ppm.pulses; TRAINER_TIMER->ARR = *(trainerPulsesData.ppm.ptr++); - switch (trainerOutputTimer.TIM_Channel) { + switch (tim->TIM_Channel) { case LL_TIM_CHANNEL_CH1: - LL_TIM_EnableIT_CC1(trainerOutputTimer.TIMx); + LL_TIM_EnableIT_CC1(tim->TIMx); break; case LL_TIM_CHANNEL_CH2: - LL_TIM_EnableIT_CC2(trainerOutputTimer.TIMx); + LL_TIM_EnableIT_CC2(tim->TIMx); break; case LL_TIM_CHANNEL_CH3: - LL_TIM_EnableIT_CC3(trainerOutputTimer.TIMx); + LL_TIM_EnableIT_CC3(tim->TIMx); break; case LL_TIM_CHANNEL_CH4: - LL_TIM_EnableIT_CC4(trainerOutputTimer.TIMx); + LL_TIM_EnableIT_CC4(tim->TIMx); break; } } @@ -219,220 +205,29 @@ static void trainer_out_isr() *(trainerPulsesData.ppm.ptr++)); } else { setupPulsesPPMTrainer(); - trainerSendNextFrame(); + trainer_send_next_frame(_trainer_timer); } } -void init_trainer_ppm() +void trainer_init_output(const stm32_pulse_timer_t* tim) { // set proper ISR handler first - _trainer_timer = &trainerOutputTimer; + _trainer_timer = tim; _trainer_timer_isr = trainer_out_isr; - stm32_pulse_init(&trainerOutputTimer, 0); - stm32_pulse_config_output(&trainerOutputTimer, GET_TRAINER_PPM_POLARITY(), + stm32_pulse_init(_trainer_timer, 0); + stm32_pulse_config_output(_trainer_timer, GET_TRAINER_PPM_POLARITY(), LL_TIM_OCMODE_PWM1, GET_TRAINER_PPM_DELAY() * 2); setupPulsesPPMTrainer(); - trainerSendNextFrame(); + trainer_send_next_frame(_trainer_timer); - LL_TIM_EnableCounter(trainerOutputTimer.TIMx); + LL_TIM_EnableCounter(_trainer_timer->TIMx); } -void stop_trainer_ppm() +void trainer_timer_isr() { - _stop_trainer(&trainerOutputTimer); -} - -static const stm32_pulse_timer_t trainerInputTimer = { - .GPIOx = TRAINER_GPIO, - .GPIO_Pin = TRAINER_IN_GPIO_PIN, - .GPIO_Alternate = TRAINER_GPIO_AF, - .TIMx = TRAINER_TIMER, - .TIM_Freq = TRAINER_TIMER_FREQ, - .TIM_Channel = TRAINER_IN_TIMER_Channel, - .TIM_IRQn = TRAINER_TIMER_IRQn, - .DMAx = nullptr, - .DMA_Stream = 0, - .DMA_Channel = 0, - .DMA_IRQn = (IRQn_Type)0, - .DMA_TC_CallbackPtr = nullptr, -}; - -void init_trainer_capture() -{ - _init_trainer_capture(&trainerInputTimer); -} - -void stop_trainer_capture() -{ - _stop_trainer(&trainerInputTimer); -} - -#endif // TRAINER_GPIO - -bool is_trainer_dsc_connected() -{ -#if defined(TRAINER_DETECT_GPIO_PIN) - bool set = LL_GPIO_IsInputPinSet(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN); -#if defined(TRAINER_DETECT_INVERTED) - return !set; -#else - return set; -#endif -#else // TRAINER_DETECT_GPIO_PIN - return true; -#endif -} - -#if defined(TRAINER_GPIO) || (defined(TRAINER_MODULE_CPPM) && \ - !defined(TRAINER_MODULE_CPPM_TIMER_IRQHandler)) - -#if !defined(TRAINER_TIMER_IRQHandler) - #error "Missing TRAINER_TIMER_IRQHandler definition" -#endif - -extern "C" void TRAINER_TIMER_IRQHandler() -{ - DEBUG_INTERRUPT(INT_TRAINER); - if (_trainer_timer && _trainer_timer_isr) _trainer_timer_isr(); } -#endif -#if defined(TRAINER_MODULE_CPPM) - -#include "hal/module_port.h" - -static_assert(__IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(TRAINER_MODULE_CPPM_TIMER_Channel), - "Unsupported trainer timer input channel"); - -static const stm32_pulse_timer_t trainerModuleTimer = { - .GPIOx = TRAINER_MODULE_CPPM_GPIO, - .GPIO_Pin = TRAINER_MODULE_CPPM_GPIO_PIN, - .GPIO_Alternate = TRAINER_MODULE_CPPM_GPIO_AF, - .TIMx = TRAINER_MODULE_CPPM_TIMER, - .TIM_Freq = TRAINER_MODULE_CPPM_FREQ, - .TIM_Channel = TRAINER_MODULE_CPPM_TIMER_Channel, - .TIM_IRQn = TRAINER_MODULE_CPPM_TIMER_IRQn, - .DMAx = nullptr, - .DMA_Stream = 0, - .DMA_Channel = 0, - .DMA_IRQn = (IRQn_Type)0, - .DMA_TC_CallbackPtr = nullptr, -}; - -void init_trainer_module_cppm() -{ - modulePortSetPower(EXTERNAL_MODULE,true); - _init_trainer_capture(&trainerModuleTimer); -} - -void stop_trainer_module_cppm() -{ - _stop_trainer(&trainerModuleTimer); - modulePortSetPower(EXTERNAL_MODULE,false); -} - -#if defined(TRAINER_MODULE_CPPM_TIMER_IRQHandler) -extern "C" void TRAINER_MODULE_CPPM_TIMER_IRQHandler() -{ - DEBUG_INTERRUPT(INT_TRAINER); - - if (_trainer_timer && _trainer_timer_isr) - _trainer_timer_isr(); -} -#endif - -#endif // TRAINER_MODULE_CPPM - -#if defined(TRAINER_MODULE_SBUS) - -const etx_serial_init sbusTrainerParams = { - .baudrate = SBUS_BAUDRATE, - .encoding = ETX_Encoding_8E2, - .direction = ETX_Dir_RX, - .polarity = ETX_Pol_Normal, -}; - -// external module may have a full-duplex USART -#if defined(EXTMODULE_USART) || defined(CONFIGURABLE_MODULE_PORT) - -#include "hal/module_port.h" - -static etx_module_state_t* sbus_trainer_mod_st = nullptr; - -void init_trainer_module_sbus() -{ - if (sbus_trainer_mod_st) return; - modulePortSetPower(EXTERNAL_MODULE,true); - - sbus_trainer_mod_st = modulePortInitSerial(EXTERNAL_MODULE, ETX_MOD_PORT_UART, - &sbusTrainerParams, false); -} - -void stop_trainer_module_sbus() -{ - if (!sbus_trainer_mod_st) return; - modulePortDeInit(sbus_trainer_mod_st); - modulePortSetPower(EXTERNAL_MODULE,false); - sbus_trainer_mod_st = nullptr; -} - -int trainerModuleSbusGetByte(uint8_t* data) -{ - if (!sbus_trainer_mod_st) return 0; - - auto serial_driver = modulePortGetSerialDrv(sbus_trainer_mod_st->rx); - auto ctx = modulePortGetCtx(sbus_trainer_mod_st->rx); - - if (ctx) { - return serial_driver->getByte(ctx, data); - } - - return 0; -} - -#elif defined(TRAINER_MODULE_SBUS_USART) -#include "stm32_serial_driver.h" - -static const stm32_usart_t sbus_trainer_USART = { - .USARTx = TRAINER_MODULE_SBUS_USART, - .GPIOx = TRAINER_MODULE_SBUS_GPIO, - .GPIO_Pin = TRAINER_MODULE_SBUS_GPIO_PIN, - .IRQn = (IRQn_Type)-1, - .IRQ_Prio = 0, - .txDMA = nullptr, - .txDMA_Stream = 0, - .txDMA_Channel = 0, - .rxDMA = TRAINER_MODULE_SBUS_DMA, - .rxDMA_Stream = TRAINER_MODULE_SBUS_DMA_STREAM_LL, - .rxDMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL, -}; - -DEFINE_STM32_SERIAL_PORT(SbusTrainer, sbus_trainer_USART, 32, 0); - -static void* _sbus_trainer_ctx = nullptr; - -void init_trainer_module_sbus() -{ - _sbus_trainer_ctx = STM32SerialDriver.init(REF_STM32_SERIAL_PORT(SbusTrainer), - &sbusTrainerParams); -} - -void stop_trainer_module_sbus() -{ - STM32SerialDriver.deinit(_sbus_trainer_ctx); - _sbus_trainer_ctx = nullptr; -} - -int trainerModuleSbusGetByte(uint8_t* data) -{ - return STM32SerialDriver.getByte(_sbus_trainer_ctx, data); -} - -#else - #error "No available SBUS trainer implementation" -#endif -#endif // TRAINER_MODULE_SBUS diff --git a/radio/src/targets/common/arm/stm32/trainer_driver.h b/radio/src/targets/common/arm/stm32/trainer_driver.h new file mode 100644 index 000000000..613bbc784 --- /dev/null +++ b/radio/src/targets/common/arm/stm32/trainer_driver.h @@ -0,0 +1,41 @@ +/* + * 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. + */ + +#pragma once + +#define __IS_TRAINER_TIMER_OUT_CHANNEL_SUPPORTED(ch) \ + (((ch) == LL_TIM_CHANNEL_CH1 || (ch) == LL_TIM_CHANNEL_CH2 || \ + (ch) == LL_TIM_CHANNEL_CH3 || (ch) == LL_TIM_CHANNEL_CH4) && \ + __STM32_PULSE_IS_TIMER_CHANNEL_SUPPORTED(ch)) + +#define __IS_TRAINER_TIMER_IN_CHANNEL_SUPPORTED(ch) \ + ((ch) == LL_TIM_CHANNEL_CH1 || (ch) == LL_TIM_CHANNEL_CH2 || \ + (ch) == LL_TIM_CHANNEL_CH3 || (ch) == LL_TIM_CHANNEL_CH4) + +struct stm32_pulse_timer_t; + +void trainer_init(); +void trainer_init_capture(const stm32_pulse_timer_t* tim); +void trainer_init_output(const stm32_pulse_timer_t* tim); +void trainer_stop(); + +// Call this from the timer's IRQ +void trainer_timer_isr(); diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index 966b8d919..3a0484009 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -60,7 +60,6 @@ void boardInit() SD_RCC_AHB1Periph | AUDIO_RCC_AHB1Periph | TELEMETRY_RCC_AHB1Periph | - TRAINER_RCC_AHB1Periph | BT_RCC_AHB1Periph | AUDIO_RCC_AHB1Periph | HAPTIC_RCC_AHB1Periph | @@ -96,9 +95,10 @@ void boardInit() (defined(INTERNAL_MODULE_PXX1) || defined(INTERNAL_MODULE_PXX2)) pulsesSetModuleInitCb(_intmodule_heartbeat_init); pulsesSetModuleDeInitCb(_intmodule_heartbeat_deinit); + trainerSetChangeCb(_intmodule_heartbeat_trainer_hook); #endif - init_trainer(); + board_trainer_init(); pwrOn(); delaysInit(); diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 8339c48ac..89492e057 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -975,7 +975,6 @@ #endif // Trainer Port -#define TRAINER_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_DMA1) #define TRAINER_GPIO GPIOC #define TRAINER_IN_GPIO_PIN LL_GPIO_PIN_6 // PC.06 @@ -997,7 +996,6 @@ #define TRAINER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) // Trainer CPPM input on heartbeat pin -#define TRAINER_MODULE_CPPM #define TRAINER_MODULE_CPPM_TIMER TIM4 #define TRAINER_MODULE_CPPM_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define TRAINER_MODULE_CPPM_GPIO INTMODULE_HEARTBEAT_GPIO diff --git a/radio/src/targets/nv14/board.cpp b/radio/src/targets/nv14/board.cpp index fcdc13638..79cd5e9ad 100644 --- a/radio/src/targets/nv14/board.cpp +++ b/radio/src/targets/nv14/board.cpp @@ -74,7 +74,6 @@ void delay_self(int count) AUDIO_RCC_AHB1Periph |\ MONITOR_RCC_AHB1Periph |\ TELEMETRY_RCC_AHB1Periph |\ - TRAINER_RCC_AHB1Periph |\ AUDIO_RCC_AHB1Periph |\ HAPTIC_RCC_AHB1Periph |\ INTMODULE_RCC_AHB1Periph |\ @@ -177,7 +176,7 @@ void boardInit() pwrInit(); boardInitModulePorts(); - init_trainer(); + board_trainer_init(); battery_charge_init(); flysky_gimbal_init(); timersInit(); diff --git a/radio/src/targets/nv14/board.h b/radio/src/targets/nv14/board.h index 711951048..3b612127f 100644 --- a/radio/src/targets/nv14/board.h +++ b/radio/src/targets/nv14/board.h @@ -246,9 +246,6 @@ void hapticOn(uint32_t pwmPercent); #define DEBUG_BAUDRATE 115200 #define LUA_DEFAULT_BAUDRATE 115200 -extern uint8_t currentTrainerMode; -void checkTrainerSettings(); - // Touch panel driver bool touchPanelEventOccured(); struct TouchState touchPanelRead(); diff --git a/radio/src/targets/nv14/hal.h b/radio/src/targets/nv14/hal.h index 4d55ef115..5d357caed 100644 --- a/radio/src/targets/nv14/hal.h +++ b/radio/src/targets/nv14/hal.h @@ -515,7 +515,6 @@ #define HEARTBEAT_GPIO_PIN GPIO_Pin_12 // PD.12 // Trainer Port -#define TRAINER_RCC_AHB1Periph (RCC_AHB1Periph_GPIOD) #define TRAINER_GPIO GPIOD #define TRAINER_IN_GPIO_PIN LL_GPIO_PIN_12 // PD.12 @@ -530,7 +529,6 @@ #define TRAINER_GPIO_AF LL_GPIO_AF_2 #define TRAINER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) - //BLUETOOTH #define BLUETOOTH_ON_RCC_AHB1Periph RCC_AHB1Periph_GPIOI #define BLUETOOTH_ON_GPIO GPIOI diff --git a/radio/src/targets/pl18/board.cpp b/radio/src/targets/pl18/board.cpp index e58096360..227a9796d 100644 --- a/radio/src/targets/pl18/board.cpp +++ b/radio/src/targets/pl18/board.cpp @@ -78,7 +78,6 @@ void delay_self(int count) ) #define RCC_AHB1PeriphOther (AUDIO_RCC_AHB1Periph |\ TELEMETRY_RCC_AHB1Periph |\ - TRAINER_RCC_AHB1Periph |\ HAPTIC_RCC_AHB1Periph |\ INTMODULE_RCC_AHB1Periph |\ EXTMODULE_RCC_AHB1Periph \ @@ -127,7 +126,7 @@ void boardInit() pwrInit(); boardInitModulePorts(); - init_trainer(); + board_trainer_init(); battery_charge_init(); flysky_gimbal_init(); timersInit(); diff --git a/radio/src/targets/pl18/board.h b/radio/src/targets/pl18/board.h index 71feb1ee3..a64903091 100644 --- a/radio/src/targets/pl18/board.h +++ b/radio/src/targets/pl18/board.h @@ -226,9 +226,6 @@ void hapticOn(uint32_t pwmPercent); #define DEBUG_BAUDRATE 115200 #define LUA_DEFAULT_BAUDRATE 115200 -extern uint8_t currentTrainerMode; -void checkTrainerSettings(); - // Touch panel driver bool touchPanelEventOccured(); struct TouchState touchPanelRead(); diff --git a/radio/src/targets/pl18/hal.h b/radio/src/targets/pl18/hal.h index c8ecc04f8..5c297d40a 100644 --- a/radio/src/targets/pl18/hal.h +++ b/radio/src/targets/pl18/hal.h @@ -653,7 +653,6 @@ #define EXTMODULE_RX_INVERTED() EXTMODULE_RX_INVERT_GPIO->BSRRL = EXTMODULE_RX_INVERT_GPIO_PIN // Trainer Port -#define TRAINER_RCC_AHB1Periph (RCC_AHB1Periph_GPIOD) #define TRAINER_GPIO GPIOD #define TRAINER_IN_GPIO_PIN LL_GPIO_PIN_12 // PD.12 diff --git a/radio/src/targets/simu/module_drivers.cpp b/radio/src/targets/simu/module_drivers.cpp index 9c5d456df..38de66c59 100644 --- a/radio/src/targets/simu/module_drivers.cpp +++ b/radio/src/targets/simu/module_drivers.cpp @@ -45,18 +45,15 @@ void extmoduleSendNextFramePxx1(void const*, unsigned short) {} void extmoduleSendNextFrameSoftSerial(void const*, unsigned short, bool) {} void extmoduleSendNextFramePpm(void*, unsigned short, unsigned short, bool) {} -void init_trainer_ppm() {} -void stop_trainer_ppm() {} -void init_trainer_capture() {} -void stop_trainer_capture() {} +bool trainer_dsc_available() { return true; } +void trainer_init_dsc_out() {} +void trainer_init_dsc_in() {} +void trainer_stop_dsc() {} bool is_trainer_dsc_connected() { return false; } -void init_trainer_module_cppm() {} -void stop_trainer_module_cppm() {} - -void init_trainer_module_sbus() {} -void stop_trainer_module_sbus() {} +void trainer_init_module_cppm() {} +void trainer_stop_module_cppm() {} void init_intmodule_heartbeat() {} void stop_intmodule_heartbeat() {} @@ -188,6 +185,15 @@ const etx_module_port_t _external_ports[] = { .drv = { .serial = &_fakeSerialDriver }, .hw_def = nullptr, }, +#elif defined(TRAINER_MODULE_SBUS_USART) + // RX on HEARTBEAT + { + .port = ETX_MOD_PORT_UART, + .type = ETX_MOD_TYPE_SERIAL, + .dir_flags = ETX_MOD_DIR_RX, + .drv = { .serial = &_fakeSerialDriver }, + .hw_def = nullptr, + }, #endif // Timer output on PPM { @@ -205,6 +211,16 @@ const etx_module_port_t _external_ports[] = { .drv = { .serial = &_fakeSerialDriver }, .hw_def = nullptr, }, +#if defined(TRAINER_MODULE_CPPM_TIMER) + // Timer input on HEARTBEAT + { + .port = ETX_MOD_PORT_TIMER, + .type = ETX_MOD_TYPE_TIMER, + .dir_flags = ETX_MOD_DIR_RX, + .drv = { .timer = nullptr }, + .hw_def = nullptr, + }, +#endif // TX/RX half-duplex on S.PORT { .port = ETX_MOD_PORT_SPORT, diff --git a/radio/src/targets/simu/opentxsimulator.cpp b/radio/src/targets/simu/opentxsimulator.cpp index e89741b62..2d0926286 100644 --- a/radio/src/targets/simu/opentxsimulator.cpp +++ b/radio/src/targets/simu/opentxsimulator.cpp @@ -242,7 +242,6 @@ void OpenTxSimulator::setTrim(unsigned int idx, int value) void OpenTxSimulator::setTrainerInput(unsigned int inputNumber, int16_t value) { static unsigned dim = DIM(trainerInput); - //setTrainerTimeout(100); if (inputNumber < dim) trainerInput[inputNumber] = qMin(qMax((int16_t)-512, value), (int16_t)512); } @@ -412,7 +411,7 @@ void OpenTxSimulator::lcdFlushed() void OpenTxSimulator::setTrainerTimeout(uint16_t ms) { - trainerInputValidityTimer = ms; + trainerSetTimer(ms); } void OpenTxSimulator::sendTelemetry(const QByteArray data) diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp index 9580c1031..b2264d3db 100644 --- a/radio/src/targets/taranis/board.cpp +++ b/radio/src/targets/taranis/board.cpp @@ -39,6 +39,7 @@ #include "timers_driver.h" #include "dataconstants.h" +#include "trainer.h" #if defined(FLYSKY_GIMBAL) #include "flysky_gimbal_driver.h" @@ -75,7 +76,6 @@ void boardInit() EXTMODULE_RCC_AHB1Periph | TELEMETRY_RCC_AHB1Periph | SPORT_UPDATE_RCC_AHB1Periph | - TRAINER_RCC_AHB1Periph | BT_RCC_AHB1Periph | USB_CHARGER_RCC_AHB1Periph, ENABLE); @@ -105,7 +105,8 @@ void boardInit() board_set_bor_level(); #endif - init_trainer(); + board_trainer_init(); + // Sets 'hardwareOption.pcbrev' as well pwrInit(); boardInitModulePorts(); @@ -118,6 +119,7 @@ void boardInit() (defined(INTERNAL_MODULE_PXX1) || defined(INTERNAL_MODULE_PXX2)) pulsesSetModuleInitCb(_intmodule_heartbeat_init); pulsesSetModuleDeInitCb(_intmodule_heartbeat_deinit); + trainerSetChangeCb(_intmodule_heartbeat_trainer_hook); #endif // #if defined(AUTOUPDATE) diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h index 85d9c6f21..6f5ae9e96 100644 --- a/radio/src/targets/taranis/hal.h +++ b/radio/src/targets/taranis/hal.h @@ -2119,10 +2119,8 @@ #endif // Trainer Port -#define HARDWARE_TRAINER_EXTERNAL_MODULE #if defined(PCBXLITES) || defined(PCBX9LITE) // on these 2 radios the trainer port already uses DMA1_Stream6, we won't use the DMA - #define TRAINER_RCC_AHB1Periph RCC_AHB1Periph_GPIOD #define TRAINER_GPIO GPIOD #define TRAINER_IN_GPIO_PIN LL_GPIO_PIN_13 // PD.13 #define TRAINER_IN_TIMER_Channel LL_TIM_CHANNEL_CH2 @@ -2139,11 +2137,9 @@ #define TRAINER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #elif defined(PCBXLITE) #define TRAINER_TIMER TIM4 - #define TRAINER_RCC_AHB1Periph 0 #define TRAINER_TIMER_IRQn TIM4_IRQn #define TRAINER_TIMER_IRQHandler TIM4_IRQHandler #else - #define TRAINER_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1) #define TRAINER_GPIO GPIOC #define TRAINER_IN_GPIO_PIN LL_GPIO_PIN_8 // PC.08 #define TRAINER_IN_TIMER_Channel LL_TIM_CHANNEL_CH3 @@ -2373,8 +2369,8 @@ // Trainer / Trainee from the module bay #if defined(PCBX9LITE) || defined(PCBXLITE) || defined(RADIO_X9DP2019) || \ - defined(PCBX7ACCESS) || defined(RADIO_ZORRO) || defined(RADIO_POCKET) || defined(RADIO_TX12MK2) || defined(RADIO_BOXER) || defined(RADIO_MT12) - #define TRAINER_MODULE_CPPM + defined(PCBX7ACCESS) || defined(RADIO_ZORRO) || defined(RADIO_POCKET) || \ + defined(RADIO_TX12MK2) || defined(RADIO_BOXER) || defined(RADIO_MT12) #define TRAINER_MODULE_CPPM_TIMER TIM3 #define TRAINER_MODULE_CPPM_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define TRAINER_MODULE_CPPM_GPIO EXTMODULE_USART_GPIO @@ -2385,10 +2381,8 @@ #if defined(PCBX9LITE) || defined(PCBXLITE) #define TRAINER_MODULE_CPPM_TIMER_IRQHandler TIM3_IRQHandler #endif - #define TRAINER_MODULE_SBUS #elif defined(INTMODULE_HEARTBEAT_GPIO) && defined(HARDWARE_EXTERNAL_MODULE) // Trainer CPPM input on heartbeat pin - #define TRAINER_MODULE_CPPM #define TRAINER_MODULE_CPPM_TIMER TRAINER_TIMER #define TRAINER_MODULE_CPPM_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define TRAINER_MODULE_CPPM_GPIO INTMODULE_HEARTBEAT_GPIO @@ -2397,7 +2391,6 @@ #define TRAINER_MODULE_CPPM_TIMER_IRQn TRAINER_TIMER_IRQn #define TRAINER_MODULE_CPPM_GPIO_AF GPIO_AF_TIM3 // Trainer SBUS input on heartbeat pin - #define TRAINER_MODULE_SBUS #define TRAINER_MODULE_SBUS_USART USART6 #define TRAINER_MODULE_SBUS_GPIO_AF GPIO_AF_USART6 #define TRAINER_MODULE_SBUS_GPIO INTMODULE_HEARTBEAT_GPIO diff --git a/radio/src/tasks/mixer_task.cpp b/radio/src/tasks/mixer_task.cpp index eb1cca21b..8b922950f 100644 --- a/radio/src/tasks/mixer_task.cpp +++ b/radio/src/tasks/mixer_task.cpp @@ -122,10 +122,8 @@ constexpr uint8_t MIXER_MAX_PERIOD = MAX_REFRESH_RATE / 1000 /*ms*/; void execMixerFrequentActions() { -#if defined(SBUS_TRAINER) // SBUS trainer processSbusInput(); -#endif #if defined(IMU) gyro.wakeup(); diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp index 36fb1eb8a..891ddcca3 100644 --- a/radio/src/telemetry/multi.cpp +++ b/radio/src/telemetry/multi.cpp @@ -29,6 +29,7 @@ #include "hitec.h" #include "hott.h" #include "mlink.h" +#include "trainer.h" constexpr int32_t MULTI_DESIRED_VERSION = (1 << 24) | (3 << 16) | (3 << 8) | 0; #define MULTI_CHAN_BITS 11 @@ -306,8 +307,7 @@ static void processMultiRxChannels(const uint8_t * data, uint8_t len) break; } - if (ch == maxCh) - trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT; + if (ch == maxCh) { trainerResetTimer(); } } #endif diff --git a/radio/src/tests/mixer.cpp b/radio/src/tests/mixer.cpp index 10121b929..e001f9e2b 100644 --- a/radio/src/tests/mixer.cpp +++ b/radio/src/tests/mixer.cpp @@ -848,7 +848,7 @@ TEST(Trainer, UnpluggedTest) g_model.mixData[0].weight = 100; g_model.mixData[0].delayUp = 50; g_model.mixData[0].delayDown = 50; - trainerInputValidityTimer = 0; + trainerSetTimer(0); trainerInput[0] = 1024; CHECK_DELAY(0, 5000); } diff --git a/radio/src/tests/module_ports.cpp b/radio/src/tests/module_ports.cpp index 9302d5961..5a6ebdc5b 100644 --- a/radio/src/tests/module_ports.cpp +++ b/radio/src/tests/module_ports.cpp @@ -22,6 +22,7 @@ #include "gtests.h" #include "hal/module_driver.h" #include "hal/module_port.h" +#include "hal/serial_driver.h" #include "pulses/modules_constants.h" #include "pulses/pulses.h" #include "translations.h" @@ -38,7 +39,8 @@ TEST(ports, softserialFallback) }; bool has_softserial = modulePortFind(EXTERNAL_MODULE, ETX_MOD_TYPE_SERIAL, - ETX_MOD_PORT_SPORT_INV, ETX_Pol_Inverted); + ETX_MOD_PORT_SPORT_INV, ETX_Pol_Inverted, + ETX_Dir_RX); if (has_softserial) { auto mod_st = modulePortInitSerial(EXTERNAL_MODULE, ETX_MOD_PORT_SPORT, &serialCfg, false); diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index 57cea89cb..e5c349766 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -19,14 +19,37 @@ * GNU General Public License for more details. */ -#include "opentx.h" #include "hal/trainer_driver.h" -#include "heartbeat_driver.h" + +#include "opentx.h" + +// Timer gets decremented in per10ms() +#define TRAINER_IN_VALID_TIMEOUT 100 // 1s int16_t trainerInput[MAX_TRAINER_CHANNELS]; uint8_t trainerInputValidityTimer; uint8_t currentTrainerMode = 0xff; +void (*_on_change_cb)(uint8_t, uint8_t) = nullptr; + +// Active signal received +bool isTrainerValid() { return trainerInputValidityTimer != 0; } + +void trainerResetTimer() +{ + trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT; +} + +void trainerDecTimer() +{ + if (trainerInputValidityTimer) trainerInputValidityTimer--; +} + +void trainerSetTimer(uint16_t t) +{ + trainerInputValidityTimer = t; +} + enum { TRAINER_NOT_CONNECTED = 0, TRAINER_CONNECTED, @@ -67,46 +90,35 @@ void checkTrainerSignalWarning() } } +static void trainer_init_module_sbus(); +static void trainer_stop_module_sbus(); +static int sbus_trainer_get_byte(uint8_t* data); + void stopTrainer() { switch (currentTrainerMode) { case TRAINER_MODE_MASTER_TRAINER_JACK: - stop_trainer_capture(); - break; - case TRAINER_MODE_SLAVE: - stop_trainer_ppm(); + trainer_stop_dsc(); break; -#if defined(SBUS_TRAINER) case TRAINER_MODE_MASTER_SERIAL: sbusSetGetByte(nullptr); break; -#endif -#if defined(TRAINER_MODULE_CPPM) case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: - stop_trainer_module_cppm(); + trainer_stop_module_cppm(); break; -#endif -#if defined(TRAINER_MODULE_SBUS) case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: sbusSetGetByte(nullptr); - stop_trainer_module_sbus(); + trainer_stop_module_sbus(); break; -#endif } -#if defined(INTMODULE_HEARTBEAT_GPIO) && !defined(SIMU) && \ - (defined(TRAINER_MODULE_CPPM) || defined(TRAINER_MODULE_SBUS)) - if ((currentTrainerMode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE || - currentTrainerMode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) - && (isModulePXX2(INTERNAL_MODULE) || isModulePXX1(INTERNAL_MODULE))) { - init_intmodule_heartbeat(); + if (_on_change_cb) { + _on_change_cb(currentTrainerMode, 0xFF); } -#endif - currentTrainerMode = 0xFF; } @@ -119,43 +131,81 @@ void checkTrainerSettings() stopTrainer(); } - currentTrainerMode = requiredTrainerMode; - switch (requiredTrainerMode) { case TRAINER_MODE_MASTER_TRAINER_JACK: - init_trainer_capture(); + trainer_init_dsc_in(); break; case TRAINER_MODE_SLAVE: - init_trainer_ppm(); + trainer_init_dsc_out(); break; -#if defined(SBUS_TRAINER) case TRAINER_MODE_MASTER_SERIAL: sbusSetGetByte(sbusAuxGetByte); break; -#endif -#if defined(TRAINER_MODULE_CPPM) case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: - init_trainer_module_cppm(); + trainer_init_module_cppm(); break; -#endif -#if defined(TRAINER_MODULE_SBUS) case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: - init_trainer_module_sbus(); - sbusSetGetByte(trainerModuleSbusGetByte); + trainer_init_module_sbus(); + sbusSetGetByte(sbus_trainer_get_byte); break; -#endif } -#if defined(INTMODULE_HEARTBEAT_GPIO) && !defined(SIMU) && \ - (defined(TRAINER_MODULE_CPPM) || defined(TRAINER_MODULE_SBUS)) - if (requiredTrainerMode == TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE || - requiredTrainerMode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE) { - stop_intmodule_heartbeat(); + if (_on_change_cb) { + _on_change_cb(currentTrainerMode, requiredTrainerMode); } -#endif + currentTrainerMode = requiredTrainerMode; } } + +void trainerSetChangeCb(void (*changeCb)(uint8_t, uint8_t)) +{ + _on_change_cb = changeCb; +} + + +static const etx_serial_init sbusTrainerParams = { + .baudrate = SBUS_BAUDRATE, + .encoding = ETX_Encoding_8E2, + .direction = ETX_Dir_RX, + .polarity = ETX_Pol_Normal, +}; + +static etx_module_state_t* sbus_trainer_mod_st = nullptr; + +static void trainer_init_module_sbus() +{ + if (sbus_trainer_mod_st) return; + + sbus_trainer_mod_st = modulePortInitSerial(EXTERNAL_MODULE, ETX_MOD_PORT_UART, + &sbusTrainerParams, false); + + if (sbus_trainer_mod_st) { + modulePortSetPower(EXTERNAL_MODULE,true); + } +} + +static void trainer_stop_module_sbus() +{ + if (!sbus_trainer_mod_st) return; + modulePortDeInit(sbus_trainer_mod_st); + modulePortSetPower(EXTERNAL_MODULE,false); + sbus_trainer_mod_st = nullptr; +} + +static int sbus_trainer_get_byte(uint8_t* data) +{ + if (!sbus_trainer_mod_st) return 0; + + auto serial_driver = modulePortGetSerialDrv(sbus_trainer_mod_st->rx); + auto ctx = modulePortGetCtx(sbus_trainer_mod_st->rx); + + if (ctx) { + return serial_driver->getByte(ctx, data); + } + + return 0; +} diff --git a/radio/src/trainer.h b/radio/src/trainer.h index ddb2da9d2..28a2954b4 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -27,48 +27,21 @@ // Trainer input channels extern int16_t trainerInput[MAX_TRAINER_CHANNELS]; -// Timer gets decremented in per10ms() -#define TRAINER_IN_VALID_TIMEOUT 100 // 1s -extern uint8_t trainerInputValidityTimer; - extern uint8_t currentTrainerMode; -void checkTrainerSignalWarning(); -void checkTrainerSettings(); -void stopTrainer(); -void forceResetTrainerSettings(); bool isTrainerConnected(); +void checkTrainerSignalWarning(); -// Needs to be inlined to avoid slow function calls in ISR routines -inline void captureTrainerPulses(uint16_t capture) -{ - static uint16_t lastCapt = 0; - static int8_t channelNumber = -1; +bool isTrainerValid(); +void trainerResetTimer(); +void trainerDecTimer(); +void trainerSetTimer(uint16_t t); - uint16_t val = (uint16_t)(capture - lastCapt) / 2; - lastCapt = capture; +void checkTrainerSettings(); +void forceResetTrainerSettings(); +void stopTrainer(); - // We process trainerInput right here to make servo movement as smooth as possible - // while under trainee control - // - // G: Prioritize reset pulse. (Needed when less than 16 incoming pulses) - // - if (val > 4000 && val < 19000) { - channelNumber = 0; // triggered - } - else { - if (channelNumber >= 0 && channelNumber < MAX_TRAINER_CHANNELS) { - if (val > 800 && val < 2200) { - trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT; - trainerInput[channelNumber++] = - // +-500 != 512, but close enough. - (int16_t)(val - 1500) * (g_eeGeneral.PPM_Multiplier+10) / 10; - } - else { - channelNumber = -1; // not triggered - } - } - } -} +// Allows notifications on trainer setting change (old_mode, new_mode) +void trainerSetChangeCb(void (*changeCb)(uint8_t, uint8_t)); #endif // _TRAINER_H_