From db73e4cd875137910431c6b594e5b3abc12298fa Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 26 Jun 2022 09:24:35 +0200 Subject: [PATCH] Move AFHDS3 to new module interface --- radio/src/pulses/afhds3.cpp | 168 +++++++++++++++++++++++++++++- radio/src/pulses/afhds3.h | 14 ++- radio/src/pulses/pulses.cpp | 33 ++---- radio/src/pulses/pulses.h | 4 + radio/src/telemetry/telemetry.cpp | 19 +++- 5 files changed, 205 insertions(+), 33 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 4f4865384..acd39f920 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -24,14 +24,27 @@ #include "../definitions.h" #include +#include "pulses.h" +#include "telemetry/telemetry.h" +#include "mixer_scheduler.h" +#include "hal/module_driver.h" + +#if defined(INTMODULE_USART) +#include "intmodule_serial_driver.h" +#endif + +#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO) +#include "extmodule_serial_driver.h" +#else +#include "extmodule_driver.h" +#endif + #define FAILSAFE_HOLD 1 #define FAILSAFE_CUSTOM 2 #define MAX_RETRIES_AFHDS3 5 extern void processFlySkySensor(const uint8_t * packet, uint8_t type); -extern void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inverted); - namespace afhds3 { @@ -74,10 +87,15 @@ const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE PulsesData* AFHDS3PulsesData[EXTERNAL_MODULE + 1] = { nullptr, nullptr }; //friends function that can access telemetry parsing method -void processTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize) +void processTelemetryData(uint8_t data, uint8_t module) { + uint8_t* rxBuffer = getTelemetryRxBuffer(module); + uint8_t& rxBufferCount = getTelemetryRxBufferCount(module); + uint8_t maxSize = TELEMETRY_RX_PACKET_SIZE; + if (AFHDS3PulsesData[module]) { - AFHDS3PulsesData[module]->processTelemetryData(data, rxBuffer, rxBufferCount, maxSize); + AFHDS3PulsesData[module]->processTelemetryData(data, rxBuffer, + rxBufferCount, maxSize); } } @@ -711,4 +729,146 @@ uint8_t PulsesData::setFailSafe(int16_t* target) return (uint8_t) (MAX_CHANNELS); } +#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO) + +static const etx_serial_init extmoduleUsartParams = { + .baudrate = AFHDS3_BAUDRATE, + .parity = ETX_Parity_None, + .stop_bits = ETX_StopBits_One, + .word_length = ETX_WordLength_8, + .rx_enable = true, +}; + +static void* initExternal(uint8_t module) +{ + extmodulePulsesData.afhds3.init(module); + ExtmoduleSerialDriver.init(&extmoduleUsartParams); + + mixerSchedulerSetPeriod(module, AFHDS3_COMMAND_TIMEOUT * 1000 /* us */); + EXTERNAL_MODULE_ON(); + + return nullptr; } + +static void deinitExternal(void* context) +{ + EXTERNAL_MODULE_OFF(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0); + ExtmoduleSerialDriver.deinit(context); +} + +static void sendPulsesExternal(void* context) +{ + ExtmoduleSerialDriver.sendBuffer(context, extmodulePulsesData.afhds3.getData(), + extmodulePulsesData.afhds3.getSize()); +} + +#else // !defined(EXTMODULE_USART) || !defined(EXTMODULE_TX_INVERT_GPIO) + +static void* initExternal(uint8_t module) +{ + extmodulePulsesData.afhds3.init(module); + extmoduleSerialStart(); + + mixerSchedulerSetPeriod(module, AFHDS3_COMMAND_TIMEOUT * 1000 /* us */); + return nullptr; +} + +static void deinitExternal(void* context) +{ + (void)context; + extmoduleStop(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0); +} + +static void sendPulsesExternal(void* context) +{ + (void)context; + extmoduleSendNextFrameSoftSerial(extmodulePulsesData.afhds3.getData(), + extmodulePulsesData.afhds3.getSize(), + false); +} + +#endif + +static void setupPulsesExternal(void* context, int16_t* channels, uint8_t nChannels) +{ + (void)context; + (void)channels; + (void)nChannels; + extmodulePulsesData.afhds3.setupFrame(); +} + +etx_module_driver_t externalDriver = { + .protocol = PROTOCOL_CHANNELS_AFHDS3, + .init = initExternal, + .deinit = deinitExternal, + .setupPulses = setupPulsesExternal, + .sendPulses = sendPulsesExternal +}; + +#if defined(INTERNAL_MODULE_AFHDS3) + +static const etx_serial_init intmoduleUsartParams = { + .baudrate = 1500000, + .parity = ETX_Parity_None, + .stop_bits = ETX_StopBits_One, + .word_length = ETX_WordLength_8, + .rx_enable = true, +}; + +static void* initInternal(uint8_t module) +{ + (void)module; + + // serial port setup + intmodulePulsesData.afhds3.init(module); + void * ctx = IntmoduleSerialDriver.init(&intmoduleUsartParams); + mixerSchedulerSetPeriod(module, AFHDS3_COMMAND_TIMEOUT * 1000 /* us */); + INTERNAL_MODULE_ON(); + + return ctx; +} + +static void deinitInternal(void* context) +{ + (void)context; + + INTERNAL_MODULE_OFF(); + mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); + intmoduleStop(); +} + +static void setupPulsesInternal(void* context, int16_t* channels, uint8_t nChannels) +{ + (void)context; + (void)channels; + (void)nChannels; + intmodulePulsesData.afhds3.setupFrame(); +} + +static void sendPulsesInternal(void* context) +{ + IntmoduleSerialDriver.sendBuffer(context, intmodulePulsesData.afhds3.getData(), + intmodulePulsesData.afhds3.getSize()); +} + +static int getByteInternal(void* context, uint8_t* data) +{ + return IntmoduleSerialDriver.getByte(context, data); +} + +etx_module_driver_t internalDriver = { + .protocol = PROTOCOL_CHANNELS_AFHDS3, + .init = initInternal, + .deinit = deinitInternal, + .setupPulses = setupPulsesInternal, + .sendPulses = sendPulsesInternal, + .getByte = getByteInternal, +}; + +#endif + +} + + diff --git a/radio/src/pulses/afhds3.h b/radio/src/pulses/afhds3.h index 338d746fc..055d49236 100644 --- a/radio/src/pulses/afhds3.h +++ b/radio/src/pulses/afhds3.h @@ -31,6 +31,8 @@ #include #include "fifo.h" +#include "hal/module_driver.h" + #define AFHDS_MAX_PULSES 72 //max number of transitions measured so far 290 + 10% #define AFHDS_MAX_PULSES_TRANSITIONS 320 @@ -95,6 +97,7 @@ struct Data void reset() { #if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO) + ptr = pulses; #else pulsesSize = 0; #endif @@ -472,7 +475,7 @@ struct CommandFifo }; -void processTelemetryData(uint8_t module, uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); +void processTelemetryData(uint8_t date, uint8_t module); class PulsesData: public Data, CommandFifo { @@ -537,7 +540,7 @@ class PulsesData: public Data, CommandFifo void processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); //friendship declaration - use for passing telemetry - friend void processTelemetryData(uint8_t module, uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize); + friend void processTelemetryData(uint8_t data, uint8_t module); /** * Returns max power that currently can be set - use it to validate before synchronization of settings @@ -594,5 +597,12 @@ class PulsesData: public Data, CommandFifo */ ModuleVersion version; }; + +extern etx_module_driver_t externalDriver; + +#if defined(INTERNAL_MODULE_AFHDS3) +extern etx_module_driver_t internalDriver; +#endif + } /* Namespace ahfds3 */ diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index 00a08db83..b2732257c 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -23,7 +23,6 @@ #include "mixer_scheduler.h" #include "heartbeat_driver.h" -#include "io/frsky_pxx2.h" #include "pulses/pxx2.h" #include "pulses/flysky.h" @@ -430,6 +429,13 @@ static void enablePulsesInternalModule(uint8_t protocol) break; #endif +#if defined(INTERNAL_MODULE_AFHDS3) + case PROTOCOL_CHANNELS_AFHDS3: + internalModuleContext = afhds3::internalDriver.init(INTERNAL_MODULE); + internalModuleDriver = &afhds3::internalDriver; + break; +#endif + default: // internal module stopped, use default mixer period mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); @@ -630,10 +636,8 @@ void enablePulsesExternalModule(uint8_t protocol) #if defined(AFHDS3) case PROTOCOL_CHANNELS_AFHDS3: - // convert to serial module interface - extmodulePulsesData.afhds3.init(EXTERNAL_MODULE); - extmoduleSerialStart(); - mixerSchedulerSetPeriod(EXTERNAL_MODULE, AFHDS3_COMMAND_TIMEOUT * 1000 /* us */); + externalModuleContext = afhds3::externalDriver.init(EXTERNAL_MODULE); + externalModuleDriver = &afhds3::externalDriver; break; #endif @@ -710,12 +714,6 @@ bool setupPulsesExternalModule(uint8_t protocol) return true; #endif -#if defined(AFHDS3) - case PROTOCOL_CHANNELS_AFHDS3: - extmodulePulsesData.afhds3.setupFrame(); - return true; -#endif - case PROTOCOL_CHANNELS_DSMP: setupPulsesLemonDSMP(); return true; @@ -750,19 +748,6 @@ void extmoduleSendNextFrame() break; #endif -#if defined(AFHDS3) - case PROTOCOL_CHANNELS_AFHDS3: -#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO) - extmoduleSendBuffer(extmodulePulsesData.afhds3.getData(), - extmodulePulsesData.afhds3.getSize()); -#else - extmoduleSendNextFrameSoftSerial(extmodulePulsesData.afhds3.getData(), - extmodulePulsesData.afhds3.getSize(), - false); -#endif - break; -#endif - #if defined(SBUS) || defined(DSM2) || defined(MULTIMODULE) case PROTOCOL_CHANNELS_SBUS: extmoduleSendNextFrameSoftSerial( diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index fea71062d..384747fb5 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -172,6 +172,10 @@ union InternalModulePulsesData { CrossfirePulsesData crossfire; #endif +#if defined(INTERNAL_MODULE_AFHDS3) + afhds3::PulsesData afhds3; +#endif + #if defined(INTERNAL_MODULE_PPM) PpmPulsesData ppm; #endif diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 0a2c58bec..bd4a7a8c1 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -218,7 +218,7 @@ void processTelemetryData(uint8_t data) #if defined(AFHDS3) if (telemetryProtocol == PROTOCOL_TELEMETRY_AFHDS3) { - afhds3::processTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer, telemetryRxBufferCount, TELEMETRY_RX_PACKET_SIZE); + afhds3::processTelemetryData(data, EXTERNAL_MODULE); return; } #endif @@ -312,7 +312,7 @@ static void pollIntCrossfire() } #endif -#if defined(PCBNV14) +#if defined(INTERNAL_MODULE_AFHDS2A) static void processFlySkyTelemetryData(uint8_t data, uint8_t idx) { (void)idx; @@ -325,6 +325,13 @@ static void pollIntAFHDS2A() } #endif +#if defined(INTERNAL_MODULE_AFHDS3) +static void pollIntAFHDS3() +{ + pollIntTelemetry(afhds3::processTelemetryData); +} +#endif + static void pollExtTelemetry() { uint8_t data; @@ -377,12 +384,18 @@ void telemetryWakeup() pollIntCrossfire(); } #endif -#if defined(PCBNV14) +#if defined(INTERNAL_MODULE_AFHDS2A) //! moduleUpdateActive(INTERNAL_MODULE) && if (isModuleAFHDS2A(INTERNAL_MODULE)) { pollIntAFHDS2A(); } #endif +#if defined(INTERNAL_MODULE_AFHDS3) + //! moduleUpdateActive(INTERNAL_MODULE) && + if (isModuleAFHDS3(INTERNAL_MODULE)) { + pollIntAFHDS3(); + } +#endif // Poll external / S.PORT telemetry // TODO: how to switch this OFF ???