diff --git a/src/html/elrs.css b/src/html/elrs.css index 5e96ed4594..75a05df124 100644 --- a/src/html/elrs.css +++ b/src/html/elrs.css @@ -348,6 +348,13 @@ body, input, select, textarea { } /* Custom code for ExpressLRS PWM Output table */ + .pwmpnl { + overflow-x: auto; + min-width: fit-content; + } + .pwmtbl table { + overflow-x: auto; + } .pwmtbl th { text-align: center; font-weight: bold; @@ -355,6 +362,10 @@ body, input, select, textarea { .pwmtbl td { text-align: center; } + .pwmitm { + min-width: 6em; + white-space: nowrap; + } /*==========================*/ diff --git a/src/html/index.html b/src/html/index.html index 789bd42f8b..6647f46f0f 100644 --- a/src/html/index.html +++ b/src/html/index.html @@ -187,11 +187,22 @@

PWM Output

  • Input: Input channel from the handset
  • Invert: Invert input channel position
  • 750us: Use half pulse width (494-1006us) with center 750us instead of 988-2012us
    • -
  • Failsafe: Absolute position to set the servo on failsafe +
  • Failsafe
    • diff --git a/src/html/scan.js b/src/html/scan.js index 95e79d2f6f..5855e1bc20 100644 --- a/src/html/scan.js +++ b/src/html/scan.js @@ -26,13 +26,15 @@ function getPwmFormData() { const invert = _(`pwm_${ch}_inv`).checked ? 1 : 0; const narrow = _(`pwm_${ch}_nar`).checked ? 1 : 0; const failsafeField = _(`pwm_${ch}_fs`); + const failsafeModeField = _(`pwm_${ch}_fsmode`); let failsafe = failsafeField.value; if (failsafe > 2011) failsafe = 2011; if (failsafe < 988) failsafe = 988; failsafeField.value = failsafe; + let failsafeMode = failsafeModeField.value; - const raw = (narrow << 19) | (mode << 15) | (invert << 14) | (inChannel << 10) | (failsafe - 988); - // console.log(`PWM ${ch} mode=${mode} input=${inChannel} fs=${failsafe} inv=${invert} nar=${narrow} raw=${raw}`); + const raw = (narrow << 19) | (mode << 15) | (invert << 14) | (inChannel << 10) | (failsafeMode << 20) | (failsafe - 988); + // console.log(`PWM ${ch} mode=${mode} input=${inChannel} fs=${failsafe} fsmode=${failsafeMode} inv=${invert} nar=${narrow} raw=${raw}`); outData.push(raw); ++ch; } @@ -41,7 +43,7 @@ function getPwmFormData() { function enumSelectGenerate(id, val, arOptions) { // Generate a ` + + const retVal = `
    -
    `); + ${failsafeModeSelect} +
    `); pinModes[index] = mode; }); htmlFields.push(''); @@ -138,6 +144,7 @@ function updatePwmSettings(arPwm) { _(`pwm_${index}_inv`).disabled = onoff; _(`pwm_${index}_nar`).disabled = onoff; _(`pwm_${index}_fs`).disabled = onoff; + _(`pwm_${index}_fsmode`).disabled = onoff; } arPwm.forEach((item,index)=>{ const pinMode = _(`pwm_${index}_mode`) @@ -161,7 +168,23 @@ function updatePwmSettings(arPwm) { pinModes[index] = pinMode.value; } pinMode.onchange(); + + // disable and hide the failsafe position field if not using the set-position failsafe mode + const failsafeMode = _(`pwm_${index}_fsmode`); + failsafeMode.onchange = () => { + const failsafeField = _(`pwm_${index}_fs`); + if (failsafeMode.value == 0) { + failsafeField.disabled = false; + failsafeField.style.display = 'block'; + } + else { + failsafeField.disabled = true; + failsafeField.style.display = 'none'; + } + }; + failsafeMode.onchange(); }); + // put some contraints on pinRx/Tx mode selects if (pinRxIndex !== undefined && pinTxIndex !== undefined) { const pinRxMode = _(`pwm_${pinRxIndex}_mode`); diff --git a/src/include/common.h b/src/include/common.h index ad2cb45522..3ff662f381 100644 --- a/src/include/common.h +++ b/src/include/common.h @@ -182,6 +182,13 @@ enum eServoOutputMode : uint8_t somPwm, // True PWM mode (NOT SUPPORTED) }; +enum eServoOutputFailsafeMode : uint8_t +{ + PWMFAILSAFE_SET_POSITION, // user customizable pulse value + PWMFAILSAFE_NO_PULSES, // stop pulsing + PWMFAILSAFE_LAST_POSITION, // continue to pulse last used value +}; + enum eSerialProtocol : uint8_t { PROTOCOL_CRSF, diff --git a/src/lib/helpers/helpers.h b/src/include/helpers.h similarity index 62% rename from src/lib/helpers/helpers.h rename to src/include/helpers.h index ea828fe555..2aea12c579 100644 --- a/src/lib/helpers/helpers.h +++ b/src/include/helpers.h @@ -1,5 +1,7 @@ #pragma once +#include "targets.h" + #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) #ifndef UNUSED @@ -9,33 +11,32 @@ class NullStream : public Stream { public: - int available(void) + int available() override { return 0; } - void flush(void) + void flush() override { - return; } - int peek(void) + int peek() override { return -1; } - int read(void) + int read() override { return -1; } - size_t write(uint8_t u_Data) + size_t write(uint8_t u_Data) override { UNUSED(u_Data); return 0x01; } - size_t write(const uint8_t *buffer, size_t size) + size_t write(const uint8_t *buffer, size_t size) override { UNUSED(buffer); return size; diff --git a/src/include/native.h b/src/include/native.h index ae0d49c494..ee23fadd02 100644 --- a/src/include/native.h +++ b/src/include/native.h @@ -34,7 +34,7 @@ class Stream // Print methods virtual size_t write(uint8_t c) = 0; - virtual size_t write(uint8_t *s, int l) = 0; + virtual size_t write(const uint8_t *s, size_t l) = 0; int print(const char *s) {return 0;} int print(uint8_t s) {return 0;} @@ -59,7 +59,7 @@ class HardwareSerial: public Stream { // Print methods size_t write(uint8_t c) {return 1;} - size_t write(uint8_t *s, int l) {return l;} + size_t write(const uint8_t *s, size_t l) {return l;} int print(const char *s) {return 0;} int print(uint8_t s) {return 0;} diff --git a/src/include/targets.h b/src/include/targets.h index d5c4fa3114..e828e6d00a 100644 --- a/src/include/targets.h +++ b/src/include/targets.h @@ -65,6 +65,7 @@ #ifndef HAS_THERMAL #define OPT_HAS_THERMAL false +#define OPT_HAS_THERMAL_LM75A false #elif !defined(OPT_HAS_THERMAL) #define OPT_HAS_THERMAL true #endif diff --git a/src/lib/BLE/devBLE.cpp b/src/lib/BLE/devBLE.cpp index f0fc6d9c1c..9a2d1c6e77 100644 --- a/src/lib/BLE/devBLE.cpp +++ b/src/lib/BLE/devBLE.cpp @@ -3,7 +3,8 @@ #if defined(PLATFORM_ESP32) #include "common.h" -#include "CRSF.h" +#include "crsf_protocol.h" +#include "handset.h" #include "POWERMGNT.h" #include "hwTimer.h" #include "logging.h" @@ -61,7 +62,7 @@ void BluetoothJoystickBegin() POWERMGNT::setPower(MinPower); Radio.End(); - CRSF::RCdataCallback = BluetoothJoystickUpdateValues; + handset->setRCDataCallback(BluetoothJoystickUpdateValues); BleGamepadConfiguration *gamepadConfig = new BleGamepadConfiguration(); gamepadConfig->setAutoReport(false); diff --git a/src/lib/BUTTON/devButton.cpp b/src/lib/BUTTON/devButton.cpp index c4c8dfdb67..4e98f3f0c7 100644 --- a/src/lib/BUTTON/devButton.cpp +++ b/src/lib/BUTTON/devButton.cpp @@ -4,7 +4,7 @@ #include "logging.h" #include "button.h" #include "config.h" -#include "CRSF.h" +#include "handset.h" #ifndef GPIO_BUTTON_INVERTED #define GPIO_BUTTON_INVERTED false @@ -92,7 +92,7 @@ static int timeout() return DURATION_NEVER; } #if defined(TARGET_TX) - if (CRSF::IsArmed()) + if (handset->IsArmed()) return MS_IN_USE; #else if (connectionState == connected) diff --git a/src/lib/Backpack/devBackpack.cpp b/src/lib/Backpack/devBackpack.cpp index 5db14e4a4f..77cec05b46 100644 --- a/src/lib/Backpack/devBackpack.cpp +++ b/src/lib/Backpack/devBackpack.cpp @@ -1,9 +1,10 @@ #include "targets.h" + #include "common.h" #include "device.h" #include "msp.h" #include "msptypes.h" -#include "CRSF.h" +#include "CRSFHandset.h" #include "config.h" #include "logging.h" @@ -34,9 +35,9 @@ bool lastRecordingState = false; devicesStop(); Radio.End(); hwTimer::stop(); - CRSF::End(); + handset->End(); - Stream *uplink = &CRSF::Port; + Stream *uplink = &CRSFHandset::Port; uint32_t baud = PASSTHROUGH_BAUD == -1 ? BACKPACK_LOGGING_BAUD : PASSTHROUGH_BAUD; // get ready for passthrough @@ -52,25 +53,25 @@ bool lastRecordingState = false; } else { - CRSF::Port.begin(baud, SERIAL_8N1, 44, 43); // pins are configured as 44 and 43 - CRSF::Port.setTxBufferSize(1024); - CRSF::Port.setRxBufferSize(16384); + CRSFHandset::Port.begin(baud, SERIAL_8N1, 44, 43); // pins are configured as 44 and 43 + CRSFHandset::Port.setTxBufferSize(1024); + CRSFHandset::Port.setRxBufferSize(16384); } #else - CRSF::Port.begin(baud, SERIAL_8N1, 3, 1); // default pin configuration 3 and 1 - CRSF::Port.setTxBufferSize(1024); - CRSF::Port.setRxBufferSize(16384); + CRSFHandset::Port.begin(baud, SERIAL_8N1, 3, 1); // default pin configuration 3 and 1 + CRSFHandset::Port.setTxBufferSize(1024); + CRSFHandset::Port.setRxBufferSize(16384); #endif } else { - CRSF::Port.begin(baud, SERIAL_8N1, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX); - CRSF::Port.setTxBufferSize(1024); - CRSF::Port.setRxBufferSize(16384); + CRSFHandset::Port.begin(baud, SERIAL_8N1, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX); + CRSFHandset::Port.setTxBufferSize(1024); + CRSFHandset::Port.setRxBufferSize(16384); } disableLoopWDT(); - auto backpack = (HardwareSerial*)TxBackpack; + const auto backpack = (HardwareSerial*)TxBackpack; if (baud != BACKPACK_LOGGING_BAUD) { backpack->begin(PASSTHROUGH_BAUD, SERIAL_8N1, GPIO_PIN_DEBUG_RX, GPIO_PIN_DEBUG_TX); @@ -201,10 +202,10 @@ static void AuxStateToMSPOut() return; } - uint8_t auxNumber = (config.GetDvrAux() - 1) / 2 + 4; - uint8_t auxInverted = (config.GetDvrAux() + 1) % 2; + const uint8_t auxNumber = (config.GetDvrAux() - 1) / 2 + 4; + const uint8_t auxInverted = (config.GetDvrAux() + 1) % 2; - bool recordingState = CRSF_to_BIT(ChannelData[auxNumber]) ^ auxInverted; + const bool recordingState = CRSF_to_BIT(ChannelData[auxNumber]) ^ auxInverted; if (recordingState == lastRecordingState) { @@ -213,17 +214,7 @@ static void AuxStateToMSPOut() } lastRecordingState = recordingState; - uint16_t delay = 0; - - if (recordingState) - { - delay = GetDvrDelaySeconds(config.GetDvrStartDelay()); - } - - if (!recordingState) - { - delay = GetDvrDelaySeconds(config.GetDvrStopDelay()); - } + const uint16_t delay = GetDvrDelaySeconds(recordingState ? config.GetDvrStartDelay() : config.GetDvrStopDelay()); mspPacket_t packet; packet.reset(); @@ -244,7 +235,7 @@ void crsfTelemToMSPOut(uint8_t *data) // Backpack telem is off return; } - + mspPacket_t packet; packet.reset(); packet.makeCommand(); @@ -256,7 +247,7 @@ void crsfTelemToMSPOut(uint8_t *data) ERRLN("CRSF frame exceeds max length"); return; } - + for (uint8_t i = 0; i < size; ++i) { packet.addByte(data[i]); @@ -280,8 +271,7 @@ static void initialize() // Rely on event() to boot } #endif - - CRSF::RCdataCallback = AuxStateToMSPOut; + handset->setRCDataCallback(AuxStateToMSPOut); } static int start() diff --git a/src/lib/Baro/devBaro.cpp b/src/lib/Baro/devBaro.cpp index 9a77fb9c0f..6c1b2bc08c 100644 --- a/src/lib/Baro/devBaro.cpp +++ b/src/lib/Baro/devBaro.cpp @@ -2,6 +2,7 @@ #if defined(HAS_BARO) +#include "CRSF.h" #include "logging.h" #include "telemetry.h" #include "baro_spl06.h" diff --git a/src/lib/Baro/devBaro.h b/src/lib/Baro/devBaro.h index 4f148200e6..7b50eaec97 100644 --- a/src/lib/Baro/devBaro.h +++ b/src/lib/Baro/devBaro.h @@ -2,7 +2,6 @@ #include "common.h" #include "device.h" -#include "devCRSF.h" #if defined(USE_ANALOG_VBAT) #include "devAnalogVbat.h" #endif diff --git a/src/lib/CONFIG/config.h b/src/lib/CONFIG/config.h index ad0992526e..eabb304743 100644 --- a/src/lib/CONFIG/config.h +++ b/src/lib/CONFIG/config.h @@ -187,7 +187,8 @@ typedef union { inverted:1, // invert channel output mode:4, // Output mode (eServoOutputMode) narrow:1, // Narrow output mode (half pulse width) - unused:12; // FUTURE: When someone complains "everyone" uses inverted polarity PWM or something :/ + failsafeMode:2, // failsafe output mode (eServoOutputFailsafeMode) + unused:10; // FUTURE: When someone complains "everyone" uses inverted polarity PWM or something :/ } val; uint32_t raw; } rx_config_pwm_t; diff --git a/src/lib/CRSF/CRSF.h b/src/lib/CRSF/CRSF.h deleted file mode 100644 index 05adde5cb8..0000000000 --- a/src/lib/CRSF/CRSF.h +++ /dev/null @@ -1,149 +0,0 @@ -#ifndef H_CRSF -#define H_CRSF - -#include "targets.h" -#include "crsf_protocol.h" -#ifndef TARGET_NATIVE -#include "HardwareSerial.h" -#endif -#include "common.h" -#include "msp.h" -#include "msptypes.h" -#include "LowPassFilter.h" -#include "../CRC/crc.h" -#include "telemetry_protocol.h" - -#ifdef PLATFORM_ESP32 -#include "esp32-hal-uart.h" -#include -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "driver/uart.h" -#include "driver/gpio.h" -#endif - -class CRSF -{ - -public: - /////Variables///// - - static volatile crsfPayloadLinkstatistics_s LinkStatistics; // Link Statisitics Stored as Struct - - static void GetDeviceInformation(uint8_t *frame, uint8_t fieldCount); - static void SetMspV2Request(uint8_t *frame, uint16_t function, uint8_t *payload, uint8_t payloadLength); - static void SetHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t destAddr); - static void SetExtendedHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t senderAddr, uint8_t destAddr); - static uint32_t VersionStrToU32(const char *verStr); - -#ifdef CRSF_TX_MODULE - static HardwareSerial Port; - static Stream *PortSecondary; // A second UART used to mirror telemetry out on the TX, not read from - - static void (*disconnected)(); - static void (*connected)(); - - static void (*RecvModelUpdate)(); - static void (*RecvParameterUpdate)(uint8_t type, uint8_t index, uint8_t arg); - static void (*RCdataCallback)(); - - // The model ID as received from the Transmitter - static uint8_t modelId; - static bool ForwardDevicePings; // true if device pings should be forwarded OTA - static bool elrsLUAmode; - - /// UART Handling /// - static uint32_t GoodPktsCountResult; // need to latch the results - static uint32_t BadPktsCountResult; // need to latch the results - - static void Begin(); //setup timers etc - static void End(); //stop timers etc - - static bool IsArmed() { return CRSF_to_BIT(ChannelData[4]); } // AUX1 - static void ICACHE_RAM_ATTR makeLinkStatisticsPacket(uint8_t buffer[LinkStatisticsFrameLength + 4]); - static void ICACHE_RAM_ATTR sendTelemetryToTX(uint8_t *data); - - static void packetQueueExtended(uint8_t type, void *data, uint8_t len); - - ///// Variables for OpenTX Syncing ////////////////////////// - #define OpenTXsyncPacketInterval 200 // in ms - static void ICACHE_RAM_ATTR setSyncParams(int32_t PacketInterval); - static void ICACHE_RAM_ATTR JustSentRFpacket(); - static void ICACHE_RAM_ATTR sendSyncPacketToTX(); - static void disableOpentxSync(); - static void enableOpentxSync(); - - static void handleUARTin(); - - static uint8_t getModelID() { return modelId; } - - static void GetMspMessage(uint8_t **data, uint8_t *len); - static void UnlockMspMessage(); - static void AddMspMessage(const uint8_t length, uint8_t* data); - static void AddMspMessage(mspPacket_t* packet); - static void ResetMspQueue(); - static uint32_t OpenTXsyncLastSent; - static uint8_t GetMaxPacketBytes() { return maxPacketBytes; } - static uint32_t GetCurrentBaudRate() { return UARTrequestedBaud; } - - static uint32_t ICACHE_RAM_ATTR GetRCdataLastRecv(); - static void ICACHE_RAM_ATTR RcPacketToChannelsData(); - - ///////////////////////////////////////////////////////////// - static bool CRSFstate; - -private: - - static inBuffer_U inBuffer; - - /// OpenTX mixer sync /// - static int32_t RequestedRCpacketInterval; - static volatile uint32_t RCdataLastRecv; - static volatile uint32_t dataLastRecv; - static volatile int32_t OpenTXsyncOffset; - static volatile int32_t OpenTXsyncWindow; - static volatile int32_t OpenTXsyncWindowSize; - static uint32_t OpenTXsyncOffsetSafeMargin; - static bool OpentxSyncActive; - static uint8_t CRSFoutBuffer[CRSF_MAX_PACKET_LEN]; - - /// UART Handling /// - static uint8_t SerialInPacketLen; // length of the CRSF packet as measured - static uint8_t SerialInPacketPtr; // index where we are reading/writing - static bool CRSFframeActive; //since we get a copy of the serial data use this flag to know when to ignore it - static uint32_t GoodPktsCount; - static uint32_t BadPktsCount; - static uint32_t UARTwdtLastChecked; - static uint8_t maxPacketBytes; - static uint8_t maxPeriodBytes; - static uint32_t TxToHandsetBauds[7]; - static uint8_t UARTcurrentBaudIdx; - static uint32_t UARTrequestedBaud; - static uint8_t MspData[ELRS_MSP_BUFFER]; - static uint8_t MspDataLength; - #if defined(PLATFORM_ESP32) - static bool UARTinverted; - #endif - - static void ICACHE_RAM_ATTR adjustMaxPacketSize(); - static void duplex_set_RX(); - static void duplex_set_TX(); - static bool ProcessPacket(); - static void handleUARTout(); - static bool UARTwdt(); - static uint32_t autobaud(); - static void flush_port_input(void); -#endif -#if defined(CRSF_RX_MODULE) -public: - static void updateUplinkPower(uint8_t uplinkPower); - static bool clearUpdatedUplinkPower(); - -private: - static bool HasUpdatedUplinkPower; -#endif -}; - -extern GENERIC_CRC8 crsf_crc; - -#endif diff --git a/src/lib/CRSF/devCRSF.h b/src/lib/CRSF/devCRSF.h deleted file mode 100644 index 568bec2379..0000000000 --- a/src/lib/CRSF/devCRSF.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -#include "device.h" -#include "CRSF.h" - -extern device_t CRSF_device; diff --git a/src/lib/CRSF/devCRSF_tx.cpp b/src/lib/CRSF/devCRSF_tx.cpp deleted file mode 100644 index fe7aa8010b..0000000000 --- a/src/lib/CRSF/devCRSF_tx.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "targets.h" -#include "devCRSF.h" - -#ifdef CRSF_TX_MODULE - -#include "../POWERMGNT/POWERMGNT.h" - -static int start() -{ - CRSF::Begin(); -#if defined(DEBUG_TX_FREERUN) - CRSF::CRSFstate = true; - if (CRSF::connected) - { - CRSF::connected(); - } - else - { - ERRLN("CRSF::connected has not been initialised"); - } -#endif - return DURATION_IMMEDIATELY; -} - -static int timeout() -{ - CRSF::handleUARTin(); - return DURATION_IMMEDIATELY; -} - -static int event() -{ - // An event should be generated every time the TX power changes - CRSF::LinkStatistics.uplink_TX_Power = powerToCrsfPower(PowerLevelContainer::currPower()); - return DURATION_IGNORE; -} - -device_t CRSF_device = { - .initialize = nullptr, - .start = start, - .event = event, - .timeout = timeout -}; -#endif diff --git a/src/lib/CrsfProtocol/crsf_protocol.h b/src/lib/CrsfProtocol/crsf_protocol.h index ca7b22525e..cb88b201f9 100644 --- a/src/lib/CrsfProtocol/crsf_protocol.h +++ b/src/lib/CrsfProtocol/crsf_protocol.h @@ -291,9 +291,6 @@ union inBuffer_U // add other packet types here }; - -typedef struct crsf_channels_s crsf_channels_t; - //CRSF_FRAMETYPE_BATTERY_SENSOR typedef struct crsf_sensor_battery_s { diff --git a/src/lib/GSENSOR/devGsensor.cpp b/src/lib/GSENSOR/devGsensor.cpp index d146a2f34d..eb4abc3ab5 100644 --- a/src/lib/GSENSOR/devGsensor.cpp +++ b/src/lib/GSENSOR/devGsensor.cpp @@ -9,6 +9,7 @@ #include "devGsensor.h" #include +#include "handset.h" #include "gsensor.h" #include "POWERMGNT.h" #include "config.h" @@ -28,7 +29,6 @@ static unsigned int bumps = 0; static unsigned long lastBumpTime = 0; static unsigned long lastBumpCommand = 0; -extern bool IsArmed(); extern void SendRxLoanOverMSP(); extern void EnterBindingMode(); extern void deferExecution(uint32_t ms, std::function f); @@ -72,7 +72,7 @@ static int timeout() float x, y, z; gsensor.getGSensorData(&x, &y, &z); // Single bump while holding the radio antenna up and NOT armed is Loan/Bind - if (!CRSF::IsArmed() && bumps == 1 && fabs(x) < 0.5 && y < -0.8 && fabs(z) < 0.5) + if (!handset->IsArmed() && bumps == 1 && fabs(x) < 0.5 && y < -0.8 && fabs(z) < 0.5) { lastBumpCommand = now; if (connectionState == connected) @@ -98,7 +98,7 @@ static int timeout() //When system is idle, set power to minimum if(config.GetMotionMode() == 1) { - if((system_quiet_state == GSENSOR_SYSTEM_STATE_QUIET) && (system_quiet_pre_state == GSENSOR_SYSTEM_STATE_MOVING) && !CRSF::IsArmed()) + if((system_quiet_state == GSENSOR_SYSTEM_STATE_QUIET) && (system_quiet_pre_state == GSENSOR_SYSTEM_STATE_MOVING) && !handset->IsArmed()) { POWERMGNT::setPower(MinPower); } diff --git a/src/lib/Handset/AutoDetect.cpp b/src/lib/Handset/AutoDetect.cpp new file mode 100644 index 0000000000..7bdf3301e7 --- /dev/null +++ b/src/lib/Handset/AutoDetect.cpp @@ -0,0 +1,104 @@ +#include "targets.h" + +#if defined(PLATFORM_ESP32) && defined(TARGET_TX) + +#include "AutoDetect.h" +#include "CRSFHandset.h" +#include "PPMHandset.h" +#include "logging.h" + +#include + +constexpr rmt_channel_t PPM_RMT_CHANNEL = RMT_CHANNEL_0; +constexpr auto RMT_TICKS_PER_US = 10; + +void AutoDetect::Begin() +{ + constexpr auto divisor = 80 / RMT_TICKS_PER_US; + + rmt_config_t rmt_rx_config = RMT_DEFAULT_CONFIG_RX(static_cast(GPIO_PIN_RCSIGNAL_RX), PPM_RMT_CHANNEL); + rmt_rx_config.clk_div = divisor; + rmt_rx_config.rx_config.filter_ticks_thresh = 1; + rmt_rx_config.rx_config.idle_threshold = 100; + rmt_config(&rmt_rx_config); + rmt_driver_install(PPM_RMT_CHANNEL, 1000, 0); + + rmt_get_ringbuf_handle(PPM_RMT_CHANNEL, &rb); + rmt_rx_start(PPM_RMT_CHANNEL, true); + input_detect = 0; +} + +void AutoDetect::End() +{ + rmt_driver_uninstall(PPM_RMT_CHANNEL); +} + +bool AutoDetect::IsArmed() +{ + return false; +} + +void AutoDetect::migrateTo(Handset *that) const +{ + that->setRCDataCallback(RCdataCallback); + that->registerParameterUpdateCallback(RecvParameterUpdate); + that->registerCallbacks(connected, disconnected, RecvModelUpdate); + that->Begin(); + that->setPacketInterval(RequestedRCpacketInterval); + delete this; + handset = that; +} + +void AutoDetect::startPPM() const +{ + migrateTo(new PPMHandset()); +} + +void AutoDetect::startCRSF() const +{ + migrateTo(new CRSFHandset()); +} + +void AutoDetect::handleInput() +{ + size_t length = 0; + const auto now = millis(); + + const auto items = static_cast(xRingbufferReceive(rb, &length, 0)); + if (items) + { + vRingbufferReturnItem(rb, static_cast(items)); + lastDetect = now; + length /= 4; // one RMT = 4 Bytes + if (length == 0) + { + input_detect++; + if (input_detect > 100) + { + DBGLN("PPM signal detected"); + rmt_driver_uninstall(PPM_RMT_CHANNEL); + startPPM(); + } + } + else + { + input_detect--; + if (input_detect < -100) + { + DBGLN("Serial signal detected"); + rmt_driver_uninstall(PPM_RMT_CHANNEL); + startCRSF(); + } + } + } + else + { + if (now - 1000 > lastDetect && input_detect != 0) + { + DBGLN("No signal detected"); + input_detect = 0; + } + } +} + +#endif diff --git a/src/lib/Handset/AutoDetect.h b/src/lib/Handset/AutoDetect.h new file mode 100644 index 0000000000..f1f58133ad --- /dev/null +++ b/src/lib/Handset/AutoDetect.h @@ -0,0 +1,23 @@ +#pragma once + +#include "handset.h" + +#include + +class AutoDetect final : public Handset +{ +public: + void Begin() override; + void End() override; + bool IsArmed() override; + void handleInput() override; + +private: + void migrateTo(Handset *that) const; + void startPPM() const; + void startCRSF() const; + + int input_detect = 0; + RingbufHandle_t rb = nullptr; + uint32_t lastDetect = 0; +}; \ No newline at end of file diff --git a/src/lib/Handset/CRSF.cpp b/src/lib/Handset/CRSF.cpp new file mode 100644 index 0000000000..5e4fb3a7ae --- /dev/null +++ b/src/lib/Handset/CRSF.cpp @@ -0,0 +1,232 @@ +#include "CRSF.h" + +#include "common.h" +#include "FIFO.h" + +volatile crsfPayloadLinkstatistics_s CRSF::LinkStatistics; +GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY); + +uint8_t CRSF::MspData[ELRS_MSP_BUFFER] = {0}; +uint8_t CRSF::MspDataLength = 0; + +static const auto MSP_SERIAL_OUT_FIFO_SIZE = 256U; +static FIFO MspWriteFIFO; + + +/*** + * @brief: Convert `version` (string) to a integer version representation + * e.g. "2.2.15 ISM24G" => 0x0002020f + * Assumes all version fields are < 256, the number portion + * MUST be followed by a space to correctly be parsed + ***/ +uint32_t CRSF::VersionStrToU32(const char *verStr) +{ + uint32_t retVal = 0; +#if !defined(FORCE_NO_DEVICE_VERSION) + uint8_t accumulator = 0; + char c; + bool trailing_data = false; + while ((c = *verStr)) + { + ++verStr; + // A decimal indicates moving to a new version field + if (c == '.') + { + retVal = (retVal << 8) | accumulator; + accumulator = 0; + trailing_data = false; + } + // Else if this is a number add it up + else if (c >= '0' && c <= '9') + { + accumulator = (accumulator * 10) + (c - '0'); + trailing_data = true; + } + // Anything except [0-9.] ends the parsing + else + { + break; + } + } + if (trailing_data) + { + retVal = (retVal << 8) | accumulator; + } + // If the version ID is < 1.0.0, we could not parse it, + // just use the OTA version as the major version number + if (retVal < 0x010000) + { + retVal = OTA_VERSION_ID << 16; + } +#endif + return retVal; +} + +void CRSF::GetDeviceInformation(uint8_t *frame, uint8_t fieldCount) +{ + const uint8_t size = strlen(device_name)+1; + auto *device = (deviceInformationPacket_t *)(frame + sizeof(crsf_ext_header_t) + size); + // Packet starts with device name + memcpy(frame + sizeof(crsf_ext_header_t), device_name, size); + // Followed by the device + device->serialNo = htobe32(0x454C5253); // ['E', 'L', 'R', 'S'], seen [0x00, 0x0a, 0xe7, 0xc6] // "Serial 177-714694" (value is 714694) + device->hardwareVer = 0; // unused currently by us, seen [ 0x00, 0x0b, 0x10, 0x01 ] // "Hardware: V 1.01" / "Bootloader: V 3.06" + device->softwareVer = htobe32(VersionStrToU32(version)); // seen [ 0x00, 0x00, 0x05, 0x0f ] // "Firmware: V 5.15" + device->fieldCnt = fieldCount; + device->parameterVersion = 0; +} + +void CRSF::SetMspV2Request(uint8_t *frame, uint16_t function, uint8_t *payload, uint8_t payloadLength) +{ + auto *packet = (uint8_t *)(frame + sizeof(crsf_ext_header_t)); + packet[0] = 0x50; // no error, version 2, beginning of the frame, first frame (0) + packet[1] = 0; // flags + packet[2] = function & 0xFF; + packet[3] = (function >> 8) & 0xFF; + packet[4] = payloadLength & 0xFF; + packet[5] = (payloadLength >> 8) & 0xFF; + memcpy(packet + 6, payload, payloadLength); + packet[6 + payloadLength] = CalcCRCMsp(packet + 1, payloadLength + 5); // crc = flags + function + length + payload +} + +void CRSF::SetHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t destAddr) +{ + auto *header = (crsf_header_t *)frame; + header->device_addr = destAddr; + header->frame_size = frameSize; + header->type = frameType; + + uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], frameSize - 1, 0); + frame[frameSize + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc; +} + +void CRSF::SetExtendedHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t senderAddr, uint8_t destAddr) +{ + auto *header = (crsf_ext_header_t *)frame; + header->dest_addr = destAddr; + header->orig_addr = senderAddr; + SetHeaderAndCrc(frame, frameType, frameSize, destAddr); +} + + +void CRSF::GetMspMessage(uint8_t **data, uint8_t *len) +{ + *len = MspDataLength; + *data = (MspDataLength > 0) ? MspData : nullptr; +} + +void CRSF::ResetMspQueue() +{ + MspWriteFIFO.flush(); + MspDataLength = 0; + memset(MspData, 0, ELRS_MSP_BUFFER); +} + +void CRSF::UnlockMspMessage() +{ + // current msp message is sent so restore next buffered write + if (MspWriteFIFO.size() > 0) + { + MspWriteFIFO.lock(); + MspDataLength = MspWriteFIFO.pop(); + MspWriteFIFO.popBytes(MspData, MspDataLength); + MspWriteFIFO.unlock(); + } + else + { + // no msp message is ready to send currently + MspDataLength = 0; + memset(MspData, 0, ELRS_MSP_BUFFER); + } +} + +void ICACHE_RAM_ATTR CRSF::AddMspMessage(mspPacket_t* packet, uint8_t destination) +{ + if (packet->payloadSize > ENCAPSULATED_MSP_MAX_PAYLOAD_SIZE) + { + return; + } + + const uint8_t totalBufferLen = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES; + uint8_t outBuffer[ENCAPSULATED_MSP_MAX_FRAME_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES]; + + // CRSF extended frame header + outBuffer[0] = CRSF_ADDRESS_BROADCAST; // address + outBuffer[1] = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC; // length + outBuffer[2] = CRSF_FRAMETYPE_MSP_WRITE; // packet type + outBuffer[3] = destination; // destination + outBuffer[4] = CRSF_ADDRESS_RADIO_TRANSMITTER; // origin + + // Encapsulated MSP payload + outBuffer[5] = 0x30; // header + outBuffer[6] = packet->payloadSize; // mspPayloadSize + outBuffer[7] = packet->function; // packet->cmd + for (uint16_t i = 0; i < packet->payloadSize; ++i) + { + // copy packet payload into outBuffer + outBuffer[8 + i] = packet->payload[i]; + } + // Encapsulated MSP crc + outBuffer[totalBufferLen - 2] = CalcCRCMsp(&outBuffer[6], packet->payloadSize + 2); + + // CRSF frame crc + outBuffer[totalBufferLen - 1] = crsf_crc.calc(&outBuffer[2], packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC - 1); + AddMspMessage(totalBufferLen, outBuffer); +} + +void ICACHE_RAM_ATTR CRSF::AddMspMessage(const uint8_t length, uint8_t* data) +{ + if (length > ELRS_MSP_BUFFER) + { + return; + } + + // store next msp message + if (MspDataLength == 0) + { + for (uint8_t i = 0; i < length; i++) + { + MspData[i] = data[i]; + } + MspDataLength = length; + } + // store all write requests since an update does send multiple writes + else + { + MspWriteFIFO.lock(); + if (MspWriteFIFO.ensure(length + 1)) + { + MspWriteFIFO.push(length); + MspWriteFIFO.pushBytes((const uint8_t *)data, length); + } + MspWriteFIFO.unlock(); + } +} + +#if defined(CRSF_RX_MODULE) + +bool CRSF::HasUpdatedUplinkPower = false; + +/*** + * @brief: Call this when new uplinkPower from the TX is availble from OTA instead of setting directly + */ +void CRSF::updateUplinkPower(uint8_t uplinkPower) +{ + if (uplinkPower != LinkStatistics.uplink_TX_Power) + { + LinkStatistics.uplink_TX_Power = uplinkPower; + HasUpdatedUplinkPower = true; + } +} + +/*** + * @brief: Returns true if HasUpdatedUplinkPower and clears the flag + */ +bool CRSF::clearUpdatedUplinkPower() +{ + bool retVal = HasUpdatedUplinkPower; + HasUpdatedUplinkPower = false; + return retVal; +} + +#endif // CRSF_RX_MODULE diff --git a/src/lib/Handset/CRSF.h b/src/lib/Handset/CRSF.h new file mode 100644 index 0000000000..3b3a77ac39 --- /dev/null +++ b/src/lib/Handset/CRSF.h @@ -0,0 +1,42 @@ +#ifndef H_CRSF +#define H_CRSF + +#include "crsf_protocol.h" +#include "telemetry_protocol.h" +#include "msp.h" +#include "msptypes.h" + +class CRSF +{ +public: + static volatile crsfPayloadLinkstatistics_s LinkStatistics; // Link Statistics Stored as Struct + + static void GetMspMessage(uint8_t **data, uint8_t *len); + static void UnlockMspMessage(); + static void AddMspMessage(uint8_t length, uint8_t *data); + static void AddMspMessage(mspPacket_t *packet, uint8_t destination); + static void ResetMspQueue(); + + static void GetDeviceInformation(uint8_t *frame, uint8_t fieldCount); + static void SetMspV2Request(uint8_t *frame, uint16_t function, uint8_t *payload, uint8_t payloadLength); + static void SetHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t destAddr); + static void SetExtendedHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t senderAddr, uint8_t destAddr); + static uint32_t VersionStrToU32(const char *verStr); + +#if defined(CRSF_RX_MODULE) +public: + static void updateUplinkPower(uint8_t uplinkPower); + static bool clearUpdatedUplinkPower(); + +private: + static bool HasUpdatedUplinkPower; +#endif + +private: + static uint8_t MspData[ELRS_MSP_BUFFER]; + static uint8_t MspDataLength; +}; + +extern GENERIC_CRC8 crsf_crc; + +#endif \ No newline at end of file diff --git a/src/lib/CRSF/CRSF.cpp b/src/lib/Handset/CRSFHandset.cpp similarity index 57% rename from src/lib/CRSF/CRSF.cpp rename to src/lib/Handset/CRSFHandset.cpp index 1e12a17a5b..a7402128e6 100644 --- a/src/lib/CRSF/CRSF.cpp +++ b/src/lib/Handset/CRSFHandset.cpp @@ -1,24 +1,23 @@ #include "CRSF.h" -#include "device.h" +#include "CRSFHandset.h" #include "FIFO.h" #include "logging.h" #include "helpers.h" -volatile crsfPayloadLinkstatistics_s CRSF::LinkStatistics; -GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY); - -#if defined(CRSF_TX_MODULE) +#if defined(CRSF_TX_MODULE) && !defined(UNIT_TEST) +#include "device.h" #if defined(PLATFORM_ESP32) +#include #include // UART0 is used since for DupleTX we can connect directly through IO_MUX and not the Matrix // for better performance, and on other targets (mostly using pin 13), it always uses Matrix -HardwareSerial CRSF::Port(0); +HardwareSerial CRSFHandset::Port(0); RTC_DATA_ATTR int rtcModelId = 0; #elif defined(PLATFORM_ESP8266) -HardwareSerial CRSF::Port(0); +HardwareSerial CRSFHandset::Port(0); #elif defined(PLATFORM_STM32) -HardwareSerial CRSF::Port(GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX); +HardwareSerial CRSFHandset::Port(GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX); #if defined(STM32F3) || defined(STM32F3xx) #include "stm32f3xx_hal.h" #include "stm32f3xx_hal_gpio.h" @@ -27,76 +26,38 @@ HardwareSerial CRSF::Port(GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX); #include "stm32f1xx_hal_gpio.h" #endif #elif defined(TARGET_NATIVE) -HardwareSerial CRSF::Port = Serial; +HardwareSerial CRSFHandset::Port = Serial; #endif -#define HANDSET_TELEMETRY_FIFO_SIZE 128 // this is the smallest telemetry FIFO size in ETX with CRSF defined +static constexpr int HANDSET_TELEMETRY_FIFO_SIZE = 128; // this is the smallest telemetry FIFO size in ETX with CRSF defined /// Out FIFO to buffer messages/// -static const auto CRSF_SERIAL_OUT_FIFO_SIZE = 256U; +static constexpr auto CRSF_SERIAL_OUT_FIFO_SIZE = 256U; static FIFO SerialOutFIFO; -inBuffer_U CRSF::inBuffer; - -Stream *CRSF::PortSecondary; - -static const auto MSP_SERIAL_OUT_FIFO_SIZE = 256U; -static FIFO MspWriteFIFO; - -void (*CRSF::disconnected)() = nullptr; // called when CRSF stream is lost -void (*CRSF::connected)() = nullptr; // called when CRSF stream is regained - -void (*CRSF::RecvParameterUpdate)(uint8_t type, uint8_t index, uint8_t arg) = nullptr; // called when recv parameter update req, ie from LUA -void (*CRSF::RecvModelUpdate)() = nullptr; // called when model id cahnges, ie command from Radio -void (*CRSF::RCdataCallback)() = nullptr; // called when there is new RC data +Stream *CRSFHandset::PortSecondary; /// UART Handling /// -uint8_t CRSF::SerialInPacketLen = 0; // length of the CRSF packet as measured -uint8_t CRSF::SerialInPacketPtr = 0; // index where we are reading/writing -bool CRSF::CRSFframeActive = false; //since we get a copy of the serial data use this flag to know when to ignore it - -uint32_t CRSF::GoodPktsCountResult = 0; -uint32_t CRSF::BadPktsCountResult = 0; +uint32_t CRSFHandset::GoodPktsCountResult = 0; +uint32_t CRSFHandset::BadPktsCountResult = 0; -uint8_t CRSF::modelId = 0; -bool CRSF::ForwardDevicePings = false; -bool CRSF::elrsLUAmode = false; +uint8_t CRSFHandset::modelId = 0; +bool CRSFHandset::ForwardDevicePings = false; +bool CRSFHandset::elrsLUAmode = false; /// OpenTX mixer sync /// -uint32_t CRSF::OpenTXsyncLastSent = 0; -int32_t CRSF::RequestedRCpacketInterval = 5000; // default to 200hz as per 'normal' -volatile uint32_t CRSF::RCdataLastRecv = 0; -volatile int32_t CRSF::OpenTXsyncOffset = 0; -volatile int32_t CRSF::OpenTXsyncWindow = 0; -volatile int32_t CRSF::OpenTXsyncWindowSize = 0; -volatile uint32_t CRSF::dataLastRecv = 0; -bool CRSF::OpentxSyncActive = true; -uint32_t CRSF::OpenTXsyncOffsetSafeMargin = 1000; // 100us +static const int32_t OpenTXsyncPacketInterval = 200; // in ms +static const int32_t OpenTXsyncOffsetSafeMargin = 1000; // 100us /// UART Handling /// -uint32_t CRSF::GoodPktsCount = 0; -uint32_t CRSF::BadPktsCount = 0; -uint32_t CRSF::UARTwdtLastChecked; - -uint8_t CRSF::CRSFoutBuffer[CRSF_MAX_PACKET_LEN] = {0}; -uint8_t CRSF::maxPacketBytes = CRSF_MAX_PACKET_LEN; -uint8_t CRSF::maxPeriodBytes = CRSF_MAX_PACKET_LEN; -uint32_t CRSF::TxToHandsetBauds[] = {400000, 115200, 5250000, 3750000, 1870000, 921600, 2250000}; -uint32_t CRSF::UARTrequestedBaud = 5250000; -uint8_t CRSF::UARTcurrentBaudIdx = 6; // only used for baud-cycling, initialized to the end so the next one we try is the first in the list -#if defined(PLATFORM_ESP32) -bool CRSF::UARTinverted = true; // default to start looking for an inverted signal -#endif - -bool CRSF::CRSFstate = false; +static const int32_t TxToHandsetBauds[] = {400000, 115200, 5250000, 3750000, 1870000, 921600, 2250000}; +uint8_t CRSFHandset::UARTcurrentBaudIdx = 6; // only used for baud-cycling, initialized to the end so the next one we try is the first in the list +uint32_t CRSFHandset::UARTrequestedBaud = 5250000; // for the UART wdt, every 1000ms we change bauds when connect is lost -#define UARTwdtInterval 1000 - -uint8_t CRSF::MspData[ELRS_MSP_BUFFER] = {0}; -uint8_t CRSF::MspDataLength = 0; +static const int UARTwdtInterval = 1000; -void CRSF::Begin() +void CRSFHandset::Begin() { DBGLN("About to start CRSF task..."); @@ -108,10 +69,10 @@ void CRSF::Begin() { UARTinverted = false; // on a full UART we will start uninverted checking first } - CRSF::Port.begin(UARTrequestedBaud, SERIAL_8N1, + CRSFHandset::Port.begin(UARTrequestedBaud, SERIAL_8N1, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX, false, 500); - CRSF::duplex_set_RX(); + CRSFHandset::duplex_set_RX(); portENABLE_INTERRUPTS(); flush_port_input(); if (esp_reset_reason() != ESP_RST_POWERON) @@ -121,7 +82,7 @@ void CRSF::Begin() } #elif defined(PLATFORM_ESP8266) // Uses default UART pins - CRSF::Port.begin(UARTrequestedBaud); + CRSFHandset::Port.begin(UARTrequestedBaud); // Invert RX/TX (not done, connection is full duplex uninverted) //USC0(UART0) |= BIT(UCRXI) | BIT(UCTXI); // No log message because this is our only UART @@ -129,17 +90,17 @@ void CRSF::Begin() #elif defined(PLATFORM_STM32) DBGLN("Start STM32 R9M TX CRSF UART"); - CRSF::Port.setTx(GPIO_PIN_RCSIGNAL_TX); - CRSF::Port.setRx(GPIO_PIN_RCSIGNAL_RX); + CRSFHandset::Port.setTx(GPIO_PIN_RCSIGNAL_TX); + CRSFHandset::Port.setRx(GPIO_PIN_RCSIGNAL_RX); #if defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN) pinMode(GPIO_PIN_BUFFER_OE, OUTPUT); digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED); // RX mode default #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX) - CRSF::Port.setHalfDuplex(); + CRSFHandset::Port.setHalfDuplex(); #endif - CRSF::Port.begin(UARTrequestedBaud); + CRSFHandset::Port.begin(UARTrequestedBaud); #if defined(TARGET_TX_GHOST) USART1->CR1 &= ~USART_CR1_UE; @@ -153,57 +114,54 @@ void CRSF::Begin() USART2->CR1 |= USART_CR1_UE; #endif DBGLN("STM32 CRSF UART LISTEN TASK STARTED"); - CRSF::Port.flush(); + CRSFHandset::Port.flush(); flush_port_input(); #endif } -void CRSF::End() +void CRSFHandset::End() { uint32_t startTime = millis(); while (SerialOutFIFO.peek() > 0) { - handleUARTin(); + handleInput(); if (millis() - startTime > 1000) { break; } } - //CRSF::Port.end(); // don't call seria.end(), it causes some sort of issue with the 900mhz hardware using gpio2 for serial + //CRSFHandset::Port.end(); // don't call serial.end(), it causes some sort of issue with the 900mhz hardware using gpio2 for serial DBGLN("CRSF UART END"); } -void CRSF::flush_port_input(void) +void CRSFHandset::flush_port_input() { // Make sure there is no garbage on the UART at the start - while (CRSF::Port.available()) + while (CRSFHandset::Port.available()) { - CRSF::Port.read(); + CRSFHandset::Port.read(); } } -void ICACHE_RAM_ATTR CRSF::makeLinkStatisticsPacket(uint8_t buffer[LinkStatisticsFrameLength + 4]) +void CRSFHandset::makeLinkStatisticsPacket(uint8_t buffer[LinkStatisticsFrameLength + 4]) { buffer[0] = CRSF_ADDRESS_RADIO_TRANSMITTER; buffer[1] = LinkStatisticsFrameLength + 2; buffer[2] = CRSF_FRAMETYPE_LINK_STATISTICS; for (uint8_t i = 0; i < LinkStatisticsFrameLength; i++) { - buffer[i + 3] = ((uint8_t *)&LinkStatistics)[i]; + buffer[i + 3] = ((uint8_t *)&CRSF::LinkStatistics)[i]; } uint8_t crc = crsf_crc.calc(buffer[2]); - buffer[LinkStatisticsFrameLength + 3] = crsf_crc.calc((byte *)&LinkStatistics, LinkStatisticsFrameLength, crc); + buffer[LinkStatisticsFrameLength + 3] = crsf_crc.calc((byte *)&CRSF::LinkStatistics, LinkStatisticsFrameLength, crc); } /** * Build a an extended type packet and queue it in the SerialOutFIFO * This is just a regular packet with 2 extra bytes with the sub src and target **/ -void CRSF::packetQueueExtended(uint8_t type, void *data, uint8_t len) +void CRSFHandset::packetQueueExtended(uint8_t type, void *data, uint8_t len) { - if (!CRSF::CRSFstate) - return; - uint8_t buf[6] = { (uint8_t)(len + 6), CRSF_ADDRESS_RADIO_TRANSMITTER, @@ -227,9 +185,9 @@ void CRSF::packetQueueExtended(uint8_t type, void *data, uint8_t len) SerialOutFIFO.unlock(); } -void ICACHE_RAM_ATTR CRSF::sendTelemetryToTX(uint8_t *data) +void CRSFHandset::sendTelemetryToTX(uint8_t *data) { - if (CRSF::CRSFstate) + if (controllerConnected) { uint8_t size = CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX]); if (size > CRSF_MAX_PACKET_LEN) @@ -249,34 +207,29 @@ void ICACHE_RAM_ATTR CRSF::sendTelemetryToTX(uint8_t *data) } } -void ICACHE_RAM_ATTR CRSF::setSyncParams(int32_t PacketInterval) +void ICACHE_RAM_ATTR CRSFHandset::setPacketInterval(int32_t PacketInterval) { - CRSF::RequestedRCpacketInterval = PacketInterval; - CRSF::OpenTXsyncOffset = 0; - CRSF::OpenTXsyncWindow = 0; - CRSF::OpenTXsyncWindowSize = std::max((int32_t)1, (int32_t)(20000/CRSF::RequestedRCpacketInterval)); - CRSF::OpenTXsyncLastSent -= OpenTXsyncPacketInterval; + RequestedRCpacketInterval = PacketInterval; + OpenTXsyncOffset = 0; + OpenTXsyncWindow = 0; + OpenTXsyncWindowSize = std::max((int32_t)1, (int32_t)(20000/RequestedRCpacketInterval)); + OpenTXsyncLastSent -= OpenTXsyncPacketInterval; adjustMaxPacketSize(); } -uint32_t ICACHE_RAM_ATTR CRSF::GetRCdataLastRecv() -{ - return CRSF::RCdataLastRecv; -} - -void ICACHE_RAM_ATTR CRSF::JustSentRFpacket() +void ICACHE_RAM_ATTR CRSFHandset::JustSentRFpacket() { // read them in this order to prevent a potential race condition - uint32_t last = CRSF::dataLastRecv; + uint32_t last = dataLastRecv; uint32_t m = micros(); - int32_t delta = (int32_t)(m - last); + auto delta = (int32_t)(m - last); - if (delta >= (int32_t)CRSF::RequestedRCpacketInterval) + if (delta >= (int32_t)RequestedRCpacketInterval) { // missing/late packet, force resync - CRSF::OpenTXsyncOffset = -(delta % CRSF::RequestedRCpacketInterval) * 10; - CRSF::OpenTXsyncWindow = 0; - CRSF::OpenTXsyncLastSent -= OpenTXsyncPacketInterval; + OpenTXsyncOffset = -(delta % RequestedRCpacketInterval) * 10; + OpenTXsyncWindow = 0; + OpenTXsyncLastSent -= OpenTXsyncPacketInterval; #ifdef DEBUG_OPENTX_SYNC DBGLN("Missed packet, forced resync (%d)!", delta); #endif @@ -285,28 +238,18 @@ void ICACHE_RAM_ATTR CRSF::JustSentRFpacket() { // The number of packets in the sync window is how many will fit in 20ms. // This gives quite quite coarse changes for 50Hz, but more fine grained changes at 1000Hz. - CRSF::OpenTXsyncWindow = std::min(CRSF::OpenTXsyncWindow + 1, (int32_t)CRSF::OpenTXsyncWindowSize); - CRSF::OpenTXsyncOffset = ((CRSF::OpenTXsyncOffset * (CRSF::OpenTXsyncWindow-1)) + delta * 10) / CRSF::OpenTXsyncWindow; + OpenTXsyncWindow = std::min(OpenTXsyncWindow + 1, (int32_t)OpenTXsyncWindowSize); + OpenTXsyncOffset = ((OpenTXsyncOffset * (OpenTXsyncWindow-1)) + delta * 10) / OpenTXsyncWindow; } } -void CRSF::disableOpentxSync() -{ - OpentxSyncActive = false; -} - -void CRSF::enableOpentxSync() -{ - OpentxSyncActive = true; -} - -void ICACHE_RAM_ATTR CRSF::sendSyncPacketToTX() // in values in us. +void CRSFHandset::sendSyncPacketToTX() // in values in us. { uint32_t now = millis(); - if (CRSF::CRSFstate && (now - OpenTXsyncLastSent) >= OpenTXsyncPacketInterval) + if (controllerConnected && (now - OpenTXsyncLastSent) >= OpenTXsyncPacketInterval) { - int32_t packetRate = CRSF::RequestedRCpacketInterval * 10; //convert from us to right format - int32_t offset = CRSF::OpenTXsyncOffset - CRSF::OpenTXsyncOffsetSafeMargin; // offset so that opentx always has some headroom + int32_t packetRate = RequestedRCpacketInterval * 10; //convert from us to right format + int32_t offset = OpenTXsyncOffset - OpenTXsyncOffsetSafeMargin; // offset so that opentx always has some headroom #ifdef DEBUG_OPENTX_SYNC DBGLN("Offset %d", offset); // in 10ths of us (OpenTX sync unit) #endif @@ -318,7 +261,7 @@ void ICACHE_RAM_ATTR CRSF::sendSyncPacketToTX() // in values in us. } PACKED; uint8_t buffer[sizeof(otxSyncData)]; - struct otxSyncData * const sync = (struct otxSyncData * const)buffer; + auto * const sync = (struct otxSyncData * const)buffer; sync->extendedType = CRSF_FRAMETYPE_OPENTX_SYNC; sync->rate = htobe32(packetRate); @@ -330,12 +273,12 @@ void ICACHE_RAM_ATTR CRSF::sendSyncPacketToTX() // in values in us. } } -void ICACHE_RAM_ATTR CRSF::RcPacketToChannelsData() // data is packed as 11 bits per channel +void CRSFHandset::RcPacketToChannelsData() // data is packed as 11 bits per channel { // for monitoring arming state uint32_t prev_AUX1 = ChannelData[4]; - uint8_t const * const payload = (uint8_t const * const)&CRSF::inBuffer.asRCPacket_t.channels; + auto payload = (uint8_t const * const)&inBuffer.asRCPacket_t.channels; constexpr unsigned srcBits = 11; constexpr unsigned dstBits = 11; constexpr unsigned inputChannelMask = (1 << srcBits) - 1; @@ -345,7 +288,7 @@ void ICACHE_RAM_ATTR CRSF::RcPacketToChannelsData() // data is packed as 11 bits uint8_t bitsMerged = 0; uint32_t readValue = 0; unsigned readByteIndex = 0; - for (unsigned n = 0; n < CRSF_NUM_CHANNELS; n++) + for (uint32_t & n : ChannelData) { while (bitsMerged < srcBits) { @@ -354,7 +297,7 @@ void ICACHE_RAM_ATTR CRSF::RcPacketToChannelsData() // data is packed as 11 bits bitsMerged += 8; } //printf("rv=%x(%x) bm=%u\n", readValue, (readValue & inputChannelMask), bitsMerged); - ChannelData[n] = (readValue & inputChannelMask) << precisionShift; + n = (readValue & inputChannelMask) << precisionShift; readValue >>= srcBits; bitsMerged -= srcBits; } @@ -367,25 +310,25 @@ void ICACHE_RAM_ATTR CRSF::RcPacketToChannelsData() // data is packed as 11 bits } } -bool ICACHE_RAM_ATTR CRSF::ProcessPacket() +bool CRSFHandset::ProcessPacket() { bool packetReceived = false; - CRSF::dataLastRecv = micros(); + CRSFHandset::dataLastRecv = micros(); - if (CRSFstate == false) + if (!controllerConnected) { - CRSFstate = true; + controllerConnected = true; DBGLN("CRSF UART Connected"); if (connected) connected(); } - const uint8_t packetType = CRSF::inBuffer.asRCPacket_t.header.type; - uint8_t *SerialInBuffer = CRSF::inBuffer.asUint8_t; + const uint8_t packetType = inBuffer.asRCPacket_t.header.type; + uint8_t *SerialInBuffer = inBuffer.asUint8_t; if (packetType == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { - CRSF::RCdataLastRecv = micros(); + RCdataLastRecv = micros(); RcPacketToChannelsData(); packetReceived = true; } @@ -398,8 +341,8 @@ bool ICACHE_RAM_ATTR CRSF::ProcessPacket() // unless connected if (ForwardDevicePings || packetType != CRSF_FRAMETYPE_DEVICE_PING) { - const uint8_t length = CRSF::inBuffer.asRCPacket_t.header.frame_size + 2; - AddMspMessage(length, SerialInBuffer); + const uint8_t length = inBuffer.asRCPacket_t.header.frame_size + 2; + CRSF::AddMspMessage(length, SerialInBuffer); } packetReceived = true; } @@ -430,114 +373,20 @@ bool ICACHE_RAM_ATTR CRSF::ProcessPacket() return packetReceived; } -void CRSF::GetMspMessage(uint8_t **data, uint8_t *len) -{ - *len = MspDataLength; - *data = (MspDataLength > 0) ? MspData : nullptr; -} - -void CRSF::ResetMspQueue() -{ - MspWriteFIFO.flush(); - MspDataLength = 0; - memset(MspData, 0, ELRS_MSP_BUFFER); -} - -void CRSF::UnlockMspMessage() -{ - // current msp message is sent so restore next buffered write - if (MspWriteFIFO.size() > 0) - { - MspWriteFIFO.lock(); - MspDataLength = MspWriteFIFO.pop(); - MspWriteFIFO.popBytes(MspData, MspDataLength); - MspWriteFIFO.unlock(); - } - else - { - // no msp message is ready to send currently - MspDataLength = 0; - memset(MspData, 0, ELRS_MSP_BUFFER); - } -} - -void ICACHE_RAM_ATTR CRSF::AddMspMessage(mspPacket_t* packet) -{ - if (packet->payloadSize > ENCAPSULATED_MSP_MAX_PAYLOAD_SIZE) - { - return; - } - - const uint8_t totalBufferLen = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES; - uint8_t outBuffer[ENCAPSULATED_MSP_MAX_FRAME_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES]; - - // CRSF extended frame header - outBuffer[0] = CRSF_ADDRESS_BROADCAST; // address - outBuffer[1] = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC; // length - outBuffer[2] = CRSF_FRAMETYPE_MSP_WRITE; // packet type - outBuffer[3] = CRSF_ADDRESS_FLIGHT_CONTROLLER; // destination - outBuffer[4] = CRSF_ADDRESS_RADIO_TRANSMITTER; // origin - - // Encapsulated MSP payload - outBuffer[5] = 0x30; // header - outBuffer[6] = packet->payloadSize; // mspPayloadSize - outBuffer[7] = packet->function; // packet->cmd - for (uint8_t i = 0; i < packet->payloadSize; ++i) - { - // copy packet payload into outBuffer - outBuffer[8 + i] = packet->payload[i]; - } - // Encapsulated MSP crc - outBuffer[totalBufferLen - 2] = CalcCRCMsp(&outBuffer[6], packet->payloadSize + 2); - - // CRSF frame crc - outBuffer[totalBufferLen - 1] = crsf_crc.calc(&outBuffer[2], packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC - 1); - AddMspMessage(totalBufferLen, outBuffer); -} - -void ICACHE_RAM_ATTR CRSF::AddMspMessage(const uint8_t length, uint8_t* data) -{ - if (length > ELRS_MSP_BUFFER) - { - return; - } - - // store next msp message - if (MspDataLength == 0) - { - for (uint8_t i = 0; i < length; i++) - { - MspData[i] = data[i]; - } - MspDataLength = length; - } - // store all write requests since an update does send multiple writes - else - { - MspWriteFIFO.lock(); - if (MspWriteFIFO.ensure(length + 1)) - { - MspWriteFIFO.push(length); - MspWriteFIFO.pushBytes((const uint8_t *)data, length); - } - MspWriteFIFO.unlock(); - } -} - -void ICACHE_RAM_ATTR CRSF::handleUARTin() +void CRSFHandset::handleInput() { - uint8_t *SerialInBuffer = CRSF::inBuffer.asUint8_t; + uint8_t *SerialInBuffer = inBuffer.asUint8_t; if (UARTwdt()) { return; } - while (CRSF::Port.available()) + while (CRSFHandset::Port.available()) { - if (CRSFframeActive == false) + if (!CRSFframeActive) { - unsigned char const inChar = CRSF::Port.read(); + unsigned char const inChar = CRSFHandset::Port.read(); // stage 1 wait for sync byte // if (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER || inChar == CRSF_SYNC_BYTE) @@ -565,7 +414,7 @@ void ICACHE_RAM_ATTR CRSF::handleUARTin() // special case where we save the expected pkt len to buffer // if (SerialInPacketPtr == 1) { - unsigned char const inChar = CRSF::Port.read(); + unsigned char const inChar = CRSFHandset::Port.read(); if (inChar <= CRSF_MAX_PACKET_LEN) { SerialInPacketLen = inChar; @@ -583,13 +432,13 @@ void ICACHE_RAM_ATTR CRSF::handleUARTin() int toRead = (SerialInPacketLen + 2) - SerialInPacketPtr; #if defined(PLATFORM_ESP32) - int count = CRSF::Port.read(&SerialInBuffer[SerialInPacketPtr], toRead); + auto count = (int)CRSFHandset::Port.read(&SerialInBuffer[SerialInPacketPtr], toRead); #else int count = 0; - int avail = CRSF::Port.available(); + auto avail = (int)CRSFHandset::Port.available(); while (count < toRead && count < avail) { - SerialInBuffer[SerialInPacketPtr + count] = CRSF::Port.read(); + SerialInBuffer[SerialInPacketPtr + count] = CRSFHandset::Port.read(); count++; } #endif @@ -597,7 +446,7 @@ void ICACHE_RAM_ATTR CRSF::handleUARTin() if (SerialInPacketPtr >= (SerialInPacketLen + 2)) // plus 2 because the packlen is referenced from the start of the 'type' flag, IE there are an extra 2 bytes. { - char CalculatedCRC = crsf_crc.calc(SerialInBuffer + 2, SerialInPacketPtr - 3); + uint8_t CalculatedCRC = crsf_crc.calc(SerialInBuffer + 2, SerialInPacketPtr - 3); if (CalculatedCRC == SerialInBuffer[SerialInPacketPtr-1]) { @@ -605,7 +454,7 @@ void ICACHE_RAM_ATTR CRSF::handleUARTin() if (ProcessPacket()) { //delayMicroseconds(50); - handleUARTout(); + handleOutput(); if (RCdataCallback) RCdataCallback(); } } @@ -624,13 +473,22 @@ void ICACHE_RAM_ATTR CRSF::handleUARTin() } } -void ICACHE_RAM_ATTR CRSF::handleUARTout() +void CRSFHandset::handleOutput() { + static uint8_t CRSFoutBuffer[CRSF_MAX_PACKET_LEN] = {0}; // both static to split up larger packages static uint8_t packageLengthRemaining = 0; static uint8_t sendingOffset = 0; - if (OpentxSyncActive && packageLengthRemaining == 0 && SerialOutFIFO.size() == 0) + if (!controllerConnected) + { + SerialOutFIFO.lock(); + SerialOutFIFO.flush(); + SerialOutFIFO.unlock(); + return; + } + + if (packageLengthRemaining == 0 && SerialOutFIFO.size() == 0) { sendSyncPacketToTX(); // calculate mixer sync packet if needed } @@ -663,9 +521,9 @@ void ICACHE_RAM_ATTR CRSF::handleUARTout() } // write the packet out, if it's a large package the offset holds the starting position - CRSF::Port.write(CRSFoutBuffer + sendingOffset, writeLength); - if (CRSF::PortSecondary) - CRSF::PortSecondary->write(CRSFoutBuffer + sendingOffset, writeLength); + CRSFHandset::Port.write(CRSFoutBuffer + sendingOffset, writeLength); + if (CRSFHandset::PortSecondary) + CRSFHandset::PortSecondary->write(CRSFoutBuffer + sendingOffset, writeLength); sendingOffset += writeLength; packageLengthRemaining -= writeLength; @@ -675,7 +533,7 @@ void ICACHE_RAM_ATTR CRSF::handleUARTout() if (SerialOutFIFO.size() == 0) break; } - CRSF::Port.flush(); + CRSFHandset::Port.flush(); duplex_set_RX(); // make sure there is no garbage on the UART left over @@ -683,7 +541,7 @@ void ICACHE_RAM_ATTR CRSF::handleUARTout() } } -void ICACHE_RAM_ATTR CRSF::duplex_set_RX() +void CRSFHandset::duplex_set_RX() const { #if defined(PLATFORM_ESP32) if (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX) @@ -708,11 +566,11 @@ void ICACHE_RAM_ATTR CRSF::duplex_set_RX() #elif defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN) digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED); #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX) - CRSF::Port.enableHalfDuplexRx(); + CRSFHandset::Port.enableHalfDuplexRx(); #endif } -void ICACHE_RAM_ATTR CRSF::duplex_set_TX() +void CRSFHandset::duplex_set_TX() const { #if defined(PLATFORM_ESP32) if (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX) @@ -746,7 +604,7 @@ void ICACHE_RAM_ATTR CRSF::duplex_set_TX() #endif } -void ICACHE_RAM_ATTR CRSF::adjustMaxPacketSize() +void ICACHE_RAM_ATTR CRSFHandset::adjustMaxPacketSize() { // baud / 10bits-per-byte / 2 windows (1RX, 1TX) / rate * 0.80 (leeway) maxPeriodBytes = UARTrequestedBaud / 10 / 2 / (1000000/RequestedRCpacketInterval) * 80 / 100; @@ -758,7 +616,7 @@ void ICACHE_RAM_ATTR CRSF::adjustMaxPacketSize() } #if defined(PLATFORM_ESP32_S3) -uint32_t CRSF::autobaud() +uint32_t CRSFHandset::autobaud() { static enum { INIT, MEASURED, INVERTED } state; @@ -768,7 +626,7 @@ uint32_t CRSF::autobaud() state = INVERTED; return UARTrequestedBaud; } - else if (state == INVERTED) + if (state == INVERTED) { UARTinverted = !UARTinverted; state = INIT; @@ -782,33 +640,33 @@ uint32_t CRSF::autobaud() REG_SET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN); // enable autobaud return 400000; } - else if (REG_GET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300) + if (REG_GET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300) { return 400000; } state = MEASURED; - uint32_t low_period = REG_READ(UART_LOWPULSE_REG(0)); - uint32_t high_period = REG_READ(UART_HIGHPULSE_REG(0)); + const uint32_t low_period = REG_READ(UART_LOWPULSE_REG(0)); + const uint32_t high_period = REG_READ(UART_HIGHPULSE_REG(0)); REG_CLR_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN); // disable autobaud REG_CLR_BIT(UART_RX_FILT_REG(0), UART_GLITCH_FILT_EN); // disable glitch filtering DBGLN("autobaud: low %d, high %d", low_period, high_period); // According to the tecnnical reference - int32_t calulatedBaud = UART_CLK_FREQ / (low_period + high_period + 2); - int32_t bestBaud = (int32_t)TxToHandsetBauds[0]; + const int32_t calulatedBaud = UART_CLK_FREQ / (low_period + high_period + 2); + int32_t bestBaud = TxToHandsetBauds[0]; for(int i=0 ; i abs(calulatedBaud - (int32_t)TxToHandsetBauds[i])) + if (abs(calulatedBaud - bestBaud) > abs(calulatedBaud - TxToHandsetBauds[i])) { - bestBaud = (int32_t)TxToHandsetBauds[i]; + bestBaud = TxToHandsetBauds[i]; } } return bestBaud; } #elif defined(PLATFORM_ESP32) -uint32_t CRSF::autobaud() +uint32_t CRSFHandset::autobaud() { static enum { INIT, MEASURED, INVERTED } state; @@ -816,7 +674,8 @@ uint32_t CRSF::autobaud() UARTinverted = !UARTinverted; state = INVERTED; return UARTrequestedBaud; - } else if (state == INVERTED) { + } + if (state == INVERTED) { UARTinverted = !UARTinverted; state = INIT; } @@ -824,53 +683,56 @@ uint32_t CRSF::autobaud() if (REG_GET_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN) == 0) { REG_WRITE(UART_AUTOBAUD_REG(0), 4 << UART_GLITCH_FILT_S | UART_AUTOBAUD_EN); // enable, glitch filter 4 return 400000; - } else if (REG_GET_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300) + } + if (REG_GET_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300) + { return 400000; + } state = MEASURED; - uint32_t low_period = REG_READ(UART_LOWPULSE_REG(0)); - uint32_t high_period = REG_READ(UART_HIGHPULSE_REG(0)); + auto low_period = (int32_t)REG_READ(UART_LOWPULSE_REG(0)); + auto high_period = (int32_t)REG_READ(UART_HIGHPULSE_REG(0)); REG_CLR_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN); // disable autobaud DBGLN("autobaud: low %d, high %d", low_period, high_period); // sample code at https://github.com/espressif/esp-idf/issues/3336 // says baud rate = 80000000/min(UART_LOWPULSE_REG, UART_HIGHPULSE_REG); // Based on testing use max and add 2 for lowest deviation - int32_t calulatedBaud = 80000000 / (max(low_period, high_period) + 3); - int32_t bestBaud = (int32_t)TxToHandsetBauds[0]; - for(int i=0 ; i abs(calulatedBaud - (int32_t)TxToHandsetBauds[i])) + if (abs(calculatedBaud - bestBaud) > abs(calculatedBaud - (int32_t)TxToHandsetBaud)) { - bestBaud = (int32_t)TxToHandsetBauds[i]; + bestBaud = (int32_t)TxToHandsetBaud; } } return bestBaud; } #else -uint32_t CRSF::autobaud() { +uint32_t CRSFHandset::autobaud() { UARTcurrentBaudIdx = (UARTcurrentBaudIdx + 1) % ARRAY_SIZE(TxToHandsetBauds); return TxToHandsetBauds[UARTcurrentBaudIdx]; } #endif -bool CRSF::UARTwdt() +bool CRSFHandset::UARTwdt() { bool retval = false; #if !defined(DEBUG_TX_FREERUN) uint32_t now = millis(); if (now >= (UARTwdtLastChecked + UARTwdtInterval)) { - if (BadPktsCount >= GoodPktsCount || !CRSFstate) + if (BadPktsCount >= GoodPktsCount || !controllerConnected) { DBGLN("Too many bad UART RX packets!"); - if (CRSFstate == true) + if (controllerConnected) { DBGLN("CRSF UART Disconnected"); if (disconnected) disconnected(); - CRSFstate = false; + controllerConnected = false; } UARTrequestedBaud = autobaud(); @@ -882,22 +744,22 @@ bool CRSF::UARTwdt() SerialOutFIFO.flush(); #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32) - CRSF::Port.flush(); - CRSF::Port.updateBaudRate(UARTrequestedBaud); + CRSFHandset::Port.flush(); + CRSFHandset::Port.updateBaudRate(UARTrequestedBaud); #elif defined(TARGET_TX_GHOST) - CRSF::Port.begin(UARTrequestedBaud); + CRSFHandset::Port.begin(UARTrequestedBaud); USART1->CR1 &= ~USART_CR1_UE; USART1->CR3 |= USART_CR3_HDSEL; USART1->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP; //inverted/swapped USART1->CR1 |= USART_CR1_UE; #elif defined(TARGET_TX_FM30_MINI) - CRSF::Port.begin(UARTrequestedBaud); + CRSFHandset::Port.begin(UARTrequestedBaud); LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN); // default is PULLUP USART2->CR1 &= ~USART_CR1_UE; USART2->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV; //inverted USART2->CR1 |= USART_CR1_UE; #else - CRSF::Port.begin(UARTrequestedBaud); + CRSFHandset::Port.begin(UARTrequestedBaud); #endif duplex_set_RX(); // cleanup input buffer @@ -927,126 +789,3 @@ bool CRSF::UARTwdt() } #endif // CRSF_TX_MODULE - -#if defined(CRSF_RX_MODULE) - -bool CRSF::HasUpdatedUplinkPower = false; - -/*** - * @brief: Call this when new uplinkPower from the TX is availble from OTA instead of setting directly - */ -void CRSF::updateUplinkPower(uint8_t uplinkPower) -{ - if (uplinkPower != LinkStatistics.uplink_TX_Power) - { - LinkStatistics.uplink_TX_Power = uplinkPower; - HasUpdatedUplinkPower = true; - } -} - -/*** - * @brief: Returns true if HasUpdatedUplinkPower and clears the flag - */ -bool CRSF::clearUpdatedUplinkPower() -{ - bool retVal = HasUpdatedUplinkPower; - HasUpdatedUplinkPower = false; - return retVal; -} - -#endif // CRSF_RX_MODULE - -/*** - * @brief: Convert `version` (string) to a integer version representation - * e.g. "2.2.15 ISM24G" => 0x0002020f - * Assumes all version fields are < 256, the number portion - * MUST be followed by a space to correctly be parsed - ***/ -uint32_t CRSF::VersionStrToU32(const char *verStr) -{ - uint32_t retVal = 0; -#if !defined(FORCE_NO_DEVICE_VERSION) - uint8_t accumulator = 0; - char c; - bool trailing_data = false; - while ((c = *verStr)) - { - ++verStr; - // A decimal indicates moving to a new version field - if (c == '.') - { - retVal = (retVal << 8) | accumulator; - accumulator = 0; - trailing_data = false; - } - // Else if this is a number add it up - else if (c >= '0' && c <= '9') - { - accumulator = (accumulator * 10) + (c - '0'); - trailing_data = true; - } - // Anything except [0-9.] ends the parsing - else - { - break; - } - } - if (trailing_data) - { - retVal = (retVal << 8) | accumulator; - } - // If the version ID is < 1.0.0, we could not parse it, - // just use the OTA version as the major version number - if (retVal < 0x010000) - { - retVal = OTA_VERSION_ID << 16; - } -#endif - return retVal; -} - -void CRSF::GetDeviceInformation(uint8_t *frame, uint8_t fieldCount) -{ - const uint8_t size = strlen(device_name)+1; - deviceInformationPacket_t *device = (deviceInformationPacket_t *)(frame + sizeof(crsf_ext_header_t) + size); - // Packet starts with device name - memcpy(frame + sizeof(crsf_ext_header_t), device_name, size); - // Followed by the device - device->serialNo = htobe32(0x454C5253); // ['E', 'L', 'R', 'S'], seen [0x00, 0x0a, 0xe7, 0xc6] // "Serial 177-714694" (value is 714694) - device->hardwareVer = 0; // unused currently by us, seen [ 0x00, 0x0b, 0x10, 0x01 ] // "Hardware: V 1.01" / "Bootloader: V 3.06" - device->softwareVer = htobe32(VersionStrToU32(version)); // seen [ 0x00, 0x00, 0x05, 0x0f ] // "Firmware: V 5.15" - device->fieldCnt = fieldCount; - device->parameterVersion = 0; -} - -void CRSF::SetMspV2Request(uint8_t *frame, uint16_t function, uint8_t *payload, uint8_t payloadLength) -{ - uint8_t *packet = (uint8_t *)(frame + sizeof(crsf_ext_header_t)); - packet[0] = 0x50; // no error, version 2, beginning of the frame, first frame (0) - packet[1] = 0; // flags - packet[2] = function & 0xFF; - packet[3] = (function >> 8) & 0xFF; - packet[4] = payloadLength & 0xFF; - packet[5] = (payloadLength >> 8) & 0xFF; - memcpy(packet + 6, payload, payloadLength); - packet[6 + payloadLength] = CalcCRCMsp(packet + 1, payloadLength + 5); // crc = flags + function + length + payload -} - -void CRSF::SetHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t destAddr) -{ - crsf_header_t *header = (crsf_header_t *)frame; - header->device_addr = destAddr; - header->frame_size = frameSize; - header->type = frameType; - - uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], frameSize - 1, 0); - frame[frameSize + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc; -} - -void CRSF::SetExtendedHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t senderAddr, uint8_t destAddr) -{ - crsf_ext_header_t *header = (crsf_ext_header_t *)frame; - header->dest_addr = destAddr; - header->orig_addr = senderAddr; - SetHeaderAndCrc(frame, frameType, frameSize, destAddr); -} diff --git a/src/lib/Handset/CRSFHandset.h b/src/lib/Handset/CRSFHandset.h new file mode 100644 index 0000000000..2ce8ad7cf6 --- /dev/null +++ b/src/lib/Handset/CRSFHandset.h @@ -0,0 +1,90 @@ +#ifndef H_CRSF_CONTROLLER +#define H_CRSF_CONTROLLER + +#include "handset.h" +#include "crsf_protocol.h" +#ifndef TARGET_NATIVE +#include "HardwareSerial.h" +#endif +#include "common.h" + +#ifdef PLATFORM_ESP32 +#include "driver/uart.h" +#endif + +class CRSFHandset final : public Handset +{ + +public: + /////Variables///// + void Begin() override; + void End() override; + +#ifdef CRSF_TX_MODULE + bool IsArmed() override { return CRSF_to_BIT(ChannelData[4]); } // AUX1 + void handleInput() override; + void handleOutput(); + + static HardwareSerial Port; + static Stream *PortSecondary; // A second UART used to mirror telemetry out on the TX, not read from + + static uint8_t modelId; // The model ID as received from the Transmitter + static bool ForwardDevicePings; // true if device pings should be forwarded OTA + static bool elrsLUAmode; + + static uint32_t GoodPktsCountResult; // need to latch the results + static uint32_t BadPktsCountResult; // need to latch the results + + static void makeLinkStatisticsPacket(uint8_t buffer[LinkStatisticsFrameLength + 4]); + + static void packetQueueExtended(uint8_t type, void *data, uint8_t len); + + void setPacketInterval(int32_t PacketInterval) override; + void JustSentRFpacket() override; + void sendTelemetryToTX(uint8_t *data) override; + + static uint8_t getModelID() { return modelId; } + + uint8_t GetMaxPacketBytes() const override { return maxPacketBytes; } + static uint32_t GetCurrentBaudRate() { return UARTrequestedBaud; } + +private: + inBuffer_U inBuffer = {}; + + /// OpenTX mixer sync /// + volatile uint32_t dataLastRecv = 0; + volatile int32_t OpenTXsyncOffset = 0; + volatile int32_t OpenTXsyncWindow = 0; + volatile int32_t OpenTXsyncWindowSize = 0; + uint32_t OpenTXsyncLastSent = 0; + + /// UART Handling /// + uint8_t SerialInPacketLen = 0; // length of the CRSF packet as measured + uint8_t SerialInPacketPtr = 0; // index where we are reading/writing + bool CRSFframeActive = false; // since we get a copy of the serial data use this flag to know when to ignore it + uint32_t GoodPktsCount = 0; + uint32_t BadPktsCount = 0; + uint32_t UARTwdtLastChecked = 0; + uint8_t maxPacketBytes = CRSF_MAX_PACKET_LEN; + uint8_t maxPeriodBytes = CRSF_MAX_PACKET_LEN; + + static uint8_t UARTcurrentBaudIdx; + static uint32_t UARTrequestedBaud; + +#if defined(PLATFORM_ESP32) + bool UARTinverted = false; +#endif + + void sendSyncPacketToTX(); + void adjustMaxPacketSize(); + void duplex_set_RX() const; + void duplex_set_TX() const; + void RcPacketToChannelsData(); + bool ProcessPacket(); + bool UARTwdt(); + uint32_t autobaud(); + void flush_port_input(); +#endif +}; + +#endif diff --git a/src/lib/Handset/PPMHandset.cpp b/src/lib/Handset/PPMHandset.cpp new file mode 100644 index 0000000000..e855cdbdfc --- /dev/null +++ b/src/lib/Handset/PPMHandset.cpp @@ -0,0 +1,76 @@ +#include "targets.h" + +#if defined(PLATFORM_ESP32) && defined(TARGET_TX) + +#include "PPMHandset.h" +#include "common.h" +#include "crsf_protocol.h" +#include "logging.h" + +#include + +constexpr rmt_channel_t PPM_RMT_CHANNEL = RMT_CHANNEL_0; +constexpr auto RMT_TICKS_PER_US = 10; + +void PPMHandset::Begin() +{ + constexpr auto divisor = 80 / RMT_TICKS_PER_US; + + rmt_config_t rmt_rx_config = RMT_DEFAULT_CONFIG_RX(static_cast(GPIO_PIN_RCSIGNAL_RX), PPM_RMT_CHANNEL); + rmt_rx_config.clk_div = divisor; + rmt_rx_config.rx_config.idle_threshold = min(65000, RMT_TICKS_PER_US * 4000); // greater than 4ms triggers end of frame + rmt_config(&rmt_rx_config); + rmt_driver_install(PPM_RMT_CHANNEL, 1000, 0); + + rmt_get_ringbuf_handle(PPM_RMT_CHANNEL, &rb); + rmt_rx_start(PPM_RMT_CHANNEL, true); + lastPPM = 0; + + if (connected) + { + connected(); + } +} + +void PPMHandset::End() +{ + rmt_driver_uninstall(PPM_RMT_CHANNEL); +} + +bool PPMHandset::IsArmed() +{ + bool maybeArmed = numChannels < 5 || CRSF_to_BIT(ChannelData[4]); + return maybeArmed && lastPPM; +} + +void PPMHandset::handleInput() +{ + const auto now = millis(); + size_t length = 0; + + auto *items = static_cast(xRingbufferReceive(rb, &length, 0)); + if (items) + { + length /= 4; // one RMT = 4 Bytes + numChannels = length; + for (int i = 0; i < length; i++) + { + const auto item = items[i]; + const auto ppm = (item.duration0 + item.duration1) / RMT_TICKS_PER_US; + ChannelData[i] = fmap(ppm, 988, 2012, CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX); + } + vRingbufferReturnItem(rb, static_cast(items)); + lastPPM = now; + } + else if (lastPPM && now - 1000 > lastPPM) + { + DBGLN("PPM signal lost, disarming"); + if (disconnected) + { + disconnected(); + } + lastPPM = 0; + } +} + +#endif \ No newline at end of file diff --git a/src/lib/Handset/PPMHandset.h b/src/lib/Handset/PPMHandset.h new file mode 100644 index 0000000000..49a8be51c6 --- /dev/null +++ b/src/lib/Handset/PPMHandset.h @@ -0,0 +1,18 @@ +#pragma once +#include "handset.h" + +#include + +class PPMHandset final : public Handset +{ +public: + void Begin() override; + void End() override; + bool IsArmed() override; + void handleInput() override; + +private: + uint32_t lastPPM = 0; + size_t numChannels = 0; + RingbufHandle_t rb = nullptr; +}; \ No newline at end of file diff --git a/src/lib/Handset/devHandset.cpp b/src/lib/Handset/devHandset.cpp new file mode 100644 index 0000000000..e1892ca9ef --- /dev/null +++ b/src/lib/Handset/devHandset.cpp @@ -0,0 +1,58 @@ +#include "targets.h" + +#ifdef TARGET_TX + +#include "CRSF.h" +#include "CRSFHandset.h" +#include "POWERMGNT.h" +#include "devHandset.h" + +#if defined(PLATFORM_ESP32) +#include "AutoDetect.h" +#endif + +Handset *handset; + +static void initialize() +{ +#if defined(PLATFORM_ESP32) + if (GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) + { + handset = new AutoDetect(); + return; + } +#endif + handset = new CRSFHandset(); +} + +static int start() +{ + handset->Begin(); +#if defined(DEBUG_TX_FREERUN) + if (!handset->connect()) + { + ERRLN("CRSF::connected has not been initialised"); + } +#endif + return DURATION_IMMEDIATELY; +} + +static int timeout() +{ + handset->handleInput(); + return DURATION_IMMEDIATELY; +} + +static int event() +{ + // An event should be generated every time the TX power changes + CRSF::LinkStatistics.uplink_TX_Power = powerToCrsfPower(PowerLevelContainer::currPower()); + return DURATION_IGNORE; +} + +device_t Handset_device = { + .initialize = initialize, + .start = start, + .event = event, + .timeout = timeout}; +#endif diff --git a/src/lib/Handset/devHandset.h b/src/lib/Handset/devHandset.h new file mode 100644 index 0000000000..e08e4e6802 --- /dev/null +++ b/src/lib/Handset/devHandset.h @@ -0,0 +1,5 @@ +#pragma once + +#include "device.h" + +extern device_t Handset_device; \ No newline at end of file diff --git a/src/lib/Handset/handset.h b/src/lib/Handset/handset.h new file mode 100644 index 0000000000..fa1ba7cf9d --- /dev/null +++ b/src/lib/Handset/handset.h @@ -0,0 +1,105 @@ +#pragma once + +#include "targets.h" + +/** + * @brief Abstract class that is extended to provide an interface to a handset. + * + * There are three implementations of the Handset class + * + * - CRSFHandset - implements the CRSF protocol for communicating with the handset + * - PPMHandset - PPM protocol, can be connected to the DSC/trainer port for simple non-CRSF handsets + * - AutoDetect - this implementation uses an RMT channel to auto-detect a PPM or CRSF handset and swap the + * global `handset` variable to point to instance of the actual implementation. This allows a TX module + * to be moved between a CRSF capable handset and PPM only handset e.g. an EdgeTX radio and a surface radio. + */ +class Handset +{ +public: + /** + * @brief Start the handset protocol + */ + virtual void Begin() = 0; + + /** + * @brief End the handset protocol + */ + virtual void End() = 0; + + /** + * @brief register a function to be called when the protocol has read an RC data packet from the handset + * @param callback + */ + void setRCDataCallback(void (*callback)()) { RCdataCallback = callback; } + /** + * @brief register a function to be called when a request to update a parameter is send from the handset + * @param callback + */ + void registerParameterUpdateCallback(void (*callback)(uint8_t type, uint8_t index, uint8_t arg)) { RecvParameterUpdate = callback; } + /** + * Register callback functions for state information about the connection or handset + * @param connectedCallback called when the protocol detects a stable connection to the handset + * @param disconnectedCallback called when the protocol loses its connection to the handset + * @param RecvModelUpdateCallback called when the handset sends a message to set the current model number + */ + void registerCallbacks(void (*connectedCallback)(), void (*disconnectedCallback)(), void (*RecvModelUpdateCallback)()) + { + connected = connectedCallback; + disconnected = disconnectedCallback; + RecvModelUpdate = RecvModelUpdateCallback; + } + + /** + * @brief Process any pending input data from the handset + */ + virtual void handleInput() = 0; + + /** + * @return true if the protocol detects that the arming state is active + */ + virtual bool IsArmed() = 0; + + /** + * Called to set the expected packet interval from the handset. + * This can be used to synchronise the packets from the handset. + * @param PacketInterval in microseconds + */ + virtual void setPacketInterval(int32_t PacketInterval) { RequestedRCpacketInterval = PacketInterval; } + /** + * @return the maximum number of bytes that the protocol can send to the handset in a single message + */ + virtual uint8_t GetMaxPacketBytes() const { return 255; } + + /** + * @brief Called to indicate to the protocol that a packet has just been sent over-the-air + * This is used to synchronise the packets from the handset to the OTA protocol to minimise latency + */ + virtual void JustSentRFpacket() {} + /** + * Send a telemetry packet back to the handset + * @param data + */ + virtual void sendTelemetryToTX(uint8_t *data) {} + + /** + * @return the time in microseconds when the last RC packet was received from the handset + */ + uint32_t GetRCdataLastRecv() const { return RCdataLastRecv; } + +protected: + virtual ~Handset() = default; + + bool controllerConnected = false; + void (*RCdataCallback)() = nullptr; // called when there is new RC data + void (*disconnected)() = nullptr; // called when RC packet stream is lost + void (*connected)() = nullptr; // called when RC packet stream is regained + void (*RecvModelUpdate)() = nullptr; // called when model id changes, ie command from Radio + void (*RecvParameterUpdate)(uint8_t type, uint8_t index, uint8_t arg) = nullptr; // called when recv parameter update req, ie from LUA + + volatile uint32_t RCdataLastRecv = 0; + int32_t RequestedRCpacketInterval = 5000; // default to 200hz as per 'normal' +}; + +#ifdef TARGET_TX +extern Handset *handset; +#endif diff --git a/src/lib/LED/esp32rgb.cpp b/src/lib/LED/esp32rgb.cpp index f9f4958776..f90fcb5073 100644 --- a/src/lib/LED/esp32rgb.cpp +++ b/src/lib/LED/esp32rgb.cpp @@ -89,7 +89,7 @@ static const int bitorder[] = {0x40, 0x80, 0x10, 0x20, 0x04, 0x08, 0x01, 0x02}; #elif defined(CONFIG_IDF_TARGET_ESP32S3) static const int bitorder[] = {0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01}; #elif defined(CONFIG_IDF_TARGET_ESP32) -static const int bitorder[] = {0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01}; +static const int bitorder[] = {0x40, 0x80, 0x10, 0x20, 0x04, 0x08, 0x01, 0x02}; #endif void ESP32S3LedDriverGRB::SetPixelColor(uint16_t indexPixel, RgbColor color) diff --git a/src/lib/LUA/lua.cpp b/src/lib/LUA/lua.cpp index 5ef1704f73..d27ace4ed4 100644 --- a/src/lib/LUA/lua.cpp +++ b/src/lib/LUA/lua.cpp @@ -5,10 +5,10 @@ #ifdef TARGET_RX #include "telemetry.h" -#endif -#ifdef TARGET_RX extern Telemetry telemetry; +#else +#include "CRSFHandset.h" #endif //LUA VARIABLES// @@ -142,7 +142,7 @@ static uint8_t sendCRSFparam(crsf_frame_type_e frameType, uint8_t fieldChunk, st #ifdef TARGET_TX // Set the hidden flag chunkBuffer[3] |= luaData->type & CRSF_FIELD_HIDDEN ? 0x80 : 0; - if (CRSF::elrsLUAmode) { + if (CRSFHandset::elrsLUAmode) { chunkBuffer[3] |= luaData->type & CRSF_FIELD_ELRS_HIDDEN ? 0x80 : 0; } #else @@ -195,7 +195,7 @@ static uint8_t sendCRSFparam(crsf_frame_type_e frameType, uint8_t fieldChunk, st // 6 bytes CRSF header/CRC: Dest, Len, Type, ExtSrc, ExtDst, CRC // 2 bytes Lua chunk header: FieldId, ChunksRemain #ifdef TARGET_TX - uint8_t chunkMax = CRSF::GetMaxPacketBytes() - 6 - 2; + uint8_t chunkMax = handset->GetMaxPacketBytes() - 6 - 2; #else uint8_t chunkMax = CRSF_MAX_PACKET_LEN - 6 - 2; #endif @@ -209,7 +209,7 @@ static uint8_t sendCRSFparam(crsf_frame_type_e frameType, uint8_t fieldChunk, st chunkStart[0] = luaData->id; // FieldId chunkStart[1] = chunkCnt - (fieldChunk + 1); // ChunksRemain #ifdef TARGET_TX - CRSF::packetQueueExtended(frameType, chunkStart, chunkSize + 2); + CRSFHandset::packetQueueExtended(frameType, chunkStart, chunkSize + 2); #else memcpy(paramInformation + sizeof(crsf_ext_header_t),chunkStart,chunkSize + 2); @@ -259,7 +259,7 @@ static void updateElrsFlags() { setLuaWarningFlag(LUA_FLAG_MODEL_MATCH, connectionState == connected && connectionHasModelMatch == false); setLuaWarningFlag(LUA_FLAG_CONNECTED, connectionState == connected); - setLuaWarningFlag(LUA_FLAG_ISARMED, CRSF::IsArmed()); + setLuaWarningFlag(LUA_FLAG_ISARMED, handset->IsArmed()); } void sendELRSstatus() @@ -287,13 +287,13 @@ void sendELRSstatus() uint8_t buffer[sizeof(tagLuaElrsParams) + strlen(warningInfo) + 1]; struct tagLuaElrsParams * const params = (struct tagLuaElrsParams *)buffer; - params->pktsBad = CRSF::BadPktsCountResult; - params->pktsGood = htobe16(CRSF::GoodPktsCountResult); + params->pktsBad = CRSFHandset::BadPktsCountResult; + params->pktsGood = htobe16(CRSFHandset::GoodPktsCountResult); params->flags = luaWarningFlags; // to support sending a params.msg, buffer should be extended by the strlen of the message // and copied into params->msg (with trailing null) strcpy(params->msg, warningInfo); - CRSF::packetQueueExtended(0x2E, &buffer, sizeof(buffer)); + CRSFHandset::packetQueueExtended(0x2E, &buffer, sizeof(buffer)); } void luaRegisterDevicePingCallback(void (*callback)()) @@ -303,7 +303,7 @@ void luaRegisterDevicePingCallback(void (*callback)()) #endif -void ICACHE_RAM_ATTR luaParamUpdateReq(uint8_t type, uint8_t index, uint8_t arg) +void luaParamUpdateReq(uint8_t type, uint8_t index, uint8_t arg) { parameterType = type; parameterIndex = index; @@ -423,7 +423,7 @@ void sendLuaDevicePacket(void) CRSF::GetDeviceInformation(deviceInformation, lastLuaField); // does append header + crc again so substract size from length #ifdef TARGET_TX - CRSF::packetQueueExtended(CRSF_FRAMETYPE_DEVICE_INFO, deviceInformation + sizeof(crsf_ext_header_t), DEVICE_INFORMATION_PAYLOAD_LENGTH); + CRSFHandset::packetQueueExtended(CRSF_FRAMETYPE_DEVICE_INFO, deviceInformation + sizeof(crsf_ext_header_t), DEVICE_INFORMATION_PAYLOAD_LENGTH); #else CRSF::SetExtendedHeaderAndCrc(deviceInformation, CRSF_FRAMETYPE_DEVICE_INFO, DEVICE_INFORMATION_FRAME_SIZE, CRSF_ADDRESS_CRSF_RECEIVER, CRSF_ADDRESS_CRSF_TRANSMITTER); telemetry.AppendTelemetryPackage(deviceInformation); diff --git a/src/lib/LUA/tx_devLUA.cpp b/src/lib/LUA/tx_devLUA.cpp index 1fa3d14a2c..828e40e956 100644 --- a/src/lib/LUA/tx_devLUA.cpp +++ b/src/lib/LUA/tx_devLUA.cpp @@ -2,6 +2,7 @@ #include "rxtx_devLua.h" #include "CRSF.h" +#include "CRSFHandset.h" #include "logging.h" #include "OTA.h" #include "FHSS.h" @@ -289,7 +290,7 @@ extern void setWifiUpdateMode(); #endif static void luadevUpdateModelID() { - itoa(CRSF::getModelID(), modelMatchUnit+6, 10); + itoa(CRSFHandset::getModelID(), modelMatchUnit+6, 10); strcat(modelMatchUnit, ")"); } @@ -534,9 +535,9 @@ static void updateFolderName_VtxAdmin() ****/ static void luadevUpdateBadGood() { - itoa(CRSF::BadPktsCountResult, luaBadGoodString, 10); + itoa(CRSFHandset::BadPktsCountResult, luaBadGoodString, 10); strcat(luaBadGoodString, "/"); - itoa(CRSF::GoodPktsCountResult, luaBadGoodString + strlen(luaBadGoodString), 10); + itoa(CRSFHandset::GoodPktsCountResult, luaBadGoodString + strlen(luaBadGoodString), 10); } /*** @@ -638,8 +639,8 @@ static void registerLuaParameters() msp.makeCommand(); msp.function = MSP_SET_RX_CONFIG; msp.addByte(MSP_ELRS_MODEL_ID); - msp.addByte(newModelMatch ? CRSF::getModelID() : 0xff); - CRSF::AddMspMessage(&msp); + msp.addByte(newModelMatch ? CRSFHandset::getModelID() : 0xff); + CRSF::AddMspMessage(&msp, CRSF_ADDRESS_CRSF_RECEIVER); } luadevUpdateModelID(); }); @@ -838,7 +839,7 @@ static int start() { return DURATION_NEVER; } - CRSF::RecvParameterUpdate = &luaParamUpdateReq; + handset->registerParameterUpdateCallback(luaParamUpdateReq); registerLuaParameters(); setLuaStringValue(&luaInfo, luaBadGoodString); diff --git a/src/lib/MSPVTX/devMSPVTX.cpp b/src/lib/MSPVTX/devMSPVTX.cpp index 7771b880b0..4548a854c3 100644 --- a/src/lib/MSPVTX/devMSPVTX.cpp +++ b/src/lib/MSPVTX/devMSPVTX.cpp @@ -4,6 +4,7 @@ #include "devVTXSPI.h" #include "freqTable.h" #include "CRSF.h" +#include "msptypes.h" #include "hwTimer.h" /** diff --git a/src/lib/OTA/OTA.cpp b/src/lib/OTA/OTA.cpp index ac05e5d79f..8555e7ae1f 100644 --- a/src/lib/OTA/OTA.cpp +++ b/src/lib/OTA/OTA.cpp @@ -8,7 +8,8 @@ #include "OTA.h" #include "common.h" -#include +#include "CRSF.h" +#include static_assert(sizeof(OTA_Packet4_s) == OTA4_PACKET_SIZE, "OTA4 packet stuct is invalid!"); static_assert(sizeof(OTA_Packet8_s) == OTA8_PACKET_SIZE, "OTA8 packet stuct is invalid!"); diff --git a/src/lib/OTA/OTA.h b/src/lib/OTA/OTA.h index c660ac9a26..efe7ecbc9b 100644 --- a/src/lib/OTA/OTA.h +++ b/src/lib/OTA/OTA.h @@ -2,8 +2,11 @@ #define H_OTA #include + #include "crc.h" -#include "devCRSF.h" +#include "CRSF.h" +#include "crsf_protocol.h" +#include "telemetry_protocol.h" #include "FIFO.h" #define OTA4_PACKET_SIZE 8U diff --git a/src/lib/PWM/PWM_ESP8266.cpp b/src/lib/PWM/PWM_ESP8266.cpp index 7627e056f8..4b34f7651e 100644 --- a/src/lib/PWM/PWM_ESP8266.cpp +++ b/src/lib/PWM/PWM_ESP8266.cpp @@ -42,7 +42,16 @@ void PWMController::setDuty(pwm_channel_t channel, uint16_t duty) void PWMController::setMicroseconds(pwm_channel_t channel, uint16_t microseconds) { - startWaveform8266(pwm_gpio[channel], microseconds, refreshInterval[channel] - microseconds); + int8_t pin = pwm_gpio[channel]; + if (microseconds > 0) { + startWaveform8266(pin, microseconds, refreshInterval[channel] - microseconds); + } + else { + // startWaveform8266 does not handle 0 properly, there's still a 1.2 microsecond pulse + // so we have to explicitly stop the waveform generation + stopWaveform8266(pin); + digitalWrite(pin, LOW); + } } #endif \ No newline at end of file diff --git a/src/lib/SCREEN/devScreen.cpp b/src/lib/SCREEN/devScreen.cpp index e3fcac84d9..abd2f937b7 100644 --- a/src/lib/SCREEN/devScreen.cpp +++ b/src/lib/SCREEN/devScreen.cpp @@ -8,6 +8,7 @@ #include "TFT/tftdisplay.h" #include "devButton.h" +#include "handset.h" FiniteStateMachine state_machine(entry_fsm); @@ -70,7 +71,7 @@ static int handle(void) #endif #ifdef HAS_FIVE_WAY_BUTTON - if (!CRSF::IsArmed()) + if (!handset->IsArmed()) { int key; bool isLongPressed; diff --git a/src/lib/SCREEN/devScreen.h b/src/lib/SCREEN/devScreen.h index 2877a7036f..2e94c3e6a3 100644 --- a/src/lib/SCREEN/devScreen.h +++ b/src/lib/SCREEN/devScreen.h @@ -1,6 +1,5 @@ #pragma once #include "device.h" -#include "CRSF.h" extern device_t Screen_device; diff --git a/src/lib/SCREEN/menu.cpp b/src/lib/SCREEN/menu.cpp index 1ba458175f..37db3bd02b 100644 --- a/src/lib/SCREEN/menu.cpp +++ b/src/lib/SCREEN/menu.cpp @@ -6,7 +6,7 @@ #include "helpers.h" #include "logging.h" #include "POWERMGNT.h" -#include "CRSF.h" +#include "handset.h" #include "OTA.h" #ifdef HAS_THERMAL @@ -79,7 +79,7 @@ static void displayIdleScreen(bool init) message_index_t disp_message; if (connectionState == noCrossfire || connectionState > FAILURE_STATES) { disp_message = MSG_ERROR; - } else if(CRSF::IsArmed()) { + } else if(handset->IsArmed()) { disp_message = MSG_ARMED; } else if(connectionState == connected) { if (connectionHasModelMatch) { diff --git a/src/lib/SX127xDriver/SX127x.cpp b/src/lib/SX127xDriver/SX127x.cpp index c9cec8eebb..f3974c0b62 100644 --- a/src/lib/SX127xDriver/SX127x.cpp +++ b/src/lib/SX127xDriver/SX127x.cpp @@ -334,7 +334,7 @@ void ICACHE_RAM_ATTR SX127xDriver::SetRxTimeoutUs(uint32_t interval) timeoutSymbols = interval / symbolTimeUs; hal.writeRegisterBits(SX127X_REG_SYMB_TIMEOUT_MSB, timeoutSymbols >> 8, SX127X_REG_SYMB_TIMEOUT_MSB_MASK, SX12XX_Radio_All); // set the timeout MSB hal.writeRegister(SX127X_REG_SYMB_TIMEOUT_LSB, timeoutSymbols & 0xFF, SX12XX_Radio_All); - DBGLN("SetRxTimeout(%u), symbolTime=%uus symbols=%u", interval, (uint32_t)symbolTimeUs, timeoutSymbols) + DBGLN("SetRxTimeout(%u), symbolTime=%uus symbols=%u", interval, (uint32_t)symbolTimeUs, timeoutSymbols); } } diff --git a/src/lib/ServoOutput/devServoOutput.cpp b/src/lib/ServoOutput/devServoOutput.cpp index 78ff5917d7..635b256ce4 100644 --- a/src/lib/ServoOutput/devServoOutput.cpp +++ b/src/lib/ServoOutput/devServoOutput.cpp @@ -85,11 +85,19 @@ static void servosFailsafe() for (unsigned ch = 0 ; ch < GPIO_PIN_PWM_OUTPUTS_COUNT ; ++ch) { const rx_config_pwm_t *chConfig = config.GetPwmChannel(ch); - // Note: Failsafe values do not respect the inverted flag, failsafe values are absolute - uint16_t us = chConfig->val.failsafe + SERVO_FAILSAFE_MIN; - // Always write the failsafe position even if the servo has never been started, - // so all the servos go to their expected position - servoWrite(ch, us); + if (chConfig->val.failsafeMode == PWMFAILSAFE_SET_POSITION) { + // Note: Failsafe values do not respect the inverted flag, failsafe values are absolute + uint16_t us = chConfig->val.failsafe + SERVO_FAILSAFE_MIN; + // Always write the failsafe position even if the servo has never been started, + // so all the servos go to their expected position + servoWrite(ch, us); + } + else if (chConfig->val.failsafeMode == PWMFAILSAFE_NO_PULSES) { + servoWrite(ch, 0); + } + else if (chConfig->val.failsafeMode == PWMFAILSAFE_LAST_POSITION) { + // do nothing + } } } diff --git a/src/lib/THERMAL/devThermal.cpp b/src/lib/THERMAL/devThermal.cpp index af3e9f5b37..fab6a86132 100644 --- a/src/lib/THERMAL/devThermal.cpp +++ b/src/lib/THERMAL/devThermal.cpp @@ -60,10 +60,14 @@ uint32_t get_rpm(); static void initialize() { #if defined(HAS_THERMAL) +#if defined(PLATFORM_ESP32_S3) + thermal.init(); +#else if (OPT_HAS_THERMAL_LM75A && GPIO_PIN_SCL != UNDEF_PIN && GPIO_PIN_SDA != UNDEF_PIN) { thermal.init(); } +#endif #endif if (GPIO_PIN_FAN_EN != UNDEF_PIN) { @@ -71,10 +75,12 @@ static void initialize() } } -#if defined(HAS_THERMAL) static void timeoutThermal() { +#if defined(HAS_THERMAL) +#if !defined(PLATFORM_ESP32_S3) if(OPT_HAS_THERMAL_LM75A) +#endif { thermal.handle(); #ifdef HAS_SMART_FAN @@ -90,8 +96,8 @@ static void timeoutThermal() } #endif } -} #endif +} #if defined(PLATFORM_ESP32) static void setFanSpeed() @@ -258,12 +264,7 @@ static int event() static int timeout() { -#if defined(HAS_THERMAL) - if (OPT_HAS_THERMAL_LM75A && GPIO_PIN_SCL != UNDEF_PIN && GPIO_PIN_SDA != UNDEF_PIN) - { - timeoutThermal(); - } -#endif + timeoutThermal(); timeoutFan(); timeoutTacho(); return THERMAL_DURATION; diff --git a/src/lib/THERMAL/thermal.cpp b/src/lib/THERMAL/thermal.cpp index dc1cafa748..484f2173b5 100644 --- a/src/lib/THERMAL/thermal.cpp +++ b/src/lib/THERMAL/thermal.cpp @@ -6,8 +6,11 @@ #include "lm75a.h" LM75A lm75a; #endif +#if defined(PLATFORM_ESP32_S3) +#include "driver/temp_sensor.h" +#endif -uint8_t thermal_threshold_data[] = { +static const uint8_t thermal_threshold_data[] = { THERMAL_FAN_DEFAULT_HIGH_THRESHOLD, THERMAL_FAN_DEFAULT_LOW_THRESHOLD, THERMAL_FAN_ALWAYS_ON_HIGH_THRESHOLD, @@ -16,25 +19,34 @@ uint8_t thermal_threshold_data[] = { THERMAL_FAN_OFF_LOW_THRESHOLD }; -int thermal_status = THERMAL_STATUS_FAIL; +static Thermal_Status_t thermal_status = THERMAL_STATUS_FAIL; void Thermal::init() { int status = -1; -#ifdef HAS_THERMAL_LM75A - status = lm75a.init(); -#endif - if(status == -1) + if (OPT_HAS_THERMAL_LM75A) { - ERRLN("Thermal failed!"); + status = lm75a.init(); } - else +#if defined(PLATFORM_ESP32_S3) + if (status == -1) { - DBGLN("Thermal OK!"); - temp_value = 0; - thermal_status = THERMAL_STATUS_NORMAL; - update_threshold(0); + temp_sensor_config_t temp_sensor = TSENS_CONFIG_DEFAULT(); + temp_sensor.dac_offset = TSENS_DAC_L2; //TSENS_DAC_L2 is default L4(-40℃ ~ 20℃), L2(-10℃ ~ 80℃) L1(20℃ ~ 100℃) L0(50℃ ~ 125℃) + temp_sensor_set_config(temp_sensor); + temp_sensor_start(); + } +#else + if (status == -1) + { + ERRLN("Thermal failed!"); + return; } +#endif + DBGLN("Thermal OK!"); + temp_value = 0; + thermal_status = THERMAL_STATUS_NORMAL; + update_threshold(0); } void Thermal::handle() @@ -49,8 +61,15 @@ uint8_t Thermal::read_temp() ERRLN("thermal not ready!"); return 0; } -#ifdef HAS_THERMAL_LM75A - return lm75a.read_lm75a(); + if (OPT_HAS_THERMAL_LM75A) + { + return lm75a.read_lm75a(); + } + +#if defined(PLATFORM_ESP32_S3) + float result = 0; + temp_sensor_read_celsius(&result); + return static_cast(result); #else return 0; #endif @@ -69,17 +88,18 @@ void Thermal::update_threshold(int index) ERRLN("thermal not ready!"); return; } - int size = sizeof(thermal_threshold_data)/sizeof(thermal_threshold_data[0]); + constexpr int size = sizeof(thermal_threshold_data)/sizeof(thermal_threshold_data[0]); if(index > size/2) { ERRLN("thermal index out of range!"); return; } - uint8_t high = thermal_threshold_data[2*index]; - uint8_t low = thermal_threshold_data[2*index+1]; -#ifdef HAS_THERMAL_LM75A - lm75a.update_lm75a_threshold(high, low); -#endif + if (OPT_HAS_THERMAL_LM75A) + { + const uint8_t high = thermal_threshold_data[2*index]; + const uint8_t low = thermal_threshold_data[2*index+1]; + lm75a.update_lm75a_threshold(high, low); + } } #endif \ No newline at end of file diff --git a/src/lib/Telemetry/telemetry.h b/src/lib/Telemetry/telemetry.h index c90d2376b2..d5776daf2f 100644 --- a/src/lib/Telemetry/telemetry.h +++ b/src/lib/Telemetry/telemetry.h @@ -2,7 +2,7 @@ #include #include "crsf_protocol.h" -#include "devCRSF.h" +#include "CRSF.h" enum CustomTelemSubTypeID : uint8_t { CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0, diff --git a/src/lib/VTX/devVTX.cpp b/src/lib/VTX/devVTX.cpp index 571e404f85..b9e417d0bd 100644 --- a/src/lib/VTX/devVTX.cpp +++ b/src/lib/VTX/devVTX.cpp @@ -8,6 +8,7 @@ #include "logging.h" #include "devButton.h" +#include "handset.h" #define PITMODE_OFF 0 #define PITMODE_ON 1 @@ -60,7 +61,7 @@ static void eepromWriteToMSPOut() packet.reset(); packet.function = MSP_EEPROM_WRITE; - CRSF::AddMspMessage(&packet); + CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER); } static void VtxConfigToMSPOut() @@ -87,9 +88,9 @@ static void VtxConfigToMSPOut() } } - CRSF::AddMspMessage(&packet); + CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER); - if (!CRSF::IsArmed()) // Do not send while armed. There is no need to change the video frequency while armed. It can also cause VRx modules to flash up their OSD menu e.g. Rapidfire. + if (!handset->IsArmed()) // Do not send while armed. There is no need to change the video frequency while armed. It can also cause VRx modules to flash up their OSD menu e.g. Rapidfire. { MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP } diff --git a/src/lib/WIFI/wifiJoystick.cpp b/src/lib/WIFI/wifiJoystick.cpp index 89f2057514..6f7e9c2342 100644 --- a/src/lib/WIFI/wifiJoystick.cpp +++ b/src/lib/WIFI/wifiJoystick.cpp @@ -8,6 +8,7 @@ #include #include "common.h" #include "CRSF.h" +#include "handset.h" #include "POWERMGNT.h" #include "hwTimer.h" #include "logging.h" @@ -70,9 +71,9 @@ void WifiJoystick::StartSending(const IPAddress& ip, int32_t updateInterval, uin updateInterval = JOYSTICK_DEFAULT_UPDATE_INTERVAL; } hwTimer::updateInterval(updateInterval); - CRSF::setSyncParams(updateInterval); + handset->setPacketInterval(updateInterval); - CRSF::RCdataCallback = &UpdateValues; + handset->setRCDataCallback(UpdateValues); // Channel Count if (newChannelCount == 0) diff --git a/src/lib/helpers/dummy.c b/src/lib/helpers/dummy.c deleted file mode 100644 index 1f65bcc117..0000000000 --- a/src/lib/helpers/dummy.c +++ /dev/null @@ -1 +0,0 @@ -int __helpers_dummy; diff --git a/src/lib/logging/logging.h b/src/lib/logging/logging.h index 9efa7bcef0..5b85e5f9bf 100644 --- a/src/lib/logging/logging.h +++ b/src/lib/logging/logging.h @@ -59,10 +59,10 @@ void debugFreeInitLogger(); #define DBGW(c) LOGGING_UART.write(c) #ifndef LOG_USE_PROGMEM #define DBG(msg, ...) debugPrintf(msg, ##__VA_ARGS__) - #define DBGLN(msg, ...) { \ + #define DBGLN(msg, ...) do { \ debugPrintf(msg, ##__VA_ARGS__); \ LOGGING_UART.println(); \ - } + } while(0) #else #define DBG(msg, ...) debugPrintf(PSTR(msg), ##__VA_ARGS__) #define DBGLN(msg, ...) { \ diff --git a/src/src/dynpower.cpp b/src/src/dynpower.cpp index e627057e07..826f6eb1c2 100644 --- a/src/src/dynpower.cpp +++ b/src/src/dynpower.cpp @@ -1,8 +1,7 @@ #include #if defined(TARGET_TX) - -#include +#include #include // LQ-based boost defines @@ -89,7 +88,7 @@ void DynamicPower_Update(uint32_t now) // ============= DYNAMIC_POWER_BOOST: Switch-triggered power boost up ============== // Or if telemetry is lost while armed (done up here because dynpower_updated is only updated on telemetry) uint8_t boostChannel = config.GetBoostChannel(); - bool armed = CRSF::IsArmed(); + bool armed = handset->IsArmed(); if ((connectionState == disconnected && armed) || (boostChannel && (CRSF_to_BIT(ChannelData[AUX9 + boostChannel - 1]) == 0))) { diff --git a/src/src/rx-serial/SerialAirPort.cpp b/src/src/rx-serial/SerialAirPort.cpp index 4ae284b019..a713e60f83 100644 --- a/src/src/rx-serial/SerialAirPort.cpp +++ b/src/src/rx-serial/SerialAirPort.cpp @@ -9,7 +9,7 @@ FIFO apInputBuffer; FIFO apOutputBuffer; -uint32_t SerialAirPort::sendRCFrame(bool frameAvailable, uint32_t *channelData) +uint32_t SerialAirPort::sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) { return DURATION_IMMEDIATELY; } diff --git a/src/src/rx-serial/SerialAirPort.h b/src/src/rx-serial/SerialAirPort.h index 09603558fb..0f23c18bad 100644 --- a/src/src/rx-serial/SerialAirPort.h +++ b/src/src/rx-serial/SerialAirPort.h @@ -13,7 +13,7 @@ class SerialAirPort : public SerialIO { void queueLinkStatisticsPacket() override {} void queueMSPFrameTransmission(uint8_t* data) override {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override; int getMaxSerialReadSize() override; void sendQueuedData(uint32_t maxBytesToSend) override; diff --git a/src/src/rx-serial/SerialCRSF.cpp b/src/src/rx-serial/SerialCRSF.cpp index aa88c418b7..14f8e2333d 100644 --- a/src/src/rx-serial/SerialCRSF.cpp +++ b/src/src/rx-serial/SerialCRSF.cpp @@ -1,5 +1,5 @@ #include "SerialCRSF.h" -#include "CRSF.h" +#include "common.h" #include "OTA.h" #include "device.h" #include "telemetry.h" @@ -55,7 +55,7 @@ void SerialCRSF::queueLinkStatisticsPacket() _fifo.unlock(); } -uint32_t SerialCRSF::sendRCFrame(bool frameAvailable, uint32_t *channelData) +uint32_t SerialCRSF::sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) { if (!frameAvailable) return DURATION_IMMEDIATELY; diff --git a/src/src/rx-serial/SerialCRSF.h b/src/src/rx-serial/SerialCRSF.h index f96cd9e760..a5f8234c7a 100644 --- a/src/src/rx-serial/SerialCRSF.h +++ b/src/src/rx-serial/SerialCRSF.h @@ -5,7 +5,7 @@ class SerialCRSF : public SerialIO { explicit SerialCRSF(Stream &out, Stream &in) : SerialIO(&out, &in) {} virtual ~SerialCRSF() {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override; void queueMSPFrameTransmission(uint8_t* data) override; void queueLinkStatisticsPacket() override; void sendQueuedData(uint32_t maxBytesToSend) override; diff --git a/src/src/rx-serial/SerialHoTT_TLM.h b/src/src/rx-serial/SerialHoTT_TLM.h index e62cf98f0f..761b2e0ec0 100644 --- a/src/src/rx-serial/SerialHoTT_TLM.h +++ b/src/src/rx-serial/SerialHoTT_TLM.h @@ -269,7 +269,7 @@ class SerialHoTT_TLM : public SerialIO void queueLinkStatisticsPacket() override {} void queueMSPFrameTransmission(uint8_t *data) override {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override { return DURATION_IMMEDIATELY; }; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override { return DURATION_IMMEDIATELY; }; int getMaxSerialReadSize() override; void sendQueuedData(uint32_t maxBytesToSend) override; diff --git a/src/src/rx-serial/SerialIO.h b/src/src/rx-serial/SerialIO.h index 86aa3037be..babd3ce3be 100644 --- a/src/src/rx-serial/SerialIO.h +++ b/src/src/rx-serial/SerialIO.h @@ -2,6 +2,7 @@ #include "targets.h" #include "FIFO.h" +#include "device.h" /** * @brief Abstract class that is to be extended by implementation classes for different serial protocols on the receiver side. @@ -60,10 +61,11 @@ class SerialIO { * * @param frameAvailable indicates that a new OTA frame of data has been received * since the last call to this function + * @param frameMissed indicates that a frame was not received in the OTA window * @param channelData pointer to the 16 channels of data * @return number of milliseconds to delay before this method is called again */ - virtual uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) = 0; + virtual uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) = 0; /** * @brief send any previously queued data to the serial port stream `_outputPort` diff --git a/src/src/rx-serial/SerialNOOP.h b/src/src/rx-serial/SerialNOOP.h index 9e80eaf4c2..73e76901f7 100644 --- a/src/src/rx-serial/SerialNOOP.h +++ b/src/src/rx-serial/SerialNOOP.h @@ -9,7 +9,7 @@ class SerialNOOP : public SerialIO { void queueLinkStatisticsPacket() override {} void queueMSPFrameTransmission(uint8_t* data) override {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override { return DURATION_NEVER; } + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override { return DURATION_NEVER; } void processSerialInput() override {} diff --git a/src/src/rx-serial/SerialSBUS.cpp b/src/src/rx-serial/SerialSBUS.cpp index 56de4a90b9..4445411771 100644 --- a/src/src/rx-serial/SerialSBUS.cpp +++ b/src/src/rx-serial/SerialSBUS.cpp @@ -8,19 +8,23 @@ #define SBUS_FLAG_SIGNAL_LOSS (1 << 2) #define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) -extern RxConfig config; - +const auto UNCONNECTED_CALLBACK_INTERVAL_MS = 10; const auto SBUS_CALLBACK_INTERVAL_MS = 9; -uint32_t SerialSBUS::sendRCFrame(bool frameAvailable, uint32_t *channelData) +uint32_t SerialSBUS::sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) { static auto sendPackets = false; if ((failsafe && config.GetFailsafeMode() == FAILSAFE_NO_PULSES) || (!sendPackets && connectionState != connected)) { - return SBUS_CALLBACK_INTERVAL_MS; + return UNCONNECTED_CALLBACK_INTERVAL_MS; } sendPackets = true; + if ((!frameAvailable && !frameMissed) || _outputPort->availableForWrite() < 25) + { + return DURATION_IMMEDIATELY; + } + // TODO: if failsafeMode == FAILSAFE_SET_POSITION then we use the set positions rather than the last values crsf_channels_s PackedRCdataOut; @@ -65,7 +69,7 @@ uint32_t SerialSBUS::sendRCFrame(bool frameAvailable, uint32_t *channelData) uint8_t extraData = 0; extraData |= failsafe ? SBUS_FLAG_FAILSAFE_ACTIVE : 0; - extraData |= frameAvailable ? 0 : SBUS_FLAG_SIGNAL_LOSS; + extraData |= frameMissed ? SBUS_FLAG_SIGNAL_LOSS : 0; _outputPort->write(0x0F); // HEADER _outputPort->write((byte *)&PackedRCdataOut, RCframeLength); diff --git a/src/src/rx-serial/SerialSBUS.h b/src/src/rx-serial/SerialSBUS.h index 16fd82b3e2..6143ba8831 100644 --- a/src/src/rx-serial/SerialSBUS.h +++ b/src/src/rx-serial/SerialSBUS.h @@ -3,11 +3,11 @@ class SerialSBUS : public SerialIO { public: explicit SerialSBUS(Stream &out, Stream &in) : SerialIO(&out, &in) {} - virtual ~SerialSBUS() {} + ~SerialSBUS() override = default; void queueLinkStatisticsPacket() override {} void queueMSPFrameTransmission(uint8_t* data) override {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override; private: void processBytes(uint8_t *bytes, uint16_t size) override {}; diff --git a/src/src/rx-serial/SerialSUMD.cpp b/src/src/rx-serial/SerialSUMD.cpp index 0bcc9e90fe..ab62b2fc1e 100644 --- a/src/src/rx-serial/SerialSUMD.cpp +++ b/src/src/rx-serial/SerialSUMD.cpp @@ -9,17 +9,17 @@ const auto SUMD_CALLBACK_INTERVAL_MS = 10; -uint32_t SerialSUMD::sendRCFrame(bool frameAvailable, uint32_t *channelData) +uint32_t SerialSUMD::sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) { if (!frameAvailable) { return DURATION_IMMEDIATELY; } - uint8_t outBuffer[SUMD_FRAME_16CH_LEN]; + uint8_t outBuffer[SUMD_FRAME_16CH_LEN]; - outBuffer[0] = 0xA8; //Graupner - outBuffer[1] = 0x01; //SUMD - outBuffer[2] = 0x10; //16CH + outBuffer[0] = 0xA8; //Graupner + outBuffer[1] = 0x01; //SUMD + outBuffer[2] = 0x10; //16CH uint16_t us = (CRSF_to_US(channelData[0]) << 3); outBuffer[3] = us >> 8; @@ -70,11 +70,11 @@ uint32_t SerialSUMD::sendRCFrame(bool frameAvailable, uint32_t *channelData) outBuffer[33] = us >> 8; outBuffer[34] = us & 0x00ff; - uint16_t crc = crc2Byte.calc(outBuffer, (SUMD_HEADER_SIZE + SUMD_DATA_SIZE_16CH), 0); - outBuffer[35] = (uint8_t)(crc >> 8); - outBuffer[36] = (uint8_t)(crc & 0x00ff); + uint16_t crc = crc2Byte.calc(outBuffer, (SUMD_HEADER_SIZE + SUMD_DATA_SIZE_16CH), 0); + outBuffer[35] = (uint8_t)(crc >> 8); + outBuffer[36] = (uint8_t)(crc & 0x00ff); - _outputPort->write(outBuffer, sizeof(outBuffer)); + _outputPort->write(outBuffer, sizeof(outBuffer)); return SUMD_CALLBACK_INTERVAL_MS; } diff --git a/src/src/rx-serial/SerialSUMD.h b/src/src/rx-serial/SerialSUMD.h index c227a0a7d8..8c8d212dcc 100644 --- a/src/src/rx-serial/SerialSUMD.h +++ b/src/src/rx-serial/SerialSUMD.h @@ -8,7 +8,7 @@ class SerialSUMD : public SerialIO { void queueLinkStatisticsPacket() override {} void queueMSPFrameTransmission(uint8_t* data) override {} - uint32_t sendRCFrame(bool frameAvailable, uint32_t *channelData) override; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override; private: Crc2Byte crc2Byte; diff --git a/src/src/rx-serial/devSerialIO.cpp b/src/src/rx-serial/devSerialIO.cpp index b90e8fe5a2..bb3974fd8d 100644 --- a/src/src/rx-serial/devSerialIO.cpp +++ b/src/src/rx-serial/devSerialIO.cpp @@ -10,12 +10,18 @@ extern SerialIO *serialIO; static volatile bool frameAvailable = false; +static volatile bool frameMissed = false; void ICACHE_RAM_ATTR crsfRCFrameAvailable() { frameAvailable = true; } +void ICACHE_RAM_ATTR crsfRCFrameMissed() +{ + frameMissed = true; +} + static int start() { return DURATION_IMMEDIATELY; @@ -40,9 +46,10 @@ static int timeout() // only send frames if we have a model match if (connectionHasModelMatch) { - duration = serialIO->sendRCFrame(frameAvailable, ChannelData); + duration = serialIO->sendRCFrame(frameAvailable, frameMissed, ChannelData); } frameAvailable = false; + frameMissed = false; // still get telemetry and send link stats if theres no model match serialIO->processSerialInput(); serialIO->sendQueuedData(serialIO->getMaxSerialWriteSize()); diff --git a/src/src/rx-serial/devSerialIO.h b/src/src/rx-serial/devSerialIO.h index 90ac72f8e8..3b0551c8eb 100644 --- a/src/src/rx-serial/devSerialIO.h +++ b/src/src/rx-serial/devSerialIO.h @@ -4,3 +4,4 @@ extern device_t Serial_device; extern void crsfRCFrameAvailable(); +extern void crsfRCFrameMissed(); diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index 504f7fbaec..8dfa08306a 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -730,10 +730,24 @@ void ICACHE_RAM_ATTR HWtimerCallbackTock() PFDloop.intEvent(micros()); // our internal osc just fired - if (ExpressLRS_currAirRate_Modparams->numOfSends > 1 && !(OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends) && LQCalcDVDA.currentIsSet()) + if (ExpressLRS_currAirRate_Modparams->numOfSends > 1 && !(OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends)) { - crsfRCFrameAvailable(); - servoNewChannelsAvailable(); + if (LQCalcDVDA.currentIsSet()) + { + crsfRCFrameAvailable(); + servoNewChannelsAvailable(); + } + else + { + crsfRCFrameMissed(); + } + } + else if (ExpressLRS_currAirRate_Modparams->numOfSends == 1) + { + if (!LQCalc.currentIsSet()) + { + crsfRCFrameMissed(); + } } if (!didFHSS) diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp index 59b431ab39..bc906b5e07 100644 --- a/src/src/tx_main.cpp +++ b/src/src/tx_main.cpp @@ -1,13 +1,15 @@ #include "rxtx_common.h" +#include "CRSFHandset.h" #include "dynpower.h" #include "lua.h" #include "msp.h" +#include "msptypes.h" #include "telemetry_protocol.h" #include "stubborn_receiver.h" #include "stubborn_sender.h" -#include "devCRSF.h" +#include "devHandset.h" #include "devLED.h" #include "devScreen.h" #include "devBuzzer.h" @@ -78,7 +80,7 @@ StubbornSender MspSender; uint8_t CRSFinBuffer[CRSF_MAX_PACKET_LEN+1]; device_affinity_t ui_devices[] = { - {&CRSF_device, 1}, + {&Handset_device, 1}, #ifdef HAS_LED {&LED_device, 0}, #endif @@ -264,7 +266,7 @@ expresslrs_tlm_ratio_e ICACHE_RAM_ATTR UpdateTlmRatioEffective() // If Armed, telemetry is disabled, otherwise use STD else if (ratioConfigured == TLM_RATIO_DISARMED) { - if (CRSF::IsArmed()) + if (handset->IsArmed()) { retVal = TLM_RATIO_NO_TLM; // Avoid updating ExpressLRS_currTlmDenom until connectionState == disconnected @@ -312,26 +314,26 @@ void ICACHE_RAM_ATTR GenerateSyncPacketData(OTA_Sync_s * const syncPtr) // For model match, the last byte of the binding ID is XORed with the inverse of the modelId if (!InBindingMode && config.GetModelMatch()) { - syncPtr->UID5 ^= (~CRSF::getModelID()) & MODELMATCH_MASK; + syncPtr->UID5 ^= (~CRSFHandset::getModelID()) & MODELMATCH_MASK; } } uint8_t adjustPacketRateForBaud(uint8_t rateIndex) { #if defined(RADIO_SX128X) - if (CRSF::GetCurrentBaudRate() == 115200 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 150Hz if we are on 115k baud on external module + if (CRSFHandset::GetCurrentBaudRate() == 115200 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 150Hz if we are on 115k baud on external module { rateIndex = get_elrs_HandsetRate_max(rateIndex, 6666); } - else if (CRSF::GetCurrentBaudRate() == 115200) // Packet rate limited to 250Hz if we are on 115k baud (on internal module) + else if (CRSFHandset::GetCurrentBaudRate() == 115200) // Packet rate limited to 250Hz if we are on 115k baud (on internal module) { rateIndex = get_elrs_HandsetRate_max(rateIndex, 4000); } - else if (CRSF::GetCurrentBaudRate() == 400000 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 333Hz if we are on 400k baud on external module + else if (CRSFHandset::GetCurrentBaudRate() == 400000 && GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX) // Packet rate limited to 333Hz if we are on 400k baud on external module { rateIndex = get_elrs_HandsetRate_max(rateIndex, 3003); } - else if (CRSF::GetCurrentBaudRate() == 400000) // Packet rate limited to 500Hz if we are on 400k baud + else if (CRSFHandset::GetCurrentBaudRate() == 400000) // Packet rate limited to 500Hz if we are on 400k baud { rateIndex = get_elrs_HandsetRate_max(rateIndex, 2000); } @@ -379,7 +381,7 @@ void SetRFLinkRate(uint8_t index) // Set speed of RF link (hz) ExpressLRS_currAirRate_RFperfParams = RFperf; CRSF::LinkStatistics.rf_Mode = ModParams->enum_rate; - CRSF::setSyncParams(interval * ExpressLRS_currAirRate_Modparams->numOfSends); + handset->setPacketInterval(interval * ExpressLRS_currAirRate_Modparams->numOfSends); connectionState = disconnected; rfModeLastChangedMS = millis(); } @@ -467,7 +469,7 @@ void ICACHE_RAM_ATTR SendRCdataToRF() uint32_t SyncInterval = (connectionState == connected && !isTlmDisarmed) ? ExpressLRS_currAirRate_RFperfParams->SyncPktIntervalConnected : ExpressLRS_currAirRate_RFperfParams->SyncPktIntervalDisconnected; bool skipSync = InBindingMode || // TLM_RATIO_DISARMED keeps sending sync packets even when armed until the RX stops sending telemetry and the TLM=Off has taken effect - (isTlmDisarmed && CRSF::IsArmed() && (ExpressLRS_currTlmDenom == 1)); + (isTlmDisarmed && handset->IsArmed() && (ExpressLRS_currTlmDenom == 1)); uint8_t NonceFHSSresult = OtaNonce % ExpressLRS_currAirRate_Modparams->FHSShopInterval; bool WithinSyncSpamResidualWindow = now - rfModeLastChangedMS < syncSpamAResidualTimeMS; @@ -604,7 +606,7 @@ void ICACHE_RAM_ATTR timerCallback() // Sync OpenTX to this point if (!(OtaNonce % ExpressLRS_currAirRate_Modparams->numOfSends)) { - CRSF::JustSentRFpacket(); + handset->JustSentRFpacket(); } // Do not transmit or advance FHSS/Nonce until in disconnected/connected state @@ -651,7 +653,7 @@ void ICACHE_RAM_ATTR timerCallback() // Do not send a stale channels packet to the RX if one has not been received from the handset // *Do* send data if a packet has never been received from handset and the timer is running // this is the case when bench testing and TXing without a handset - uint32_t lastRcData = CRSF::GetRCdataLastRecv(); + uint32_t lastRcData = handset->GetRCdataLastRecv(); if (!lastRcData || (micros() - lastRcData < 1000000)) { busyTransmitting = true; @@ -691,7 +693,7 @@ void ResetPower() // (user may be turning up the power while flying and dropping the power may compromise the link) if (config.GetDynamicPower()) { - if (!CRSF::IsArmed()) + if (!handset->IsArmed()) { // if dynamic power enabled and not armed then set to MinPower POWERMGNT::setPower(MinPower); @@ -722,7 +724,7 @@ static void ChangeRadioParams() void ModelUpdateReq() { // Force synspam with the current rate parameters in case already have a connection established - if (config.SetModelId(CRSF::getModelID())) + if (config.SetModelId(CRSFHandset::getModelID())) { syncSpamCounter = syncSpamAmount; ModelUpdatePending = true; @@ -851,7 +853,7 @@ static void UpdateConnectDisconnectStatus() if (connectionState != connected) { connectionState = connected; - CRSF::ForwardDevicePings = true; + CRSFHandset::ForwardDevicePings = true; DBGLN("got downlink conn"); if (firmwareOptions.is_airport) @@ -867,7 +869,7 @@ static void UpdateConnectDisconnectStatus() { connectionState = disconnected; connectionHasModelMatch = true; - CRSF::ForwardDevicePings = false; + CRSFHandset::ForwardDevicePings = false; } } @@ -897,7 +899,7 @@ static void CheckReadyToSend() if (RxWiFiReadyToSend) { RxWiFiReadyToSend = false; - if (!CRSF::IsArmed()) + if (!handset->IsArmed()) { SendRxWiFiOverMSP(); } @@ -1180,7 +1182,7 @@ static void setupTarget() digitalWrite(GPIO_PIN_ANT_CTRL_FIXED, LOW); // LEFT antenna HardwareSerial *uart2 = new HardwareSerial(USART2); uart2->begin(57600); - CRSF::PortSecondary = uart2; + CRSFHandset::PortSecondary = uart2; #endif #if defined(TARGET_TX_FM30_MINI) @@ -1259,12 +1261,8 @@ void setup() Radio.RXdoneCallback = &RXdoneISR; Radio.TXdoneCallback = &TXdoneISR; - CRSF::connected = &UARTconnected; // it will auto init when it detects UART connection - if (!firmwareOptions.is_airport) - { - CRSF::disconnected = &UARTdisconnected; - } - CRSF::RecvModelUpdate = &ModelUpdateReq; + handset->registerCallbacks(UARTconnected, firmwareOptions.is_airport ? nullptr : UARTdisconnected, ModelUpdateReq); + DBGLN("ExpressLRS TX Module Booted..."); eeprom.Begin(); // Init the eeprom @@ -1399,15 +1397,15 @@ void loop() (now >= (uint32_t)(firmwareOptions.tlm_report_interval + TLMpacketReported))) { // 3 byte header + 1 byte CRC uint8_t linkStatisticsFrame[LinkStatisticsFrameLength + 4]; - CRSF::makeLinkStatisticsPacket(linkStatisticsFrame); - CRSF::sendTelemetryToTX(linkStatisticsFrame); + CRSFHandset::makeLinkStatisticsPacket(linkStatisticsFrame); + handset->sendTelemetryToTX(linkStatisticsFrame); crsfTelemToMSPOut(linkStatisticsFrame); TLMpacketReported = now; } if (TelemetryReceiver.HasFinishedData()) { - CRSF::sendTelemetryToTX(CRSFinBuffer); + handset->sendTelemetryToTX(CRSFinBuffer); crsfTelemToMSPOut(CRSFinBuffer); TelemetryReceiver.Unlock(); } diff --git a/src/test/test_crc/test_crc.cpp b/src/test/test_crc/test_crc.cpp index 5a9ed9539e..f7ab0a7416 100644 --- a/src/test/test_crc/test_crc.cpp +++ b/src/test/test_crc/test_crc.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "ucrc_t.h" #include diff --git a/src/test/test_crc/ucrc_t.h b/src/test/test_crc/ucrc_t.h index 4425076405..213be5cbba 100644 --- a/src/test/test_crc/ucrc_t.h +++ b/src/test/test_crc/ucrc_t.h @@ -44,7 +44,7 @@ #ifndef UCRC_T_H #define UCRC_T_H -#include +#include #include #include // for std::ifstream #include // for std::ios_base, etc. diff --git a/src/test/test_crsf/test_crsf.cpp b/src/test/test_crsf/test_crsf.cpp index e8f42b9ee4..0f0ee126f7 100644 --- a/src/test/test_crsf/test_crsf.cpp +++ b/src/test/test_crsf/test_crsf.cpp @@ -1,10 +1,10 @@ #include -#include #include +#include #include "../test_msp/mock_serial.h" #include "common.h" -#include "devCRSF.h" +#include "CRSF.h" using namespace std; diff --git a/src/test/test_msp/encapsulated_msp_tests.cpp b/src/test/test_msp/encapsulated_msp_tests.cpp index 4839399b34..60d315c05a 100644 --- a/src/test/test_msp/encapsulated_msp_tests.cpp +++ b/src/test/test_msp/encapsulated_msp_tests.cpp @@ -1,9 +1,11 @@ +#include +#include #include #include "msp.h" #include "msptypes.h" #include "mock_serial.h" -#include "devCRSF.h" +#include "CRSF.h" void test_encapsulated_msp_send(void) { @@ -28,7 +30,7 @@ void test_encapsulated_msp_send(void) packet.addByte(0x00); // don't enable pitmode // Ask the CRSF class to send the encapsulated packet to the stream - CRSF::AddMspMessage(&packet); + CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER); uint8_t* data; uint8_t len; @@ -79,7 +81,7 @@ void test_encapsulated_msp_send_too_long(void) packet.addByte(0x05); // Ask the CRSF class to send the encapsulated packet to the stream - CRSF::AddMspMessage(&packet); + CRSF::AddMspMessage(&packet, CRSF_ADDRESS_FLIGHT_CONTROLLER); uint8_t* data; uint8_t len; diff --git a/src/test/test_msp/mock_serial.h b/src/test/test_msp/mock_serial.h index 970a12d586..9b3d085830 100644 --- a/src/test/test_msp/mock_serial.h +++ b/src/test/test_msp/mock_serial.h @@ -21,7 +21,7 @@ class StringStream : public Stream { buf += (char)c; return 1; } - size_t write(uint8_t *c, int l) + size_t write(const uint8_t *c, size_t l) { for (int i=0; i +#include #include #include "msp.h" #include "common.h" diff --git a/src/test/test_msp_vtx/msp_vtx_tests.cpp b/src/test/test_msp_vtx/msp_vtx_tests.cpp index 26adb68532..ca121f344f 100644 --- a/src/test/test_msp_vtx/msp_vtx_tests.cpp +++ b/src/test/test_msp_vtx/msp_vtx_tests.cpp @@ -1,9 +1,10 @@ #include -#include #include +#include #include "common.h" #include "CRSF.h" +#include "msptypes.h" using namespace std; diff --git a/src/test/test_ota/test_switches.cpp b/src/test/test_ota/test_switches.cpp index 74167d1268..48632323f6 100644 --- a/src/test/test_ota/test_switches.cpp +++ b/src/test/test_ota/test_switches.cpp @@ -8,6 +8,8 @@ */ +#include +#include #include #include "targets.h" diff --git a/src/test/test_stubborn/test_stubborn.cpp b/src/test/test_stubborn/test_stubborn.cpp index 22ce32a324..d5fb174e9e 100644 --- a/src/test/test_stubborn/test_stubborn.cpp +++ b/src/test/test_stubborn/test_stubborn.cpp @@ -1,10 +1,10 @@ #include +#include +#include #include #include #include #include -#include -#include #include "targets.h" #include "helpers.h" diff --git a/src/test/test_telemetry/test_telemetry.cpp b/src/test/test_telemetry/test_telemetry.cpp index dca8bee7fe..656f49d2a2 100644 --- a/src/test/test_telemetry/test_telemetry.cpp +++ b/src/test/test_telemetry/test_telemetry.cpp @@ -2,6 +2,8 @@ #include #include +#include "common.h" + Telemetry telemetry; uint32_t ChannelData[CRSF_NUM_CHANNELS]; // Current state of channels, CRSF format