From fe2266fb70c59ae3fcb4e9835c85a1e1bb54d81f Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sat, 20 Nov 2021 18:30:45 +0100 Subject: [PATCH] AFHDS2 and PXX1 serial module drivers --- radio/src/pulses/afhds2.cpp | 64 ++++++++++++++++-- radio/src/pulses/pulses.cpp | 52 ++------------- radio/src/pulses/pxx1.cpp | 66 +++++++++++++++++++ radio/src/pulses/pxx1.h | 4 ++ .../arm/stm32/intmodule_serial_driver.cpp | 17 +---- .../arm/stm32/intmodule_serial_driver.h | 2 +- 6 files changed, 137 insertions(+), 68 deletions(-) diff --git a/radio/src/pulses/afhds2.cpp b/radio/src/pulses/afhds2.cpp index 79aad0b0e..f0cf48fab 100644 --- a/radio/src/pulses/afhds2.cpp +++ b/radio/src/pulses/afhds2.cpp @@ -21,7 +21,18 @@ #include "afhds2.h" #include "flysky.h" + #include "intmodule_serial_driver.h" +#include "mixer_scheduler.h" + +afhds2::afhds2() { + // TODO Auto-generated constructor stub + +} + +afhds2::~afhds2() { + // TODO Auto-generated destructor stub +} etx_serial_init afhds2SerialInitParams = { .baudrate = INTMODULE_USART_AFHDS2_BAUDRATE, @@ -33,12 +44,57 @@ etx_serial_init afhds2SerialInitParams = { .on_error = intmoduleFifoError, }; -afhds2::afhds2() { - // TODO Auto-generated constructor stub +static void* afhds2Init(uint8_t module) +{ + (void)module; + // serial port setup + resetPulsesAFHDS2(); + intmoduleFifo.clear(); + IntmoduleSerialDriver.init(&afhds2SerialInitParams); + + // mixer setup + mixerSchedulerSetPeriod(INTERNAL_MODULE, AFHDS2_PERIOD); + + return nullptr; } -afhds2::~afhds2() { - // TODO Auto-generated destructor stub +static void afhds2DeInit(void* context) +{ + (void)context; + + intmoduleFifo.clear(); + IntmoduleSerialDriver.deinit(); + + // mixer setup + mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); } +static void afhds2SetupPulses(void* context) +{ + (void)context; + + ModuleSyncStatus& status = getModuleSyncStatus(INTERNAL_MODULE); + mixerSchedulerSetPeriod(INTERNAL_MODULE, status.isValid() + ? status.getAdjustedRefreshRate() + : AFHDS2_PERIOD); + status.invalidate(); + setupPulsesAFHDS2(); +} + +static void afhds2SendPulses(void* context) +{ + (void)context; + + uint8_t* data = (uint8_t*)intmodulePulsesData.flysky.pulses; + uint16_t size = intmodulePulsesData.flysky.ptr - data; + IntmoduleSerialDriver.sendBuffer(data, size); +} + +etx_module_driver_t Afhds2InternalDriver = { + .protocol = PROTOCOL_CHANNELS_AFHDS2A, + .init = afhds2Init, + .deinit = afhds2DeInit, + .setupPulses = afhds2SetupPulses, + .sendPulses = afhds2SendPulses, +}; diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index dd1ae356c..60a25d81e 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -312,11 +312,8 @@ static void enablePulsesInternalModule(uint8_t protocol) #if defined(PXX1) && defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_SERIAL: - intmodulePxx1SerialStart(); -#if defined(INTMODULE_HEARTBEAT) - init_intmodule_heartbeat(); -#endif - mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); + internalModuleContext = Pxx1InternalSerialDriver.init(INTERNAL_MODULE); + internalModuleDriver = &Pxx1InternalSerialDriver; break; #endif @@ -350,13 +347,8 @@ static void enablePulsesInternalModule(uint8_t protocol) #if defined(AFHDS2) case PROTOCOL_CHANNELS_AFHDS2A: - // serial port setup - resetPulsesAFHDS2(); - intmoduleFifo.clear(); - IntmoduleSerialDriver.init(&afhds2SerialInitParams); - - // mixer setup - mixerSchedulerSetPeriod(INTERNAL_MODULE, AFHDS2_PERIOD); + internalModuleContext = Afhds2InternalDriver.init(INTERNAL_MODULE); + internalModuleDriver = &Afhds2InternalDriver; break; #endif @@ -386,31 +378,12 @@ bool setupPulsesInternalModule(uint8_t protocol) return true; #endif -#if defined(PXX1) && defined(INTMODULE_USART) - case PROTOCOL_CHANNELS_PXX1_SERIAL: - intmodulePulsesData.pxx_uart.setupFrame(INTERNAL_MODULE); - return true; -#endif - #if defined(PCBTARANIS) && defined(INTERNAL_MODULE_PPM) case PROTOCOL_CHANNELS_PPM: setupPulsesPPMInternalModule(); return true; #endif -#if defined(AFHDS2) - case PROTOCOL_CHANNELS_AFHDS2A: - { - ModuleSyncStatus& status = getModuleSyncStatus(INTERNAL_MODULE); - mixerSchedulerSetPeriod( - INTERNAL_MODULE, - status.isValid() ? status.getAdjustedRefreshRate() : AFHDS2_PERIOD); - status.invalidate(); - setupPulsesAFHDS2(); - return true; - } -#endif - default: return false; } @@ -448,26 +421,11 @@ void intmoduleSendNextFrame() break; #endif -#if defined(PXX1) -#if defined(INTMODULE_USART) - case PROTOCOL_CHANNELS_PXX1_SERIAL: - IntmoduleSerialDriver.sendBuffer(intmodulePulsesData.pxx_uart.getData(), - intmodulePulsesData.pxx_uart.getSize()); - break; -#else +#if defined(PXX1) && !defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_PULSES: intmoduleSendNextFramePxx1(intmodulePulsesData.pxx.getData(), intmodulePulsesData.pxx.getSize()); break; -#endif -#endif - -#if defined(AFHDS2) - case PROTOCOL_CHANNELS_AFHDS2A: { - uint8_t* data = (uint8_t*)intmodulePulsesData.flysky.pulses; - uint16_t size = intmodulePulsesData.flysky.ptr - data; - IntmoduleSerialDriver.sendBuffer(data, size); - } break; #endif } } diff --git a/radio/src/pulses/pxx1.cpp b/radio/src/pulses/pxx1.cpp index 8cefd849b..faf1cd469 100644 --- a/radio/src/pulses/pxx1.cpp +++ b/radio/src/pulses/pxx1.cpp @@ -21,6 +21,8 @@ #include "opentx.h" #include "pulses/pxx1.h" +#include "mixer_scheduler.h" +#include "heartbeat_driver.h" template void Pxx1Pulses::addFlag1(uint8_t module, uint8_t sendFailsafe) @@ -219,3 +221,67 @@ void Pxx1Pulses::setupFrame(uint8_t module) template class Pxx1Pulses >; template class Pxx1Pulses >; template class Pxx1Pulses; + +#if defined(INTMODULE_USART) +#include "intmodule_serial_driver.h" + +static const etx_serial_init pxx1SerialInit = { + .baudrate = INTMODULE_PXX1_SERIAL_BAUDRATE, + .parity = ETX_Parity_None, + .stop_bits = ETX_StopBits_One, + .word_length = ETX_WordLength_8, + .rx_enable = false, + .on_receive = intmoduleFifoReceive, + .on_error = intmoduleFifoError, +}; + +static void* pxx1InitInternal(uint8_t module) +{ + (void)module; + + IntmoduleSerialDriver.init(&pxx1SerialInit); + +#if defined(INTMODULE_HEARTBEAT) + init_intmodule_heartbeat(); +#endif + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); + + return nullptr; +} + +static void pxx1DeInitInternal(void* context) +{ + (void)context; + + mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); +#if defined(INTMODULE_HEARTBEAT) + stop_intmodule_heartbeat(); +#endif + IntmoduleSerialDriver.deinit(); +} + +static void pxx1SetupPulsesInternal(void* context, int16_t* channels, uint8_t nChannels) +{ + (void)context; + + // TODO: + (void)channels; + (void)nChannels; + intmodulePulsesData.pxx_uart.setupFrame(INTERNAL_MODULE); +} + +static void pxx1SendPulsesInternal(void* context) +{ + (void)context; + IntmoduleSerialDriver.sendBuffer(intmodulePulsesData.pxx_uart.getData(), + intmodulePulsesData.pxx_uart.getSize()); +} + +const etx_module_driver_t Pxx1InternalSerialDriver = { + .init = pxx1InitInternal, + .deinit = pxx1DeInitInternal, + .setupPulses = pxx1SetupPulsesInternal, + .sendPulses = pxx1SendPulsesInternal, +}; + +#endif diff --git a/radio/src/pulses/pxx1.h b/radio/src/pulses/pxx1.h index 6c2fd8d7b..40d219d55 100644 --- a/radio/src/pulses/pxx1.h +++ b/radio/src/pulses/pxx1.h @@ -25,6 +25,8 @@ #include "pxx.h" #include "crc.h" +#include "hal/module_driver.h" + class Pxx1CrcMixin { protected: void initCrc() @@ -166,4 +168,6 @@ typedef Pxx1Pulses UartPxx1Pulses; typedef Pxx1Pulses> PwmPxx1Pulses; typedef Pxx1Pulses> SerialPxx1Pulses; +extern const etx_module_driver_t Pxx1InternalSerialDriver; + #endif diff --git a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp index 05342e6c3..42759e4c5 100755 --- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp @@ -71,21 +71,6 @@ void intmoduleStop() intmodule_driver.on_error = nullptr; } -static const etx_serial_init pxx1SerialInit = { - .baudrate = INTMODULE_PXX1_SERIAL_BAUDRATE, - .parity = ETX_Parity_None, - .stop_bits = ETX_StopBits_One, - .word_length = ETX_WordLength_8, - .rx_enable = false, - .on_receive = intmoduleFifoReceive, - .on_error = intmoduleFifoError, -}; - -void intmodulePxx1SerialStart() -{ - IntmoduleSerialDriver.init(&pxx1SerialInit); -} - void intmoduleSerialStart(const etx_serial_init* params) { if (!params) return; @@ -227,7 +212,7 @@ void intmoduleWaitForTxCompleted() #endif } -etx_serial_driver_t IntmoduleSerialDriver = { +const etx_serial_driver_t IntmoduleSerialDriver = { .init = intmoduleSerialStart, .deinit = intmoduleStop, .sendByte = intmoduleSendByte, diff --git a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.h b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.h index 52f4e9f1e..b07ec6a63 100644 --- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.h +++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.h @@ -28,4 +28,4 @@ void intmoduleFifoReceive(uint8_t data); void intmoduleFifoError(); -extern etx_serial_driver_t IntmoduleSerialDriver; +extern const etx_serial_driver_t IntmoduleSerialDriver;