diff --git a/radio/src/pulses/multi.cpp b/radio/src/pulses/multi.cpp index fb588b734..7a0bcc28c 100644 --- a/radio/src/pulses/multi.cpp +++ b/radio/src/pulses/multi.cpp @@ -24,6 +24,7 @@ #include "io/multi_protolist.h" #include "telemetry/multi.h" +#include "mixer_scheduler.h" // for the MULTI protocol definition // see https://github.com/pascallanger/DIY-Multiprotocol-TX-Module @@ -249,6 +250,67 @@ void setupPulsesMultiInternalModule() intmodulePulsesData.multi.initFrame(); setupPulsesMulti(INTERNAL_MODULE); } + +static void* multiInit(uint8_t module) +{ + (void)module; + + // serial port setup + intmodulePulsesData.multi.initFrame(); + intmoduleFifo.clear(); + IntmoduleSerialDriver.init(&multiSerialInitParams); + + // mixer setup + mixerSchedulerSetPeriod(INTERNAL_MODULE, MULTIMODULE_PERIOD); + + // reset status + getMultiModuleStatus(INTERNAL_MODULE).failsafeChecked = false; + getMultiModuleStatus(INTERNAL_MODULE).flags = 0; + +#if defined(MULTI_PROTOLIST) + TRACE("enablePulsesInternalModule(): trigger scan"); + MultiRfProtocols::instance(INTERNAL_MODULE)->triggerScan(); + TRACE("counter = %d", moduleState[INTERNAL_MODULE].counter); +#endif + + return nullptr; +} + +static void multiDeInit(void* context) +{ + (void)context; + mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); + IntmoduleSerialDriver.deinit(); + //TODO: power OFF ??? +} + +static void multiSetupPulses(void* context, int16_t* channels, uint8_t nChannels) +{ + (void)context; + // TODO: + (void)channels; + (void)nChannels; + + intmodulePulsesData.multi.initFrame(); + setupPulsesMulti(INTERNAL_MODULE); +} + +static void multiSendPulses(void* context) +{ + (void)context; + IntmoduleSerialDriver.sendBuffer(intmodulePulsesData.multi.getData(), + intmodulePulsesData.multi.getSize()); +} + +#include "hal/module_driver.h" + +const etx_module_driver_t MultiInternalDriver = { + .protocol = PROTOCOL_CHANNELS_MULTIMODULE, + .init = multiInit, + .deinit = multiDeInit, + .setupPulses = multiSetupPulses, + .sendPulses = multiSendPulses +}; #endif void sendChannels(uint8_t moduleIdx) diff --git a/radio/src/pulses/multi.h b/radio/src/pulses/multi.h index 5721bdbe1..1b5f11fa3 100644 --- a/radio/src/pulses/multi.h +++ b/radio/src/pulses/multi.h @@ -24,6 +24,7 @@ #include "pulses_common.h" #include "hal/serial_driver.h" +#include "hal/module_driver.h" void multiPatchCustom(uint8_t moduleIdx); void convertEtxProtocolToMulti(int *protocol, int *subprotocol); @@ -45,5 +46,6 @@ class UartMultiPulses: public DataBuffer }; extern etx_serial_init multiSerialInitParams; +extern const etx_module_driver_t MultiInternalDriver; #endif diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index 5407d431d..44f5bdf52 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -24,7 +24,6 @@ #include "heartbeat_driver.h" #include "io/frsky_pxx2.h" -#include "io/multi_protolist.h" #include "pulses/pxx2.h" #include "pulses/flysky.h" @@ -40,6 +39,11 @@ #include "pulses/crossfire.h" #endif +#if defined(MULTIMODULE) +#include "io/multi_protolist.h" +#include "pulses/multi.h" +#endif + uint8_t s_pulses_paused = 0; ModuleState moduleState[NUM_MODULES]; InternalModulePulsesData intmodulePulsesData __DMA; @@ -324,24 +328,8 @@ static void enablePulsesInternalModule(uint8_t protocol) #if defined(INTERNAL_MODULE_MULTI) case PROTOCOL_CHANNELS_MULTIMODULE: - // TODO: use module interface - // serial port setup - intmodulePulsesData.multi.initFrame(); - intmoduleFifo.clear(); - IntmoduleSerialDriver.init(&multiSerialInitParams); - - // mixer setup - mixerSchedulerSetPeriod(INTERNAL_MODULE, MULTIMODULE_PERIOD); - - // reset status - getMultiModuleStatus(INTERNAL_MODULE).failsafeChecked = false; - getMultiModuleStatus(INTERNAL_MODULE).flags = 0; - -#if defined(MULTI_PROTOLIST) - TRACE("enablePulsesInternalModule(): trigger scan"); - MultiRfProtocols::instance(INTERNAL_MODULE)->triggerScan(); - TRACE("counter = %d", moduleState[INTERNAL_MODULE].counter); -#endif + internalModuleContext = MultiInternalDriver.init(INTERNAL_MODULE); + internalModuleDriver = &MultiInternalDriver; break; #endif @@ -409,13 +397,6 @@ bool setupPulsesInternalModule(uint8_t protocol) return true; #endif -#if defined(INTERNAL_MODULE_MULTI) - case PROTOCOL_CHANNELS_MULTIMODULE: - setupPulsesMultiInternalModule(); - mixerSchedulerSetPeriod(INTERNAL_MODULE, MULTIMODULE_PERIOD); - return true; -#endif - #if defined(AFHDS2) case PROTOCOL_CHANNELS_AFHDS2A: { @@ -480,13 +461,6 @@ void intmoduleSendNextFrame() #endif #endif -#if defined(INTERNAL_MODULE_MULTI) - case PROTOCOL_CHANNELS_MULTIMODULE: - IntmoduleSerialDriver.sendBuffer(intmodulePulsesData.multi.getData(), - intmodulePulsesData.multi.getSize()); - break; -#endif - #if defined(AFHDS2) case PROTOCOL_CHANNELS_AFHDS2A: { uint8_t* data = (uint8_t*)intmodulePulsesData.flysky.pulses;