diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 83408e477f..2ac1b2cbb2 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -96,5 +96,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"VTX_TRAMP",
"GHST",
"SCHEDULER_DETERMINISM",
- "TIMING_ACCURACY"
+ "TIMING_ACCURACY",
+ "RX_EXPRESSLRS_SPI",
+ "RX_EXPRESSLRS_PHASELOCK",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index cc8ddb5fe7..acde8b9003 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -95,6 +95,8 @@ typedef enum {
DEBUG_GHST,
DEBUG_SCHEDULER_DETERMINISM,
DEBUG_TIMING_ACCURACY,
+ DEBUG_RX_EXPRESSLRS_SPI,
+ DEBUG_RX_EXPRESSLRS_PHASELOCK,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 5d24400975..53c2eda01b 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -146,6 +146,7 @@ bool cliMode = false;
#include "pg/rx.h"
#include "pg/rx_pwm.h"
#include "pg/rx_spi_cc2500.h"
+#include "pg/rx_spi_expresslrs.h"
#include "pg/serial_uart.h"
#include "pg/sdio.h"
#include "pg/timerio.h"
@@ -5198,6 +5199,10 @@ const cliResourceValue_t resourceTable[] = {
DEFS( OWNER_RX_SPI_CC2500_ANT_SEL, PG_RX_CC2500_SPI_CONFIG, rxCc2500SpiConfig_t, antSelIoTag ),
#endif
#endif
+#if defined(USE_RX_EXPRESSLRS)
+ DEFS( OWNER_RX_SPI_EXPRESSLRS_RESET, PG_RX_EXPRESSLRS_SPI_CONFIG, rxExpressLrsSpiConfig_t, resetIoTag ),
+ DEFS( OWNER_RX_SPI_EXPRESSLRS_BUSY, PG_RX_EXPRESSLRS_SPI_CONFIG, rxExpressLrsSpiConfig_t, busyIoTag ),
+#endif
#endif
#ifdef USE_GYRO_EXTI
DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, MAX_GYRODEV_COUNT ),
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index c56ba3d73e..e47c45f818 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -96,6 +96,7 @@
#include "pg/rx_pwm.h"
#include "pg/rx_spi.h"
#include "pg/rx_spi_cc2500.h"
+#include "pg/rx_spi_expresslrs.h"
#include "pg/sdcard.h"
#include "pg/vcd.h"
#include "pg/vtx_io.h"
@@ -269,6 +270,7 @@ static const char * const lookupTableRxSpi[] = {
"REDPINE",
"FRSKY_X_V2",
"FRSKY_X_LBT_V2",
+ "EXPRESSLRS",
};
#endif
@@ -505,6 +507,24 @@ const char * const lookupTableCMSMenuBackgroundType[] = {
};
#endif
+#ifdef USE_RX_EXPRESSLRS
+static const char* const lookupTableFreqDomain[] = {
+#ifdef USE_RX_SX127X
+ "AU433", "AU915", "EU433", "EU868", "IN866", "FCC915",
+#endif
+#ifdef USE_RX_SX1280
+ "ISM2400",
+#endif
+#if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
+ "NONE",
+#endif
+};
+
+static const char* const lookupTableSwitchMode[] = {
+ "HYBRID", "WIDE",
+};
+#endif
+
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
@@ -626,6 +646,10 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_OSD
LOOKUP_TABLE_ENTRY(lookupTableCMSMenuBackgroundType),
#endif
+#ifdef USE_RX_EXPRESSLRS
+ LOOKUP_TABLE_ENTRY(lookupTableFreqDomain),
+ LOOKUP_TABLE_ENTRY(lookupTableSwitchMode),
+#endif
};
#undef LOOKUP_TABLE_ENTRY
@@ -1649,6 +1673,13 @@ const clivalue_t valueTable[] = {
{ "spektrum_spi_mfg_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, mfgId) },
{ "spektrum_spi_num_channels", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DSM_MAX_CHANNEL_COUNT }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, numChannels) },
#endif
+#ifdef USE_RX_EXPRESSLRS
+ { "expresslrs_uid", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 6, PG_RX_EXPRESSLRS_SPI_CONFIG, offsetof(rxExpressLrsSpiConfig_t, UID) },
+ { "expresslrs_domain", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FREQ_DOMAIN }, PG_RX_EXPRESSLRS_SPI_CONFIG, offsetof(rxExpressLrsSpiConfig_t, domain) },
+ { "expresslrs_rate_index", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RX_EXPRESSLRS_SPI_CONFIG, offsetof(rxExpressLrsSpiConfig_t, rateIndex) },
+ { "expresslrs_switch_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SWITCH_MODE }, PG_RX_EXPRESSLRS_SPI_CONFIG, offsetof(rxExpressLrsSpiConfig_t, switchMode) },
+ { "expresslrs_model_id", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_EXPRESSLRS_SPI_CONFIG, offsetof(rxExpressLrsSpiConfig_t, modelId) },
+#endif
// PG_TIMECONFIG
#ifdef USE_RTC_TIME
diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h
index 32770344e1..3bf0e292a8 100644
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@@ -140,6 +140,10 @@ typedef enum {
TABLE_SIMPLIFIED_TUNING_PIDS_MODE,
#ifdef USE_OSD
TABLE_CMS_BACKGROUND,
+#endif
+#ifdef USE_RX_EXPRESSLRS
+ TABLE_FREQ_DOMAIN,
+ TABLE_SWITCH_MODE,
#endif
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3967c83ec6..8e887aeb56 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -291,3 +291,21 @@ FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float i
const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
return filter->movingSum / denom;
}
+
+// Simple fixed-point lowpass filter based on integer math
+
+int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
+{
+ filter->fp = (filter->fp << filter->beta) - filter->fp;
+ filter->fp += newVal << filter->fpShift;
+ filter->fp >>= filter->beta;
+ int32_t result = filter->fp >> filter->fpShift;
+ return result;
+}
+
+void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
+{
+ filter->fp = 0;
+ filter->beta = beta;
+ filter->fpShift = fpShift;
+}
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index f218b30719..fe00bee8a5 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -110,3 +110,12 @@ float pt3FilterApply(pt3Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);
+
+typedef struct simpleLowpassFilter_s {
+ int32_t fp;
+ int32_t beta;
+ int32_t fpShift;
+} simpleLowpassFilter_t;
+
+int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
+void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);
diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c
index 40d28b0dc9..9643ff1738 100644
--- a/src/main/drivers/resource.c
+++ b/src/main/drivers/resource.c
@@ -109,4 +109,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"PULLDOWN",
"DSHOT_BITBANG",
"SWD",
+ "RX_SPI_EXPRESSLRS_RESET",
+ "RX_SPI_EXPRESSLRS_BUSY",
};
diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h
index 9c70571b26..9a9a569f06 100644
--- a/src/main/drivers/resource.h
+++ b/src/main/drivers/resource.h
@@ -107,6 +107,8 @@ typedef enum {
OWNER_PULLDOWN,
OWNER_DSHOT_BITBANG,
OWNER_SWD,
+ OWNER_RX_SPI_EXPRESSLRS_RESET,
+ OWNER_RX_SPI_EXPRESSLRS_BUSY,
OWNER_TOTAL_COUNT
} resourceOwner_e;
diff --git a/src/main/drivers/rx/expresslrs_driver.c b/src/main/drivers/rx/expresslrs_driver.c
new file mode 100644
index 0000000000..fff6280423
--- /dev/null
+++ b/src/main/drivers/rx/expresslrs_driver.c
@@ -0,0 +1,246 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ *
+ * Authors:
+ * Dominic Clifton/Hydra - Timer-based timeout implementation.
+ * AlessandroAU - stdperiph Timer-based timeout implementation.
+ */
+
+#include
+#include "platform.h"
+
+#ifdef USE_RX_EXPRESSLRS
+
+#include "build/debug.h"
+#include "build/debug_pin.h"
+
+#include "drivers/timer.h"
+#include "drivers/nvic.h"
+#include "drivers/rx/expresslrs_driver.h"
+
+#include "common/maths.h"
+
+#define TIMER_INTERVAL_US_DEFAULT 20000
+#define TICK_TOCK_COUNT 2
+
+TIM_TypeDef *timer;
+
+typedef enum {
+ TICK,
+ TOCK
+} tickTock_e;
+
+typedef struct elrsTimerState_s {
+ bool running;
+ volatile tickTock_e tickTock;
+ uint32_t intervalUs;
+ int32_t frequencyOffsetTicks;
+ int32_t phaseShiftUs;
+} elrsTimerState_t;
+
+// Use a little ram to keep the amount of CPU cycles used in the ISR lower.
+typedef struct elrsPhaseShiftLimits_s {
+ int32_t min;
+ int32_t max;
+} elrsPhaseShiftLimits_t;
+
+elrsPhaseShiftLimits_t phaseShiftLimits;
+
+static elrsTimerState_t timerState = {
+ false,
+ TOCK, // Start on TOCK (in ELRS isTick is initialised to false)
+ TIMER_INTERVAL_US_DEFAULT,
+ 0,
+ 0
+};
+
+void expressLrsTimerDebug(void)
+{
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 2, timerState.frequencyOffsetTicks);
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 3, timerState.phaseShiftUs);
+}
+
+static void expressLrsRecalculatePhaseShiftLimits(void)
+{
+ phaseShiftLimits.max = (timerState.intervalUs / TICK_TOCK_COUNT);
+ phaseShiftLimits.min = -phaseShiftLimits.max;
+}
+
+static uint16_t expressLrsCalculateMaximumExpectedPeriod(uint16_t intervalUs)
+{
+ // The timer reload register must not overflow when frequencyOffsetTicks is added to it.
+ // frequencyOffsetTicks is not expected to be higher than 1/4 of the interval.
+ // also, timer resolution must be as high as possible.
+ const uint16_t maximumExpectedPeriod = (intervalUs / TICK_TOCK_COUNT) + (timerState.intervalUs / 4);
+ return maximumExpectedPeriod;
+}
+
+void expressLrsUpdateTimerInterval(uint16_t intervalUs)
+{
+ timerState.intervalUs = intervalUs;
+ expressLrsRecalculatePhaseShiftLimits();
+
+#ifdef USE_HAL_DRIVER
+ timerReconfigureTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
+ LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1);
+#else
+ configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
+ TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1);
+#endif
+}
+
+void expressLrsUpdatePhaseShift(int32_t newPhaseShift)
+{
+ timerState.phaseShiftUs = constrain(newPhaseShift, phaseShiftLimits.min, phaseShiftLimits.max);
+}
+
+void expressLrsTimerIncreaseFrequencyOffset(void)
+{
+ timerState.frequencyOffsetTicks++;
+}
+
+void expressLrsTimerDecreaseFrequencyOffset(void)
+{
+ timerState.frequencyOffsetTicks--;
+}
+
+void expressLrsTimerResetFrequencyOffset(void)
+{
+ timerState.frequencyOffsetTicks = 0;
+}
+
+static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_t capture)
+{
+ UNUSED(cbRec);
+ UNUSED(capture);
+
+ if (timerState.tickTock == TICK) {
+ dbgPinHi(0);
+
+ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.frequencyOffsetTicks;
+
+#ifdef USE_HAL_DRIVER
+ LL_TIM_SetAutoReload(timer, adjustedPeriod - 1);
+#else
+ TIM_SetAutoreload(timer, adjustedPeriod - 1);
+#endif
+
+ expressLrsOnTimerTickISR();
+
+ timerState.tickTock = TOCK;
+ } else {
+ dbgPinLo(0);
+
+ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.phaseShiftUs + timerState.frequencyOffsetTicks;
+
+#ifdef USE_HAL_DRIVER
+ LL_TIM_SetAutoReload(timer, adjustedPeriod - 1);
+#else
+ TIM_SetAutoreload(timer, adjustedPeriod - 1);
+#endif
+
+ timerState.phaseShiftUs = 0;
+
+ expressLrsOnTimerTockISR();
+
+ timerState.tickTock = TICK;
+ }
+}
+
+bool expressLrsTimerIsRunning(void)
+{
+ return timerState.running;
+}
+
+void expressLrsTimerStop(void)
+{
+#ifdef USE_HAL_DRIVER
+ LL_TIM_DisableIT_UPDATE(timer);
+ LL_TIM_DisableCounter(timer);
+ LL_TIM_SetCounter(timer, 0);
+#else
+ TIM_ITConfig(timer, TIM_IT_Update, DISABLE);
+ TIM_Cmd(timer, DISABLE);
+ TIM_SetCounter(timer, 0);
+#endif
+ timerState.running = false;
+}
+
+void expressLrsTimerResume(void)
+{
+ timerState.tickTock = TOCK;
+
+#ifdef USE_HAL_DRIVER
+ LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT));
+ LL_TIM_SetCounter(timer, 0);
+
+ LL_TIM_ClearFlag_UPDATE(timer);
+ LL_TIM_EnableIT_UPDATE(timer);
+#else
+ TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT));
+ TIM_SetCounter(timer, 0);
+
+ TIM_ClearFlag(timer, TIM_FLAG_Update);
+ TIM_ITConfig(timer, TIM_IT_Update, ENABLE);
+#endif
+
+ timerState.running = true;
+
+#ifdef USE_HAL_DRIVER
+ LL_TIM_EnableCounter(timer);
+ LL_TIM_GenerateEvent_UPDATE(timer);
+#else
+ TIM_Cmd(timer, ENABLE);
+ TIM_GenerateEvent(timer, TIM_EventSource_Update);
+#endif
+}
+
+void expressLrsInitialiseTimer(TIM_TypeDef *t, timerOvrHandlerRec_t *timerUpdateCb)
+{
+ timer = t;
+
+ configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1));
+
+ expressLrsUpdateTimerInterval(timerState.intervalUs);
+
+ timerChOvrHandlerInit(timerUpdateCb, expressLrsOnTimerUpdate);
+
+ timerConfigUpdateCallback(timer, timerUpdateCb);
+}
+
+void expressLrsTimerEnableIRQs(void)
+{
+ uint8_t irq = timerInputIrq(timer);
+
+ // Use the NVIC TIMER priority for now
+#ifdef USE_HAL_DRIVER
+ HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ HAL_NVIC_EnableIRQ(irq);
+#else
+ NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER));
+ NVIC_EnableIRQ(irq);
+#endif
+}
+
+#endif
diff --git a/src/main/drivers/rx/expresslrs_driver.h b/src/main/drivers/rx/expresslrs_driver.h
new file mode 100644
index 0000000000..08e787bc31
--- /dev/null
+++ b/src/main/drivers/rx/expresslrs_driver.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Author: Dominic Clifton / Seriously Pro Racing
+ */
+
+#pragma once
+
+void expressLrsInitialiseTimer(TIM_TypeDef *timer, timerOvrHandlerRec_t *timerUpdateCb);
+void expressLrsTimerEnableIRQs(void);
+void expressLrsUpdateTimerInterval(uint16_t intervalUs);
+void expressLrsUpdatePhaseShift(int32_t newPhaseShift);
+void expressLrsOnTimerTickISR(void);
+void expressLrsOnTimerTockISR(void);
+
+void expressLrsTimerIncreaseFrequencyOffset(void);
+void expressLrsTimerDecreaseFrequencyOffset(void);
+void expressLrsTimerResetFrequencyOffset(void);
+
+void expressLrsTimerStop(void);
+void expressLrsTimerResume(void);
+
+bool expressLrsTimerIsRunning(void);
+
+void expressLrsTimerDebug(void);
+
diff --git a/src/main/drivers/rx/rx_spi.c b/src/main/drivers/rx/rx_spi.c
index cbbee66226..3778fcf2ba 100644
--- a/src/main/drivers/rx/rx_spi.c
+++ b/src/main/drivers/rx/rx_spi.c
@@ -58,6 +58,8 @@ static bool extiLevel = true;
static volatile bool extiHasOccurred = false;
static volatile timeUs_t lastExtiTimeUs = 0;
+static uint32_t spiNormalSpeedMhz = RX_MAX_SPI_CLK_HZ;
+
void rxSpiDevicePreInit(const rxSpiConfig_t *rxSpiConfig)
{
spiPreinitRegister(rxSpiConfig->csnTag, IOCFG_IPU, 1);
@@ -75,9 +77,14 @@ void rxSpiExtiHandler(extiCallbackRec_t* callback)
}
}
+void rxSpiSetNormalSpeedMhz(uint32_t mhz)
+{
+ spiNormalSpeedMhz = mhz;
+}
+
void rxSpiNormalSpeed()
{
- spiSetClkDivisor(dev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ));
+ spiSetClkDivisor(dev, spiCalculateDivider(spiNormalSpeedMhz));
}
void rxSpiStartupSpeed()
@@ -91,6 +98,8 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
return false;
}
+ spiSetAtomicWait(dev);
+
const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag);
IOInit(rxCsPin, OWNER_RX_SPI_CS, 0);
IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG);
@@ -184,4 +193,15 @@ timeUs_t rxSpiGetLastExtiTimeUs(void)
{
return lastExtiTimeUs;
}
+
+bool rxSpiIsBusy(void)
+{
+ return spiIsBusy(dev);
+}
+
+void rxSpiTransferCommandMulti(uint8_t *data, uint8_t length)
+{
+ spiReadWriteBuf(dev, data, data, length);
+}
+
#endif
diff --git a/src/main/drivers/rx/rx_spi.h b/src/main/drivers/rx/rx_spi.h
index 2633bcc93d..8f84ec8d0c 100644
--- a/src/main/drivers/rx/rx_spi.h
+++ b/src/main/drivers/rx/rx_spi.h
@@ -32,6 +32,7 @@ struct rxSpiConfig_s;
void rxSpiDevicePreInit(const struct rxSpiConfig_s *rxSpiConfig);
bool rxSpiDeviceInit(const struct rxSpiConfig_s *rxSpiConfig);
+void rxSpiSetNormalSpeedMhz(uint32_t mhz);
void rxSpiNormalSpeed();
void rxSpiStartupSpeed();
void rxSpiDmaEnable(bool enable);
@@ -47,3 +48,5 @@ bool rxSpiGetExtiState(void);
bool rxSpiPollExti(void);
void rxSpiResetExti(void);
timeUs_t rxSpiGetLastExtiTimeUs(void);
+void rxSpiTransferCommandMulti(uint8_t *data, uint8_t length);
+bool rxSpiIsBusy(void);
diff --git a/src/main/drivers/rx/rx_sx127x.c b/src/main/drivers/rx/rx_sx127x.c
new file mode 100644
index 0000000000..51ab625211
--- /dev/null
+++ b/src/main/drivers/rx/rx_sx127x.c
@@ -0,0 +1,474 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_RX_SX127X
+
+#include "drivers/bus_spi.h"
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/rx/rx_sx127x.h"
+#include "drivers/rx/rx_spi.h"
+#include "drivers/time.h"
+
+static const uint8_t sx127xAllowedSyncwords[105] =
+ {0, 5, 6, 7, 11, 12, 13, 15, 18,
+ 21, 23, 26, 29, 30, 31, 33, 34,
+ 37, 38, 39, 40, 42, 44, 50, 51,
+ 54, 55, 57, 58, 59, 61, 63, 65,
+ 67, 68, 71, 77, 78, 79, 80, 82,
+ 84, 86, 89, 92, 94, 96, 97, 99,
+ 101, 102, 105, 106, 109, 111, 113, 115,
+ 117, 118, 119, 121, 122, 124, 126, 127,
+ 129, 130, 138, 143, 161, 170, 172, 173,
+ 175, 180, 181, 182, 187, 190, 191, 192,
+ 193, 196, 199, 201, 204, 205, 208, 209,
+ 212, 213, 219, 220, 221, 223, 227, 229,
+ 235, 239, 240, 242, 243, 246, 247, 255};
+
+static bool sx127xDetectChip(void)
+{
+ uint8_t i = 0;
+ bool flagFound = false;
+ while ((i < 3) && !flagFound) {
+ uint8_t version = sx127xReadRegister(SX127X_REG_VERSION);
+ if (version == 0x12) {
+ flagFound = true;
+ } else {
+ delay(50);
+ i++;
+ }
+ }
+
+ sx127xSetRegisterValue(SX127X_REG_OP_MODE, SX127x_OPMODE_SLEEP, 2, 0);
+ return flagFound;
+}
+
+uint8_t sx127xISR(timeUs_t *timeStamp)
+{
+ if (rxSpiPollExti()) {
+ if (rxSpiGetLastExtiTimeUs()) {
+ *timeStamp = rxSpiGetLastExtiTimeUs();
+ }
+
+ uint8_t irqReason;
+ irqReason = sx127xGetIrqReason();
+
+ rxSpiResetExti();
+
+ return irqReason;
+ }
+ return 0;
+}
+
+bool sx127xInit(IO_t resetPin, IO_t busyPin)
+{
+ UNUSED(busyPin);
+
+ if (!rxSpiExtiConfigured()) {
+ return false;
+ }
+
+ if (resetPin) {
+ IOInit(resetPin, OWNER_RX_SPI_EXPRESSLRS_RESET, 0);
+ IOConfigGPIO(resetPin, IOCFG_OUT_PP);
+ } else {
+ resetPin = IO_NONE;
+ }
+
+ IOLo(resetPin);
+ delay(50);
+ IOConfigGPIO(resetPin, IOCFG_IN_FLOATING); // leave floating
+
+ return sx127xDetectChip();
+}
+
+void sx127xWriteRegister(const uint8_t address, const uint8_t data)
+{
+ rxSpiWriteCommand(address | SX127x_SPI_WRITE, data);
+}
+
+void sx127xWriteRegisterBurst(const uint8_t address, const uint8_t *data, const uint8_t length)
+{
+ rxSpiWriteCommandMulti(address | SX127x_SPI_WRITE, &data[0], length);
+}
+
+uint8_t sx127xReadRegister(const uint8_t address)
+{
+ return rxSpiReadCommand(address | SX127x_SPI_READ, 0xFF);
+}
+
+void sx127xReadRegisterBurst(const uint8_t address, uint8_t data[], const uint8_t length)
+{
+ rxSpiReadCommandMulti(address | SX127x_SPI_READ, 0xFF, &data[0], length);
+}
+
+uint8_t sx127xGetRegisterValue(const uint8_t reg, const uint8_t msb, const uint8_t lsb)
+{
+ if ((msb > 7) || (lsb > 7) || (lsb > msb)) {
+ return (SX127x_ERR_INVALID_BIT_RANGE);
+ }
+ uint8_t rawValue = sx127xReadRegister(reg);
+ return rawValue & ((0xFF << lsb) & (0xFF >> (7 - msb)));
+}
+
+uint8_t sx127xSetRegisterValue(const uint8_t reg, const uint8_t value, const uint8_t msb, const uint8_t lsb)
+{
+ if ((msb > 7) || (lsb > 7) || (lsb > msb)) {
+ return (SX127x_ERR_INVALID_BIT_RANGE);
+ }
+
+ uint8_t currentValue = sx127xReadRegister(reg);
+ uint8_t mask = ~((0xFF << (msb + 1)) | (0xFF >> (8 - lsb)));
+ uint8_t newValue = (currentValue & ~mask) | (value & mask);
+ sx127xWriteRegister(reg, newValue);
+ return (SX127x_ERR_NONE);
+}
+
+void sx127xReadRegisterFIFO(uint8_t data[], const uint8_t length)
+{
+ sx127xReadRegisterBurst(SX127X_REG_FIFO, data, length);
+}
+
+void sx127xWriteRegisterFIFO(const uint8_t *data, const uint8_t length)
+{
+ sx127xWriteRegisterBurst(SX127X_REG_FIFO, data, length);
+}
+
+void sx127xSetBandwidthCodingRate(const sx127xBandwidth_e bw, const sx127xCodingRate_e cr, const sx127xSpreadingFactor_e sf, const bool headerExplMode, const bool crcEnabled)
+{
+ if (sf == SX127x_SF_6) { // set SF6 optimizations
+ sx127xWriteRegister(SX127X_REG_MODEM_CONFIG_1, bw | cr | SX1278_HEADER_IMPL_MODE);
+ sx127xSetRegisterValue(SX127X_REG_MODEM_CONFIG_2, SX1278_RX_CRC_MODE_OFF, 2, 2);
+ } else {
+ if (headerExplMode) {
+ sx127xWriteRegister(SX127X_REG_MODEM_CONFIG_1, bw | cr | SX1278_HEADER_EXPL_MODE);
+ } else {
+ sx127xWriteRegister(SX127X_REG_MODEM_CONFIG_1, bw | cr | SX1278_HEADER_IMPL_MODE);
+ }
+
+ if (crcEnabled) {
+ sx127xSetRegisterValue(SX127X_REG_MODEM_CONFIG_2, SX1278_RX_CRC_MODE_ON, 2, 2);
+ } else {
+ sx127xSetRegisterValue(SX127X_REG_MODEM_CONFIG_2, SX1278_RX_CRC_MODE_OFF, 2, 2);
+ }
+ }
+
+ if (bw == SX127x_BW_500_00_KHZ) {
+ //datasheet errata recommendation http://caxapa.ru/thumbs/972894/SX1276_77_8_ErrataNote_1.1_STD.pdf
+ sx127xWriteRegister(0x36, 0x02);
+ sx127xWriteRegister(0x3a, 0x64);
+ } else {
+ sx127xWriteRegister(0x36, 0x03);
+ }
+}
+
+static bool sx127xSyncWordOk(const uint8_t syncWord)
+{
+ for (unsigned int i = 0; i < sizeof(sx127xAllowedSyncwords); i++) {
+ if (syncWord == sx127xAllowedSyncwords[i]) {
+ return true;
+ }
+ }
+ return false;
+}
+
+void sx127xSetSyncWord(uint8_t syncWord)
+{
+ while (sx127xSyncWordOk(syncWord) == false) {
+ syncWord++;
+ }
+
+ sx127xWriteRegister(SX127X_REG_SYNC_WORD, syncWord); //TODO: possible bug in original code
+}
+
+void sx127xSetMode(const sx127xRadioOpMode_e mode)
+{
+ sx127xWriteRegister(SX127x_OPMODE_LORA | SX127X_REG_OP_MODE, mode);
+}
+
+void sx127xSetOutputPower(const uint8_t power)
+{
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+ sx127xWriteRegister(SX127X_REG_PA_CONFIG, SX127X_PA_SELECT_BOOST | SX127X_MAX_OUTPUT_POWER | power);
+}
+
+void sx127xSetPreambleLength(const uint8_t preambleLen)
+{
+ sx127xWriteRegister(SX127X_REG_PREAMBLE_LSB, preambleLen);
+}
+
+void sx127xSetSpreadingFactor(const sx127xSpreadingFactor_e sf)
+{
+ sx127xSetRegisterValue(SX127X_REG_MODEM_CONFIG_2, sf | SX127X_TX_MODE_SINGLE, 7, 3);
+ if (sf == SX127x_SF_6) {
+ sx127xSetRegisterValue(SX127X_REG_DETECT_OPTIMIZE, SX127X_DETECT_OPTIMIZE_SF_6, 2, 0);
+ sx127xWriteRegister(SX127X_REG_DETECTION_THRESHOLD, SX127X_DETECTION_THRESHOLD_SF_6);
+ } else {
+ sx127xSetRegisterValue(SX127X_REG_DETECT_OPTIMIZE, SX127X_DETECT_OPTIMIZE_SF_7_12, 2, 0);
+ sx127xWriteRegister(SX127X_REG_DETECTION_THRESHOLD, SX127X_DETECTION_THRESHOLD_SF_7_12);
+ }
+}
+
+void sx127xSetFrequencyHZ(const uint32_t freq)
+{
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+
+ int32_t FRQ = ((uint32_t)(freq / SX127x_FREQ_STEP));
+
+ uint8_t FRQ_MSB = (uint8_t)((FRQ >> 16) & 0xFF);
+ uint8_t FRQ_MID = (uint8_t)((FRQ >> 8) & 0xFF);
+ uint8_t FRQ_LSB = (uint8_t)(FRQ & 0xFF);
+
+ uint8_t outbuff[3] = {FRQ_MSB, FRQ_MID, FRQ_LSB};
+
+ sx127xWriteRegisterBurst(SX127X_REG_FRF_MSB, outbuff, sizeof(outbuff));
+}
+
+void sx127xSetFrequencyReg(const uint32_t freq)
+{
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+
+ uint8_t FRQ_MSB = (uint8_t)((freq >> 16) & 0xFF);
+ uint8_t FRQ_MID = (uint8_t)((freq >> 8) & 0xFF);
+ uint8_t FRQ_LSB = (uint8_t)(freq & 0xFF);
+
+ uint8_t outbuff[3] = {FRQ_MSB, FRQ_MID, FRQ_LSB}; //check speedup
+
+ sx127xWriteRegisterBurst(SX127X_REG_FRF_MSB, outbuff, sizeof(outbuff));
+}
+
+void sx127xTransmitData(const uint8_t *data, const uint8_t length)
+{
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+
+ sx127xWriteRegister(SX127X_REG_FIFO_ADDR_PTR, SX127X_FIFO_TX_BASE_ADDR_MAX);
+ sx127xWriteRegisterFIFO(data, length);
+
+ sx127xSetMode(SX127x_OPMODE_TX);
+}
+
+void sx127xReceiveData(uint8_t *data, const uint8_t length)
+{
+ sx127xReadRegisterFIFO(data, length);
+}
+
+void sx127xStartReceiving(void)
+{
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+ sx127xWriteRegister(SX127X_REG_FIFO_ADDR_PTR, SX127X_FIFO_RX_BASE_ADDR_MAX);
+ sx127xSetMode(SX127x_OPMODE_RXCONTINUOUS);
+}
+
+void sx127xConfig(const sx127xBandwidth_e bw, const sx127xSpreadingFactor_e sf, const sx127xCodingRate_e cr,
+ const uint32_t freq, const uint8_t preambleLen, const bool iqInverted)
+{
+ sx127xConfigLoraDefaults(iqInverted);
+ sx127xSetPreambleLength(preambleLen);
+ sx127xSetOutputPower(SX127x_MAX_POWER);
+ sx127xSetSpreadingFactor(sf);
+ sx127xSetBandwidthCodingRate(bw, cr, sf, false, false);
+ sx127xSetFrequencyReg(freq);
+}
+
+uint32_t sx127xGetCurrBandwidth(const sx127xBandwidth_e bw)
+{
+ switch (bw) {
+ case SX127x_BW_7_80_KHZ:
+ return 7.8E3;
+ case SX127x_BW_10_40_KHZ:
+ return 10.4E3;
+ case SX127x_BW_15_60_KHZ:
+ return 15.6E3;
+ case SX127x_BW_20_80_KHZ:
+ return 20.8E3;
+ case SX127x_BW_31_25_KHZ:
+ return 31.25E3;
+ case SX127x_BW_41_70_KHZ:
+ return 41.7E3;
+ case SX127x_BW_62_50_KHZ:
+ return 62.5E3;
+ case SX127x_BW_125_00_KHZ:
+ return 125E3;
+ case SX127x_BW_250_00_KHZ:
+ return 250E3;
+ case SX127x_BW_500_00_KHZ:
+ return 500E3;
+ }
+ return -1;
+}
+
+// this is basically just used for speedier calc of the freq offset, pre compiled for 32mhz xtal
+uint32_t sx127xGetCurrBandwidthNormalisedShifted(const sx127xBandwidth_e bw)
+{
+ switch (bw) {
+ case SX127x_BW_7_80_KHZ:
+ return 1026;
+ case SX127x_BW_10_40_KHZ:
+ return 769;
+ case SX127x_BW_15_60_KHZ:
+ return 513;
+ case SX127x_BW_20_80_KHZ:
+ return 385;
+ case SX127x_BW_31_25_KHZ:
+ return 256;
+ case SX127x_BW_41_70_KHZ:
+ return 192;
+ case SX127x_BW_62_50_KHZ:
+ return 128;
+ case SX127x_BW_125_00_KHZ:
+ return 64;
+ case SX127x_BW_250_00_KHZ:
+ return 32;
+ case SX127x_BW_500_00_KHZ:
+ return 16;
+ }
+ return -1;
+}
+
+void sx127xSetPPMoffsetReg(const int32_t offset, const uint32_t freq)
+{
+ int8_t offsetPPM = (offset * 1e6 / freq) * 95 / 100;
+ sx127xWriteRegister(SX127x_PPMOFFSET, (uint8_t) offsetPPM);
+}
+
+static bool sx127xGetFrequencyErrorbool(void)
+{
+ return (sx127xReadRegister(SX127X_REG_FEI_MSB) & 0x08) >> 3; // returns true if pos freq error, neg if false
+}
+
+int32_t sx127xGetFrequencyError(const sx127xBandwidth_e bw)
+{
+ uint8_t reg[3] = {0x0, 0x0, 0x0};
+ sx127xReadRegisterBurst(SX127X_REG_FEI_MSB, reg, sizeof(reg));
+
+ uint32_t regFei = ((reg[0] & 0x07) << 16) + (reg[1] << 8) + reg[2];
+
+ int32_t intFreqError = regFei;
+
+ if ((reg[0] & 0x08) >> 3) {
+ intFreqError -= 524288; // Sign bit is on
+ }
+
+ // bit shift hackery so we don't have to use floaty bois; the >> 3 is intentional and is a simplification of the formula on page 114 of sx1276 datasheet
+ int32_t fErrorHZ = (intFreqError >> 3) * (sx127xGetCurrBandwidthNormalisedShifted(bw));
+ fErrorHZ >>= 4;
+
+ return fErrorHZ;
+}
+
+void sx127xAdjustFrequency(int32_t offset, const uint32_t freq)
+{
+ if (sx127xGetFrequencyErrorbool()) { //logic is flipped compared to original code
+ if (offset > SX127x_FREQ_CORRECTION_MIN) {
+ offset -= 1;
+ } else {
+ offset = 0; //reset because something went wrong
+ }
+ } else {
+ if (offset < SX127x_FREQ_CORRECTION_MAX) {
+ offset += 1;
+ } else {
+ offset = 0; //reset because something went wrong
+ }
+ }
+ sx127xSetPPMoffsetReg(offset, freq); //as above but corrects a different PPM offset based on freq error
+}
+
+uint8_t sx127xUnsignedGetLastPacketRSSI(void)
+{
+ return sx127xGetRegisterValue(SX127X_REG_PKT_RSSI_VALUE, 7, 0);
+}
+
+int8_t sx127xGetLastPacketRSSI(void)
+{
+ return (-157 + sx127xGetRegisterValue(SX127X_REG_PKT_RSSI_VALUE, 7, 0));
+}
+
+int8_t sx127xGetCurrRSSI(void)
+{
+ return (-157 + sx127xGetRegisterValue(SX127X_REG_RSSI_VALUE, 7, 0));
+}
+
+int8_t sx127xGetLastPacketSNR(void)
+{
+ int8_t rawSNR = (int8_t)sx127xGetRegisterValue(SX127X_REG_PKT_SNR_VALUE, 7, 0);
+ return (rawSNR / 4);
+}
+
+void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr)
+{
+ *rssi = sx127xGetLastPacketRSSI();
+ *snr = sx127xGetLastPacketSNR();
+}
+
+uint8_t sx127xGetIrqFlags(void)
+{
+ return sx127xGetRegisterValue(SX127X_REG_IRQ_FLAGS, 7, 0);
+}
+
+void sx127xClearIrqFlags(void)
+{
+ sx127xWriteRegister(SX127X_REG_IRQ_FLAGS, 0xFF);
+}
+
+uint8_t sx127xGetIrqReason(void)
+{
+ uint8_t irqFlags = sx127xGetIrqFlags();
+ sx127xClearIrqFlags();
+ if ((irqFlags & SX127X_CLEAR_IRQ_FLAG_TX_DONE)) {
+ return 2;
+ } else if ((irqFlags & SX127X_CLEAR_IRQ_FLAG_RX_DONE)) {
+ return 1;
+ }
+ return 0;
+}
+
+void sx127xConfigLoraDefaults(const bool iqInverted)
+{
+ sx127xWriteRegister(SX127X_REG_OP_MODE, SX127x_OPMODE_SLEEP);
+ sx127xWriteRegister(SX127X_REG_OP_MODE, SX127x_OPMODE_LORA); //must be written in sleep mode
+ sx127xSetMode(SX127x_OPMODE_STANDBY);
+
+ sx127xClearIrqFlags();
+
+ sx127xWriteRegister(SX127X_REG_PAYLOAD_LENGTH, 8);
+ sx127xSetSyncWord(SX127X_SYNC_WORD);
+ sx127xWriteRegister(SX127X_REG_FIFO_TX_BASE_ADDR, SX127X_FIFO_TX_BASE_ADDR_MAX);
+ sx127xWriteRegister(SX127X_REG_FIFO_RX_BASE_ADDR, SX127X_FIFO_RX_BASE_ADDR_MAX);
+ sx127xSetRegisterValue(SX127X_REG_DIO_MAPPING_1, 0xC0, 7, 6); //undocumented "hack", looking at Table 18 from datasheet SX127X_REG_DIO_MAPPING_1 = 11 appears to be unspported by infact it generates an intterupt on both RXdone and TXdone, this saves switching modes.
+ sx127xWriteRegister(SX127X_REG_LNA, SX127X_LNA_BOOST_ON);
+ sx127xWriteRegister(SX1278_REG_MODEM_CONFIG_3, SX1278_AGC_AUTO_ON | SX1278_LOW_DATA_RATE_OPT_OFF);
+ sx127xSetRegisterValue(SX127X_REG_OCP, SX127X_OCP_ON | SX127X_OCP_150MA, 5, 0); //150ma max current
+ sx127xSetPreambleLength(SX127X_PREAMBLE_LENGTH_LSB);
+ sx127xSetRegisterValue(SX127X_REG_INVERT_IQ, (uint8_t)iqInverted, 6, 6);
+}
+
+#endif /* USE_RX_SX127X */
diff --git a/src/main/drivers/rx/rx_sx127x.h b/src/main/drivers/rx/rx_sx127x.h
new file mode 100644
index 0000000000..63a085dd19
--- /dev/null
+++ b/src/main/drivers/rx/rx_sx127x.h
@@ -0,0 +1,338 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#pragma once
+
+//SX127x_ModulationModes
+typedef enum {
+ SX127x_OPMODE_FSK_OOK = 0x00,
+ SX127x_OPMODE_LORA = 0x80,
+ SX127X_ACCESS_SHARED_REG_OFF = 0x00,
+ SX127X_ACCESS_SHARED_REG_ON = 0x40,
+} sx127xModulationMode_e;
+
+//SX127x_RadioOPmodes
+typedef enum {
+ SX127x_OPMODE_SLEEP = 0x00,
+ SX127x_OPMODE_STANDBY = 0x01,
+ SX127x_OPMODE_FSTX = 0x02,
+ SX127x_OPMODE_TX = 0x03,
+ SX127x_OPMODE_FSRX = 0x04,
+ SX127x_OPMODE_RXCONTINUOUS = 0x05,
+ SX127x_OPMODE_RXSINGLE = 0x06,
+ SX127x_OPMODE_CAD = 0x07,
+} sx127xRadioOpMode_e;
+
+//SX127x_Bandwidth
+typedef enum {
+ SX127x_BW_7_80_KHZ = 0x00,
+ SX127x_BW_10_40_KHZ = 0x10,
+ SX127x_BW_15_60_KHZ = 0x20,
+ SX127x_BW_20_80_KHZ = 0x30,
+ SX127x_BW_31_25_KHZ = 0x40,
+ SX127x_BW_41_70_KHZ = 0x50,
+ SX127x_BW_62_50_KHZ = 0x60,
+ SX127x_BW_125_00_KHZ = 0x70,
+ SX127x_BW_250_00_KHZ = 0x80,
+ SX127x_BW_500_00_KHZ = 0x90,
+} sx127xBandwidth_e;
+
+//SX127x_SpreadingFactor
+typedef enum {
+ SX127x_SF_6 = 0x60,
+ SX127x_SF_7 = 0x70,
+ SX127x_SF_8 = 0x80,
+ SX127x_SF_9 = 0x90,
+ SX127x_SF_10 = 0xA0,
+ SX127x_SF_11 = 0xB0,
+ SX127x_SF_12 = 0xC0,
+} sx127xSpreadingFactor_e;
+
+//SX127x_CodingRate
+typedef enum {
+ SX127x_CR_4_5 = 0x02,
+ SX127x_CR_4_6 = 0x04,
+ SX127x_CR_4_7 = 0x06,
+ SX127x_CR_4_8 = 0x08,
+} sx127xCodingRate_e;
+
+// SX127x series common registers
+#define SX127X_REG_FIFO 0x00
+#define SX127X_REG_OP_MODE 0x01
+#define SX127X_REG_FRF_MSB 0x06
+#define SX127X_REG_FRF_MID 0x07
+#define SX127X_REG_FRF_LSB 0x08
+#define SX127X_REG_PA_CONFIG 0x09
+#define SX127X_REG_PA_RAMP 0x0A
+#define SX127X_REG_OCP 0x0B
+#define SX127X_REG_LNA 0x0C
+#define SX127X_REG_FIFO_ADDR_PTR 0x0D
+#define SX127X_REG_FIFO_TX_BASE_ADDR 0x0E
+#define SX127X_REG_FIFO_RX_BASE_ADDR 0x0F
+#define SX127X_REG_FIFO_RX_CURRENT_ADDR 0x10
+#define SX127X_REG_IRQ_FLAGS_MASK 0x11
+#define SX127X_REG_IRQ_FLAGS 0x12
+#define SX127X_REG_RX_NB_BYTES 0x13
+#define SX127X_REG_RX_HEADER_CNT_VALUE_MSB 0x14
+#define SX127X_REG_RX_HEADER_CNT_VALUE_LSB 0x15
+#define SX127X_REG_RX_PACKET_CNT_VALUE_MSB 0x16
+#define SX127X_REG_RX_PACKET_CNT_VALUE_LSB 0x17
+#define SX127X_REG_MODEM_STAT 0x18
+#define SX127X_REG_PKT_SNR_VALUE 0x19
+#define SX127X_REG_PKT_RSSI_VALUE 0x1A
+#define SX127X_REG_RSSI_VALUE 0x1B
+#define SX127X_REG_HOP_CHANNEL 0x1C
+#define SX127X_REG_MODEM_CONFIG_1 0x1D
+#define SX127X_REG_MODEM_CONFIG_2 0x1E
+#define SX127X_REG_SYMB_TIMEOUT_LSB 0x1F
+#define SX127X_REG_PREAMBLE_MSB 0x20
+#define SX127X_REG_PREAMBLE_LSB 0x21
+#define SX127X_REG_PAYLOAD_LENGTH 0x22
+#define SX127X_REG_MAX_PAYLOAD_LENGTH 0x23
+#define SX127X_REG_HOP_PERIOD 0x24
+#define SX127X_REG_FIFO_RX_BYTE_ADDR 0x25
+#define SX127X_REG_FEI_MSB 0x28
+#define SX127X_REG_FEI_MID 0x29
+#define SX127X_REG_FEI_LSB 0x2A
+#define SX127X_REG_RSSI_WIDEBAND 0x2C
+#define SX127X_REG_DETECT_OPTIMIZE 0x31
+#define SX127X_REG_INVERT_IQ 0x33
+#define SX127X_REG_DETECTION_THRESHOLD 0x37
+#define SX127X_REG_SYNC_WORD 0x39
+#define SX127X_REG_DIO_MAPPING_1 0x40
+#define SX127X_REG_DIO_MAPPING_2 0x41
+#define SX127X_REG_VERSION 0x42
+
+// SX127X_REG_PA_CONFIG
+#define SX127X_PA_SELECT_RFO 0x00 // 7 7 RFO pin output, power limited to +14 dBm
+#define SX127X_PA_SELECT_BOOST 0x80 // 7 7 PA_BOOST pin output, power limited to +20 dBm
+#define SX127X_OUTPUT_POWER 0x0F // 3 0 output power: P_out = 17 - (15 - OUTPUT_POWER) [dBm] for PA_SELECT_BOOST
+#define SX127X_MAX_OUTPUT_POWER 0x70 // Enable max output power
+
+// SX127X_REG_OCP
+#define SX127X_OCP_OFF 0x00 // 5 5 PA overload current protection disabled
+#define SX127X_OCP_ON 0x20 // 5 5 PA overload current protection enabled
+#define SX127X_OCP_TRIM 0x0B // 4 0 OCP current: I_max(OCP_TRIM = 0b1011) = 100 mA
+#define SX127X_OCP_150MA 0x12 // 4 0 OCP current: I_max(OCP_TRIM = 10010) = 150 mA
+
+// SX127X_REG_LNA
+#define SX127X_LNA_GAIN_0 0x00 // 7 5 LNA gain setting: not used
+#define SX127X_LNA_GAIN_1 0x20 // 7 5 max gain
+#define SX127X_LNA_GAIN_2 0x40 // 7 5 .
+#define SX127X_LNA_GAIN_3 0x60 // 7 5 .
+#define SX127X_LNA_GAIN_4 0x80 // 7 5 .
+#define SX127X_LNA_GAIN_5 0xA0 // 7 5 .
+#define SX127X_LNA_GAIN_6 0xC0 // 7 5 min gain
+#define SX127X_LNA_GAIN_7 0xE0 // 7 5 not used
+#define SX127X_LNA_BOOST_OFF 0x00 // 1 0 default LNA current
+#define SX127X_LNA_BOOST_ON 0x03 // 1 0 150% LNA current
+
+#define SX127X_TX_MODE_SINGLE 0x00 // 3 3 single TX
+#define SX127X_TX_MODE_CONT 0x08 // 3 3 continuous TX
+#define SX127X_RX_TIMEOUT_MSB 0x00 // 1 0
+
+// SX127X_REG_SYMB_TIMEOUT_LSB
+#define SX127X_RX_TIMEOUT_LSB 0x64 // 7 0 10 bit RX operation timeout
+
+// SX127X_REG_PREAMBLE_MSB + REG_PREAMBLE_LSB
+#define SX127X_PREAMBLE_LENGTH_MSB 0x00 // 7 0 2 byte preamble length setting: l_P = PREAMBLE_LENGTH + 4.25
+#define SX127X_PREAMBLE_LENGTH_LSB 0x08 // 7 0 where l_p = preamble length
+//#define SX127X_PREAMBLE_LENGTH_LSB 0x04 // 7 0 where l_p = preamble length //CHANGED
+
+// SX127X_REG_DETECT_OPTIMIZE
+#define SX127X_DETECT_OPTIMIZE_SF_6 0x05 // 2 0 SF6 detection optimization
+#define SX127X_DETECT_OPTIMIZE_SF_7_12 0x03 // 2 0 SF7 to SF12 detection optimization
+
+// SX127X_REG_DETECTION_THRESHOLD
+#define SX127X_DETECTION_THRESHOLD_SF_6 0x0C // 7 0 SF6 detection threshold
+#define SX127X_DETECTION_THRESHOLD_SF_7_12 0x0A // 7 0 SF7 to SF12 detection threshold
+
+// SX127X_REG_PA_DAC
+#define SX127X_PA_BOOST_OFF 0x04 // 2 0 PA_BOOST disabled
+#define SX127X_PA_BOOST_ON 0x07 // 2 0 +20 dBm on PA_BOOST when OUTPUT_POWER = 0b1111
+
+// SX127X_REG_HOP_PERIOD
+#define SX127X_HOP_PERIOD_OFF 0x00 // 7 0 number of periods between frequency hops; 0 = disabled
+#define SX127X_HOP_PERIOD_MAX 0xFF // 7 0
+
+// SX127X_REG_DIO_MAPPING_1
+#define SX127X_DIO0_RX_DONE 0x00 // 7 6
+#define SX127X_DIO0_TX_DONE 0x40 // 7 6
+#define SX127X_DIO0_CAD_DONE 0x80 // 7 6
+#define SX127X_DIO1_RX_TIMEOUT 0x00 // 5 4
+#define SX127X_DIO1_FHSS_CHANGE_CHANNEL 0x10 // 5 4
+#define SX127X_DIO1_CAD_DETECTED 0x20 // 5 4
+
+// SX127X_REG_IRQ_FLAGS
+#define SX127X_CLEAR_IRQ_FLAG_RX_TIMEOUT 0x80 // 7 7 timeout
+#define SX127X_CLEAR_IRQ_FLAG_RX_DONE 0x40 // 6 6 packet reception complete
+#define SX127X_CLEAR_IRQ_FLAG_PAYLOAD_CRC_ERROR 0x20 // 5 5 payload CRC error
+#define SX127X_CLEAR_IRQ_FLAG_VALID_HEADER 0x10 // 4 4 valid header received
+#define SX127X_CLEAR_IRQ_FLAG_TX_DONE 0x08 // 3 3 payload transmission complete
+#define SX127X_CLEAR_IRQ_FLAG_CAD_DONE 0x04 // 2 2 CAD complete
+#define SX127X_CLEAR_IRQ_FLAG_FHSS_CHANGE_CHANNEL 0x02 // 1 1 FHSS change channel
+#define SX127X_CLEAR_IRQ_FLAG_CAD_DETECTED 0x01 // 0 0 valid LoRa signal detected during CAD operation
+
+// SX127X_REG_IRQ_FLAGS_MASK
+#define SX127X_MASK_IRQ_FLAG_RX_TIMEOUT 0x7F // 7 7 timeout
+#define SX127X_MASK_IRQ_FLAG_RX_DONE 0xBF // 6 6 packet reception complete
+#define SX127X_MASK_IRQ_FLAG_PAYLOAD_CRC_ERROR 0xDF // 5 5 payload CRC error
+#define SX127X_MASK_IRQ_FLAG_VALID_HEADER 0xEF // 4 4 valid header received
+#define SX127X_MASK_IRQ_FLAG_TX_DONE 0xF7 // 3 3 payload transmission complete
+#define SX127X_MASK_IRQ_FLAG_CAD_DONE 0xFB // 2 2 CAD complete
+#define SX127X_MASK_IRQ_FLAG_FHSS_CHANGE_CHANNEL 0xFD // 1 1 FHSS change channel
+#define SX127X_MASK_IRQ_FLAG_CAD_DETECTED 0xFE // 0 0 valid LoRa signal detected during CAD operation
+
+// SX127X_REG_FIFO_TX_BASE_ADDR
+#define SX127X_FIFO_TX_BASE_ADDR_MAX 0x00 // 7 0 allocate the entire FIFO buffer for TX only
+
+// SX127X_REG_FIFO_RX_BASE_ADDR
+#define SX127X_FIFO_RX_BASE_ADDR_MAX 0x00 // 7 0 allocate the entire FIFO buffer for RX only
+
+// SX127X_REG_SYNC_WORD
+//#define SX127X_SYNC_WORD 0xC8 // 200 0 default ExpressLRS sync word - 200Hz
+#define SX127X_SYNC_WORD 0x12 // 18 0 default LoRa sync word
+#define SX127X_SYNC_WORD_LORAWAN 0x34 // 52 0 sync word reserved for LoRaWAN networks
+
+///Added by Sandro
+#define SX127x_TXCONTINUOUSMODE_MASK 0xF7
+#define SX127x_TXCONTINUOUSMODE_ON 0x08
+#define SX127x_TXCONTINUOUSMODE_OFF 0x00
+#define SX127x_PPMOFFSET 0x27
+
+///// SX1278 Regs /////
+//SX1278 specific register map
+#define SX1278_REG_MODEM_CONFIG_3 0x26
+#define SX1278_REG_TCXO 0x4B
+#define SX1278_REG_PA_DAC 0x4D
+#define SX1278_REG_FORMER_TEMP 0x5D
+#define SX1278_REG_AGC_REF 0x61
+#define SX1278_REG_AGC_THRESH_1 0x62
+#define SX1278_REG_AGC_THRESH_2 0x63
+#define SX1278_REG_AGC_THRESH_3 0x64
+#define SX1278_REG_PLL 0x70
+
+//SX1278 LoRa modem settings
+//SX1278_REG_OP_MODE MSB LSB DESCRIPTION
+#define SX1278_HIGH_FREQ 0x00 // 3 3 access HF test registers
+#define SX1278_LOW_FREQ 0x08 // 3 3 access LF test registers
+
+//SX1278_REG_FRF_MSB + REG_FRF_MID + REG_FRF_LSB
+#define SX1278_FRF_MSB 0x6C // 7 0 carrier frequency setting: f_RF = (F(XOSC) * FRF)/2^19
+#define SX1278_FRF_MID 0x80 // 7 0 where F(XOSC) = 32 MHz
+#define SX1278_FRF_LSB 0x00 // 7 0 FRF = 3 byte value of FRF registers
+
+//SX1278_REG_PA_CONFIG
+#define SX1278_MAX_POWER 0x70 // 6 4 max power: P_max = 10.8 + 0.6*MAX_POWER [dBm]; P_max(MAX_POWER = 0b111) = 15 dBm
+//#define SX1278_MAX_POWER 0x10 // 6 4 changed
+
+//SX1278_REG_LNA
+#define SX1278_LNA_BOOST_LF_OFF 0x00 // 4 3 default LNA current
+
+#define SX1278_HEADER_EXPL_MODE 0x00 // 0 0 explicit header mode
+#define SX1278_HEADER_IMPL_MODE 0x01 // 0 0 implicit header mode
+
+//SX1278_REG_MODEM_CONFIG_2
+#define SX1278_RX_CRC_MODE_OFF 0x00 // 2 2 CRC disabled
+#define SX1278_RX_CRC_MODE_ON 0x04 // 2 2 CRC enabled
+
+//SX1278_REG_MODEM_CONFIG_3
+#define SX1278_LOW_DATA_RATE_OPT_OFF 0x00 // 3 3 low data rate optimization disabled
+#define SX1278_LOW_DATA_RATE_OPT_ON 0x08 // 3 3 low data rate optimization enabled
+#define SX1278_AGC_AUTO_OFF 0x00 // 2 2 LNA gain set by REG_LNA
+#define SX1278_AGC_AUTO_ON 0x04 // 2 2 LNA gain set by internal AGC loop
+
+#define SX1276_HEADER_EXPL_MODE 0x00 // 0 0 explicit header mode
+#define SX1276_HEADER_IMPL_MODE 0x01 // 0 0 implicit header mode
+
+#define SX127x_ERR_NONE 0x00
+#define SX127x_ERR_CHIP_NOT_FOUND 0x01
+#define SX127x_ERR_EEPROM_NOT_INITIALIZED 0x02
+
+#define SX127x_ERR_PACKET_TOO_LONG 0x10
+#define SX127x_ERR_TX_TIMEOUT 0x11
+
+#define SX127x_ERR_RX_TIMEOUT 0x20
+#define SX127x_ERR_CRC_MISMATCH 0x21
+
+#define SX127x_ERR_INVALID_BANDWIDTH 0x30
+#define SX127x_ERR_INVALID_SPREADING_FACTOR 0x31
+#define SX127x_ERR_INVALID_CODING_RATE 0x32
+#define SX127x_ERR_INVALID_FREQUENCY 0x33
+
+#define SX127x_ERR_INVALID_BIT_RANGE 0x40
+
+#define SX127x_CHANNEL_FREE 0x50
+#define SX127x_PREAMBLE_DETECTED 0x51
+
+#define SX127x_SPI_READ 0x00
+#define SX127x_SPI_WRITE 0x80
+
+#define SX127x_MAX_POWER 0x0F //17dBm
+
+#define SX127x_FREQ_STEP 61.03515625
+
+#define SX127x_FREQ_CORRECTION_MAX ((int32_t)(100000 / SX127x_FREQ_STEP))
+#define SX127x_FREQ_CORRECTION_MIN ((int32_t)(-100000 / SX127x_FREQ_STEP))
+
+bool sx127xInit(IO_t resetPin, IO_t busyPin);
+uint8_t sx127xISR(uint32_t *timeStamp);
+void sx127xWriteRegister(const uint8_t address, const uint8_t data);
+void sx127xWriteRegisterBurst(const uint8_t address, const uint8_t *data, const uint8_t length);
+uint8_t sx127xReadRegister(const uint8_t address);
+void sx127xReadRegisterBurst(const uint8_t address, uint8_t *data, const uint8_t length);
+uint8_t sx127xGetRegisterValue(const uint8_t reg, const uint8_t msb, const uint8_t lsb);
+uint8_t sx127xSetRegisterValue(const uint8_t reg, const uint8_t value, const uint8_t msb, const uint8_t lsb);
+void sx127xReadRegisterFIFO(uint8_t *data, const uint8_t length);
+void sx127xWriteRegisterFIFO(const uint8_t *data, const uint8_t length);
+void sx127xSetBandwidthCodingRate(const sx127xBandwidth_e bw, const sx127xCodingRate_e cr, const sx127xSpreadingFactor_e sf, const bool headerExplMode, const bool crcEnabled);
+void sx127xSetSyncWord(uint8_t syncWord);
+void sx127xSetMode(const sx127xRadioOpMode_e mode);
+void sx127xSetOutputPower(const uint8_t power);
+void sx127xSetPreambleLength(const uint8_t preambleLen);
+void sx127xSetSpreadingFactor(const sx127xSpreadingFactor_e sf);
+void sx127xSetFrequencyHZ(const uint32_t freq);
+void sx127xSetFrequencyReg(const uint32_t freq);
+
+void sx127xTransmitData(const uint8_t *data, const uint8_t length);
+void sx127xReceiveData(uint8_t *data, const uint8_t length);
+void sx127xStartReceiving(void);
+void sx127xConfig(const sx127xBandwidth_e bw, const sx127xSpreadingFactor_e sf, const sx127xCodingRate_e cr, const uint32_t freq, const uint8_t preambleLen, const bool iqInverted);
+
+uint32_t sx127xGetCurrBandwidth(const sx127xBandwidth_e bw);
+uint32_t sx127xGetCurrBandwidthNormalisedShifted(const sx127xBandwidth_e bw);
+void sx127xSetPPMoffsetReg(const int32_t offset, const uint32_t freq);
+int32_t sx127xGetFrequencyError(const sx127xBandwidth_e bw);
+void sx127xAdjustFrequency(int32_t offset, const uint32_t freq);
+uint8_t sx127xUnsignedGetLastPacketRSSI(void);
+int8_t sx127xGetLastPacketRSSI(void);
+int8_t sx127xGetCurrRSSI(void);
+int8_t sx127xGetLastPacketSNR(void);
+uint8_t sx127xGetIrqFlags(void);
+void sx127xClearIrqFlags(void);
+uint8_t sx127xGetIrqReason(void);
+void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr);
+
+void sx127xConfigLoraDefaults(const bool iqInverted);
diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c
new file mode 100644
index 0000000000..f0c6354968
--- /dev/null
+++ b/src/main/drivers/rx/rx_sx1280.c
@@ -0,0 +1,463 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_RX_SX1280
+
+#include "drivers/bus_spi.h"
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/rx/rx_sx1280.h"
+#include "drivers/rx/rx_spi.h"
+#include "drivers/time.h"
+
+#define SX1280_MAX_SPI_MHZ 10000000
+
+static IO_t busy;
+
+bool sx1280IsBusy(void)
+{
+ return IORead(busy);
+}
+
+static bool sx1280PollBusy(void)
+{
+ uint32_t startTime = micros();
+ while (IORead(busy)) {
+ if ((micros() - startTime) > 1000) {
+ return false;
+ } else {
+ __asm__("nop");
+ }
+ }
+ return true;
+}
+
+bool sx1280Init(IO_t resetPin, IO_t busyPin)
+{
+
+ if (!rxSpiExtiConfigured()) {
+ return false;
+ }
+
+ rxSpiSetNormalSpeedMhz(SX1280_MAX_SPI_MHZ);
+ rxSpiNormalSpeed();
+
+ if (resetPin) {
+ IOInit(resetPin, OWNER_RX_SPI_EXPRESSLRS_RESET, 0);
+ IOConfigGPIO(resetPin, IOCFG_OUT_PP);
+ } else {
+ resetPin = IO_NONE;
+ }
+
+ if (busyPin) {
+ IOInit(busyPin, OWNER_RX_SPI_EXPRESSLRS_BUSY, 0);
+ IOConfigGPIO(busyPin, IOCFG_IPU);
+ } else {
+ busyPin = IO_NONE;
+ }
+
+ busy = busyPin;
+
+ IOLo(resetPin);
+ delay(50);
+ IOConfigGPIO(resetPin, IOCFG_IN_FLOATING); // leave floating, internal pullup on sx1280 side
+ delay(20);
+
+ uint16_t firmwareRev = (((sx1280ReadRegister(REG_LR_FIRMWARE_VERSION_MSB)) << 8) | (sx1280ReadRegister(REG_LR_FIRMWARE_VERSION_MSB + 1)));
+
+ if ((firmwareRev == 0) || (firmwareRev == 65535)) {
+ return false;
+ }
+
+ return true;
+}
+
+uint8_t sx1280ISR(timeUs_t *timeStamp)
+{
+ if (rxSpiPollExti()) {
+ if (rxSpiGetLastExtiTimeUs()) {
+ *timeStamp = rxSpiGetLastExtiTimeUs();
+ }
+
+ uint8_t irqReason;
+ irqReason = sx1280GetIrqReason();
+
+ rxSpiResetExti();
+
+ return irqReason;
+ }
+ return 0;
+}
+
+void sx1280WriteCommand(const uint8_t address, const uint8_t data)
+{
+ sx1280PollBusy();
+ rxSpiWriteCommand(address, data);
+}
+
+void sx1280WriteCommandBurst(const uint8_t address, const uint8_t *data, const uint8_t length)
+{
+ uint8_t outBuffer[length + 1];
+
+ outBuffer[0] = address;
+
+ memcpy(outBuffer + 1, data, length);
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], length + 1);
+}
+
+void sx1280ReadCommandBurst(const uint8_t address, uint8_t *data, const uint8_t length)
+{
+ uint8_t outBuffer[length + 2];
+
+ outBuffer[0] = address;
+ outBuffer[1] = 0x00;
+
+ memcpy(outBuffer + 2, data, length);
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], length + 2);
+ memcpy(data, outBuffer + 2, length);
+}
+
+void sx1280WriteRegisterBurst(const uint16_t address, const uint8_t *buffer, const uint8_t size)
+{
+ uint8_t outBuffer[size + 3];
+
+ outBuffer[0] = (uint8_t) SX1280_RADIO_WRITE_REGISTER;
+ outBuffer[1] = ((address & 0xFF00) >> 8);
+ outBuffer[2] = (address & 0x00FF);
+
+ memcpy(outBuffer + 3, buffer, size);
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], size + 3);
+}
+
+void sx1280WriteRegister(const uint16_t address, const uint8_t value)
+{
+ sx1280WriteRegisterBurst(address, &value, 1);
+}
+
+void sx1280ReadRegisterBurst(const uint16_t address, uint8_t *buffer, const uint8_t size)
+{
+ uint8_t outBuffer[size + 4];
+
+ outBuffer[0] = (uint8_t) SX1280_RADIO_READ_REGISTER;
+ outBuffer[1] = ((address & 0xFF00) >> 8);
+ outBuffer[2] = (address & 0x00FF);
+ outBuffer[3] = 0x00;
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], size + 4);
+ memcpy(buffer, outBuffer + 4, size);
+}
+
+uint8_t sx1280ReadRegister(const uint16_t address)
+{
+ uint8_t data;
+ sx1280ReadRegisterBurst(address, &data, 1);
+ return data;
+}
+
+void sx1280WriteBuffer(const uint8_t offset, const uint8_t *buffer, const uint8_t size)
+{
+ uint8_t outBuffer[size + 2];
+
+ outBuffer[0] = (uint8_t) SX1280_RADIO_WRITE_BUFFER;
+ outBuffer[1] = offset;
+
+ memcpy(outBuffer + 2, buffer, size);
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], size + 2);
+}
+
+void sx1280ReadBuffer(const uint8_t offset, uint8_t *buffer, const uint8_t size)
+{
+ uint8_t outBuffer[size + 3];
+
+ outBuffer[0] = (uint8_t) SX1280_RADIO_READ_BUFFER;
+ outBuffer[1] = offset;
+ outBuffer[2] = 0x00;
+
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&outBuffer[0], size + 3);
+ memcpy(buffer, outBuffer + 3, size);
+}
+
+uint8_t sx1280GetStatus(void)
+{
+ uint8_t buffer[3] = {(uint8_t) SX1280_RADIO_GET_STATUS, 0, 0};
+ sx1280PollBusy();
+ rxSpiTransferCommandMulti(&buffer[0], 3);
+ return buffer[0];
+}
+
+void sx1280ConfigLoraDefaults(void)
+{
+ sx1280SetMode(SX1280_MODE_STDBY_RC); //step 1 put in STDBY_RC mode
+ sx1280WriteCommand(SX1280_RADIO_SET_PACKETTYPE, SX1280_PACKET_TYPE_LORA); //Step 2: set packet type to LoRa
+ sx1280ConfigLoraModParams(SX1280_LORA_BW_0800, SX1280_LORA_SF6, SX1280_LORA_CR_4_7); //Step 5: Configure Modulation Params
+ sx1280WriteCommand(SX1280_RADIO_SET_AUTOFS, 0x01); //enable auto FS
+ sx1280WriteRegister(0x0891, (sx1280ReadRegister(0x0891) | 0xC0)); //default is low power mode, switch to high sensitivity instead
+ sx1280SetPacketParams(12, SX1280_LORA_PACKET_IMPLICIT, 8, SX1280_LORA_CRC_OFF, SX1280_LORA_IQ_NORMAL); //default params
+ sx1280SetFrequencyHZ(2400000000); //Step 3: Set Freq
+ sx1280SetFIFOaddr(0x00, 0x00); //Step 4: Config FIFO addr
+ sx1280SetDioIrqParams(SX1280_IRQ_RADIO_ALL, SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE, SX1280_IRQ_RADIO_NONE, SX1280_IRQ_RADIO_NONE); //set IRQ to both RXdone/TXdone on DIO1
+}
+
+void sx1280Config(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFactors_e sf, const sx1280LoraCodingRates_e cr,
+ const uint32_t freq, const uint8_t preambleLength, const bool iqInverted)
+{
+ sx1280SetMode(SX1280_MODE_SLEEP);
+ sx1280PollBusy();
+
+ sx1280ConfigLoraDefaults();
+ sx1280SetOutputPower(13); //default is max power (12.5dBm for SX1280 RX)
+ sx1280SetMode(SX1280_MODE_STDBY_XOSC);
+ sx1280ClearIrqStatus(SX1280_IRQ_RADIO_ALL);
+ sx1280ConfigLoraModParams(bw, sf, cr);
+ sx1280SetPacketParams(preambleLength, SX1280_LORA_PACKET_IMPLICIT, 8, SX1280_LORA_CRC_OFF, (sx1280LoraIqModes_e)((uint8_t)!iqInverted << 6)); // TODO don't make static etc.
+ sx1280SetFrequencyReg(freq);
+}
+
+void sx1280SetOutputPower(const int8_t power)
+{
+ uint8_t buf[2];
+ buf[0] = power + 18;
+ buf[1] = (uint8_t) SX1280_RADIO_RAMP_04_US;
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_TXPARAMS, buf, 2);
+}
+
+void sx1280SetPacketParams(const uint8_t preambleLength, const sx1280LoraPacketLengthsModes_e headerType, const uint8_t payloadLength,
+ const sx1280LoraCrcModes_e crc, const sx1280LoraIqModes_e invertIQ)
+{
+ uint8_t buf[7];
+
+ buf[0] = preambleLength;
+ buf[1] = headerType;
+ buf[2] = payloadLength;
+ buf[3] = crc;
+ buf[4] = invertIQ;
+ buf[5] = 0x00;
+ buf[6] = 0x00;
+
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_PACKETPARAMS, buf, 7);
+}
+
+void sx1280SetMode(const sx1280OperatingModes_e opMode)
+{
+ uint8_t buf[3];
+
+ switch (opMode) {
+ case SX1280_MODE_SLEEP:
+ sx1280WriteCommand(SX1280_RADIO_SET_SLEEP, 0x01);
+ break;
+ case SX1280_MODE_CALIBRATION:
+ break;
+ case SX1280_MODE_STDBY_RC:
+ sx1280WriteCommand(SX1280_RADIO_SET_STANDBY, SX1280_STDBY_RC);
+ break;
+ case SX1280_MODE_STDBY_XOSC:
+ sx1280WriteCommand(SX1280_RADIO_SET_STANDBY, SX1280_STDBY_XOSC);
+ break;
+ case SX1280_MODE_FS:
+ sx1280WriteCommand(SX1280_RADIO_SET_FS, 0x00);
+ break;
+ case SX1280_MODE_RX:
+ buf[0] = 0x00; // periodBase = 1ms, page 71 datasheet, set to FF for cont RX
+ buf[1] = 0xFF;
+ buf[2] = 0xFF;
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_RX, buf, 3);
+ break;
+ case SX1280_MODE_TX:
+ //uses timeout Time-out duration = periodBase * periodBaseCount
+ buf[0] = 0x00; // periodBase = 1ms, page 71 datasheet
+ buf[1] = 0xFF; // no timeout set for now
+ buf[2] = 0xFF; // TODO dynamic timeout based on expected onairtime
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_TX, buf, 3);
+ break;
+ case SX1280_MODE_CAD:
+ break;
+ default:
+ break;
+ }
+}
+
+void sx1280ConfigLoraModParams(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFactors_e sf, const sx1280LoraCodingRates_e cr)
+{
+ // Care must therefore be taken to ensure that modulation parameters are set using the command
+ // SetModulationParam() only after defining the packet type SetPacketType() to be used
+
+ uint8_t rfparams[3] = {0};
+
+ rfparams[0] = (uint8_t)sf;
+ rfparams[1] = (uint8_t)bw;
+ rfparams[2] = (uint8_t)cr;
+
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_MODULATIONPARAMS, rfparams, 3);
+
+ switch (sf) {
+ case SX1280_LORA_SF5:
+ case SX1280_LORA_SF6:
+ sx1280WriteRegister(0x925, 0x1E); // for SF5 or SF6
+ break;
+ case SX1280_LORA_SF7:
+ case SX1280_LORA_SF8:
+ sx1280WriteRegister(0x925, 0x37); // for SF7 or SF8
+ break;
+ default:
+ sx1280WriteRegister(0x925, 0x32); // for SF9, SF10, SF11, SF12
+ }
+}
+
+void sx1280SetFrequencyHZ(const uint32_t reqFreq)
+{
+ uint8_t buf[3] = {0};
+
+ uint32_t freq = (uint32_t)(reqFreq / SX1280_FREQ_STEP);
+ buf[0] = (uint8_t)((freq >> 16) & 0xFF);
+ buf[1] = (uint8_t)((freq >> 8) & 0xFF);
+ buf[2] = (uint8_t)(freq & 0xFF);
+
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_RFFREQUENCY, buf, 3);
+}
+
+void sx1280SetFrequencyReg(const uint32_t freq)
+{
+ uint8_t buf[3] = {0};
+
+ buf[0] = (uint8_t)((freq >> 16) & 0xFF);
+ buf[1] = (uint8_t)((freq >> 8) & 0xFF);
+ buf[2] = (uint8_t)(freq & 0xFF);
+
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_RFFREQUENCY, buf, 3);
+}
+
+void sx1280AdjustFrequency(int32_t offset, const uint32_t freq)
+{
+ // just a stub to show that frequency adjustment is not used on this chip as opposed to sx127x
+ UNUSED(offset);
+ UNUSED(freq);
+}
+
+void sx1280SetFIFOaddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr)
+{
+ uint8_t buf[2];
+
+ buf[0] = txBaseAddr;
+ buf[1] = rxBaseAddr;
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_BUFFERBASEADDRESS, buf, 2);
+}
+
+void sx1280SetDioIrqParams(const uint16_t irqMask, const uint16_t dio1Mask, const uint16_t dio2Mask, const uint16_t dio3Mask)
+{
+ uint8_t buf[8];
+
+ buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF);
+ buf[1] = (uint8_t)(irqMask & 0x00FF);
+ buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF);
+ buf[3] = (uint8_t)(dio1Mask & 0x00FF);
+ buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF);
+ buf[5] = (uint8_t)(dio2Mask & 0x00FF);
+ buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF);
+ buf[7] = (uint8_t)(dio3Mask & 0x00FF);
+
+ sx1280WriteCommandBurst(SX1280_RADIO_SET_DIOIRQPARAMS, buf, 8);
+}
+
+uint16_t sx1280GetIrqStatus(void)
+{
+ uint8_t status[2];
+
+ sx1280ReadCommandBurst(SX1280_RADIO_GET_IRQSTATUS, status, 2);
+ return status[0] << 8 | status[1];
+}
+
+void sx1280ClearIrqStatus(const uint16_t irqMask)
+{
+ uint8_t buf[2];
+
+ buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF);
+ buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF);
+
+ sx1280WriteCommandBurst(SX1280_RADIO_CLR_IRQSTATUS, buf, 2);
+}
+
+uint8_t sx1280GetIrqReason(void)
+{
+ uint16_t irqStatus = sx1280GetIrqStatus();
+ sx1280ClearIrqStatus(SX1280_IRQ_RADIO_ALL);
+ if ((irqStatus & SX1280_IRQ_TX_DONE)) {
+ return 2;
+ } else if ((irqStatus & SX1280_IRQ_RX_DONE)) {
+ return 1;
+ }
+ return 0;
+}
+
+void sx1280TransmitData(const uint8_t *data, const uint8_t length)
+{
+ sx1280WriteBuffer(0x00, data, length);
+ sx1280SetMode(SX1280_MODE_TX);
+}
+
+static uint8_t sx1280GetRxBufferAddr(void)
+{
+ uint8_t status[2] = {0};
+ sx1280ReadCommandBurst(SX1280_RADIO_GET_RXBUFFERSTATUS, status, 2);
+ return status[1];
+}
+
+void sx1280ReceiveData(uint8_t *data, const uint8_t length)
+{
+ uint8_t FIFOaddr = sx1280GetRxBufferAddr();
+ sx1280ReadBuffer(FIFOaddr, data, length);
+}
+
+void sx1280StartReceiving(void)
+{
+ sx1280SetMode(SX1280_MODE_RX);
+}
+
+void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr)
+{
+ uint8_t status[2];
+
+ sx1280ReadCommandBurst(SX1280_RADIO_GET_PACKETSTATUS, status, 2);
+ *rssi = -(int8_t)(status[0] / 2);
+ *snr = ((int8_t) status[1]) / 4;
+}
+
+#endif /* USE_RX_SX1280 */
diff --git a/src/main/drivers/rx/rx_sx1280.h b/src/main/drivers/rx/rx_sx1280.h
new file mode 100644
index 0000000000..6a92b69bc1
--- /dev/null
+++ b/src/main/drivers/rx/rx_sx1280.h
@@ -0,0 +1,282 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#pragma once
+
+#define REG_LR_FIRMWARE_VERSION_MSB 0x0153 //The address of the register holding the firmware version MSB
+#define SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB 0x0954
+#define SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK 0x0FFFFF
+
+#define SX1280_XTAL_FREQ 52000000
+#define SX1280_FREQ_STEP (SX1280_XTAL_FREQ / 262144.0)
+
+typedef enum {
+ SX1280_RF_IDLE = 0x00, // The radio is idle
+ SX1280_RF_RX_RUNNING, // The radio is in reception state
+ SX1280_RF_TX_RUNNING, // The radio is in transmission state
+ SX1280_RF_CAD, // The radio is doing channel activity detection
+} sxx1280States_e;
+
+/*!
+ * \brief Represents the operating mode the radio is actually running
+ */
+typedef enum {
+ SX1280_MODE_SLEEP = 0x00, // The radio is in sleep mode
+ SX1280_MODE_CALIBRATION, // The radio is in calibration mode
+ SX1280_MODE_STDBY_RC, // The radio is in standby mode with RC oscillator
+ SX1280_MODE_STDBY_XOSC, // The radio is in standby mode with XOSC oscillator
+ SX1280_MODE_FS, // The radio is in frequency synthesis mode
+ SX1280_MODE_RX, // The radio is in receive mode
+ SX1280_MODE_TX, // The radio is in transmit mode
+ SX1280_MODE_CAD // The radio is in channel activity detection mode
+} sx1280OperatingModes_e;
+
+/*!
+ * \brief Declares the oscillator in use while in standby mode
+ *
+ * Using the STDBY_RC standby mode allow to reduce the energy consumption
+ * STDBY_XOSC should be used for time critical applications
+ */
+typedef enum {
+ SX1280_STDBY_RC = 0x00,
+ SX1280_STDBY_XOSC = 0x01,
+} sx1280StandbyModes_e;
+
+/*!
+ * \brief Declares the power regulation used to power the device
+ *
+ * This command allows the user to specify if DC-DC or LDO is used for power regulation.
+ * Using only LDO implies that the Rx or Tx current is doubled
+ */
+typedef enum {
+ SX1280_USE_LDO = 0x00, // Use LDO (default value)
+ SX1280_USE_DCDC = 0x01, // Use DCDC
+} sx1280RegulatorModes_e;
+
+/*!
+ * \brief Represents the possible packet type (i.e. modem) used
+ */
+typedef enum {
+ SX1280_PACKET_TYPE_GFSK = 0x00,
+ SX1280_PACKET_TYPE_LORA,
+ SX1280_PACKET_TYPE_RANGING,
+ SX1280_PACKET_TYPE_FLRC,
+ SX1280_PACKET_TYPE_BLE,
+ SX1280_PACKET_TYPE_NONE = 0x0F,
+} sx1280PacketTypes_e;
+
+typedef enum {
+ SX1280_LORA_IQ_NORMAL = 0x40,
+ SX1280_LORA_IQ_INVERTED = 0x00,
+} sx1280LoraIqModes_e;
+
+typedef enum {
+ SX1280_RADIO_CRC_OFF = 0x00, // No CRC in use
+ SX1280_RADIO_CRC_1_BYTES = 0x10,
+ SX1280_RADIO_CRC_2_BYTES = 0x20,
+ SX1280_RADIO_CRC_3_BYTES = 0x30,
+} sx1280CrcTypes_e;
+
+/*!
+ * \brief Represents the ramping time for power amplifier
+ */
+typedef enum {
+ SX1280_RADIO_RAMP_02_US = 0x00,
+ SX1280_RADIO_RAMP_04_US = 0x20,
+ SX1280_RADIO_RAMP_06_US = 0x40,
+ SX1280_RADIO_RAMP_08_US = 0x60,
+ SX1280_RADIO_RAMP_10_US = 0x80,
+ SX1280_RADIO_RAMP_12_US = 0xA0,
+ SX1280_RADIO_RAMP_16_US = 0xC0,
+ SX1280_RADIO_RAMP_20_US = 0xE0,
+} sx1280RampTimes_e;
+
+/*!
+ * \brief Represents the number of symbols to be used for channel activity detection operation
+ */
+typedef enum {
+ SX1280_LORA_CAD_01_SYMBOL = 0x00,
+ SX1280_LORA_CAD_02_SYMBOLS = 0x20,
+ SX1280_LORA_CAD_04_SYMBOLS = 0x40,
+ SX1280_LORA_CAD_08_SYMBOLS = 0x60,
+ SX1280_LORA_CAD_16_SYMBOLS = 0x80,
+} sx1280LoraCadSymbols_e;
+
+/*!
+ * \brief Represents the possible spreading factor values in LORA packet types
+ */
+typedef enum {
+ SX1280_LORA_SF5 = 0x50,
+ SX1280_LORA_SF6 = 0x60,
+ SX1280_LORA_SF7 = 0x70,
+ SX1280_LORA_SF8 = 0x80,
+ SX1280_LORA_SF9 = 0x90,
+ SX1280_LORA_SF10 = 0xA0,
+ SX1280_LORA_SF11 = 0xB0,
+ SX1280_LORA_SF12 = 0xC0,
+} sx1280LoraSpreadingFactors_e;
+
+/*!
+ * \brief Represents the bandwidth values for LORA packet type
+ */
+typedef enum {
+ SX1280_LORA_BW_0200 = 0x34,
+ SX1280_LORA_BW_0400 = 0x26,
+ SX1280_LORA_BW_0800 = 0x18,
+ SX1280_LORA_BW_1600 = 0x0A,
+} sx1280LoraBandwidths_e;
+
+/*!
+ * \brief Represents the coding rate values for LORA packet type
+ */
+typedef enum {
+ SX1280_LORA_CR_4_5 = 0x01,
+ SX1280_LORA_CR_4_6 = 0x02,
+ SX1280_LORA_CR_4_7 = 0x03,
+ SX1280_LORA_CR_4_8 = 0x04,
+ SX1280_LORA_CR_LI_4_5 = 0x05,
+ SX1280_LORA_CR_LI_4_6 = 0x06,
+ SX1280_LORA_CR_LI_4_7 = 0x07,
+} sx1280LoraCodingRates_e;
+
+typedef enum {
+ SX1280_LORA_PACKET_VARIABLE_LENGTH = 0x00, // The packet is on variable size, header included
+ SX1280_LORA_PACKET_FIXED_LENGTH = 0x80, // The packet is known on both sides, no header included in the packet
+ SX1280_LORA_PACKET_EXPLICIT = SX1280_LORA_PACKET_VARIABLE_LENGTH,
+ SX1280_LORA_PACKET_IMPLICIT = SX1280_LORA_PACKET_FIXED_LENGTH,
+} sx1280LoraPacketLengthsModes_e;
+
+typedef enum {
+ SX1280_LORA_CRC_ON = 0x20, // CRC activated
+ SX1280_LORA_CRC_OFF = 0x00, // CRC not used
+} sx1280LoraCrcModes_e;
+
+typedef enum {
+ SX1280_RADIO_GET_STATUS = 0xC0,
+ SX1280_RADIO_WRITE_REGISTER = 0x18,
+ SX1280_RADIO_READ_REGISTER = 0x19,
+ SX1280_RADIO_WRITE_BUFFER = 0x1A,
+ SX1280_RADIO_READ_BUFFER = 0x1B,
+ SX1280_RADIO_SET_SLEEP = 0x84,
+ SX1280_RADIO_SET_STANDBY = 0x80,
+ SX1280_RADIO_SET_FS = 0xC1,
+ SX1280_RADIO_SET_TX = 0x83,
+ SX1280_RADIO_SET_RX = 0x82,
+ SX1280_RADIO_SET_RXDUTYCYCLE = 0x94,
+ SX1280_RADIO_SET_CAD = 0xC5,
+ SX1280_RADIO_SET_TXCONTINUOUSWAVE = 0xD1,
+ SX1280_RADIO_SET_TXCONTINUOUSPREAMBLE = 0xD2,
+ SX1280_RADIO_SET_PACKETTYPE = 0x8A,
+ SX1280_RADIO_GET_PACKETTYPE = 0x03,
+ SX1280_RADIO_SET_RFFREQUENCY = 0x86,
+ SX1280_RADIO_SET_TXPARAMS = 0x8E,
+ SX1280_RADIO_SET_CADPARAMS = 0x88,
+ SX1280_RADIO_SET_BUFFERBASEADDRESS = 0x8F,
+ SX1280_RADIO_SET_MODULATIONPARAMS = 0x8B,
+ SX1280_RADIO_SET_PACKETPARAMS = 0x8C,
+ SX1280_RADIO_GET_RXBUFFERSTATUS = 0x17,
+ SX1280_RADIO_GET_PACKETSTATUS = 0x1D,
+ SX1280_RADIO_GET_RSSIINST = 0x1F,
+ SX1280_RADIO_SET_DIOIRQPARAMS = 0x8D,
+ SX1280_RADIO_GET_IRQSTATUS = 0x15,
+ SX1280_RADIO_CLR_IRQSTATUS = 0x97,
+ SX1280_RADIO_CALIBRATE = 0x89,
+ SX1280_RADIO_SET_REGULATORMODE = 0x96,
+ SX1280_RADIO_SET_SAVECONTEXT = 0xD5,
+ SX1280_RADIO_SET_AUTOTX = 0x98,
+ SX1280_RADIO_SET_AUTOFS = 0x9E,
+ SX1280_RADIO_SET_LONGPREAMBLE = 0x9B,
+ SX1280_RADIO_SET_UARTSPEED = 0x9D,
+ SX1280_RADIO_SET_RANGING_ROLE = 0xA3,
+} sx1280Commands_e;
+
+typedef enum {
+ SX1280_IRQ_RADIO_NONE = 0x0000,
+ SX1280_IRQ_TX_DONE = 0x0001,
+ SX1280_IRQ_RX_DONE = 0x0002,
+ SX1280_IRQ_SYNCWORD_VALID = 0x0004,
+ SX1280_IRQ_SYNCWORD_ERROR = 0x0008,
+ SX1280_IRQ_HEADER_VALID = 0x0010,
+ SX1280_IRQ_HEADER_ERROR = 0x0020,
+ SX1280_IRQ_CRC_ERROR = 0x0040,
+ SX1280_IRQ_RANGING_SLAVE_RESPONSE_DONE = 0x0080,
+ SX1280_IRQ_RANGING_SLAVE_REQUEST_DISCARDED = 0x0100,
+ SX1280_IRQ_RANGING_MASTER_RESULT_VALID = 0x0200,
+ SX1280_IRQ_RANGING_MASTER_TIMEOUT = 0x0400,
+ SX1280_IRQ_RANGING_SLAVE_REQUEST_VALID = 0x0800,
+ SX1280_IRQ_CAD_DONE = 0x1000,
+ SX1280_IRQ_CAD_DETECTED = 0x2000,
+ SX1280_IRQ_RX_TX_TIMEOUT = 0x4000,
+ SX1280_IRQ_PREAMBLE_DETECTED = 0x8000,
+ SX1280_IRQ_RADIO_ALL = 0xFFFF,
+} sx1280IrqMasks_e;
+
+typedef enum {
+ SX1280_RADIO_DIO1 = 0x02,
+ SX1280_RADIO_DIO2 = 0x04,
+ SX1280_RADIO_DIO3 = 0x08,
+} sx1280Dios_e;
+
+typedef enum {
+ SX1280_RADIO_TICK_SIZE_0015_US = 0x00,
+ SX1280_RADIO_TICK_SIZE_0062_US = 0x01,
+ SX1280_RADIO_TICK_SIZE_1000_US = 0x02,
+ SX1280_RADIO_TICK_SIZE_4000_US = 0x03,
+} sx1280TickSizes_e;
+
+bool sx1280Init(IO_t resetPin, IO_t busyPin);
+uint8_t sx1280ISR(uint32_t *timeStamp);
+bool sx1280IsBusy(void);
+void sx1280WriteCommand(const uint8_t address, const uint8_t data);
+void sx1280WriteCommandBurst(const uint8_t address, const uint8_t *data, const uint8_t length);
+void sx1280ReadCommandBurst(const uint8_t address, uint8_t *data, const uint8_t length);
+void sx1280WriteRegisterBurst(const uint16_t address, const uint8_t *buffer, const uint8_t size);
+void sx1280WriteRegister(const uint16_t address, const uint8_t value);
+void sx1280ReadRegisterBurst(const uint16_t address, uint8_t *buffer, const uint8_t size);
+uint8_t sx1280ReadRegister(const uint16_t address);
+void sx1280WriteBuffer(const uint8_t offset, const uint8_t *buffer, const uint8_t size);
+void sx1280ReadBuffer(const uint8_t offset, uint8_t *buffer, const uint8_t size);
+
+uint8_t sx1280GetStatus(void);
+void sx1280ConfigLoraDefaults(void);
+void sx1280Config(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFactors_e sf, const sx1280LoraCodingRates_e cr, const uint32_t freq, const uint8_t preambleLength, const bool iqInverted);
+void sx1280SetOutputPower(const int8_t power);
+void sx1280SetPacketParams(const uint8_t preambleLength, const sx1280LoraPacketLengthsModes_e headerType, const uint8_t payloadLength, const sx1280LoraCrcModes_e crc, const sx1280LoraIqModes_e invertIQ);
+void sx1280SetMode(const sx1280OperatingModes_e opMode);
+void sx1280ConfigLoraModParams(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFactors_e sf, const sx1280LoraCodingRates_e cr);
+void sx1280SetFrequencyHZ(const uint32_t reqFreq);
+void sx1280SetFrequencyReg(const uint32_t freq);
+void sx1280AdjustFrequency(int32_t offset, const uint32_t freq);
+void sx1280SetFIFOaddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr);
+void sx1280SetDioIrqParams(const uint16_t irqMask, const uint16_t dio1Mask, const uint16_t dio2Mask, const uint16_t dio3Mask);
+uint16_t sx1280GetIrqStatus(void);
+void sx1280ClearIrqStatus(const uint16_t irqMask);
+uint8_t sx1280GetIrqReason(void);
+
+void sx1280TransmitData(const uint8_t *data, const uint8_t length);
+void sx1280ReceiveData(uint8_t *data, const uint8_t length);
+void sx1280StartReceiving(void);
+
+void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr);
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index c1463b205d..d55094228e 100644
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -56,9 +56,15 @@
#define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4))
typedef struct timerConfig_s {
+ // per-timer
+ timerOvrHandlerRec_t *updateCallback;
+
+ // per-channel
timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
- timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
+
+ // state
+ timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linked list of active overflow callbacks
uint32_t forcedOverflowTimerValue;
} timerConfig_t;
timerConfig_t timerConfig[USED_TIMER_COUNT];
@@ -410,9 +416,15 @@ void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *
// update overflow callback list
// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now)
-static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
+static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, const TIM_TypeDef *tim) {
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+
+ if (cfg->updateCallback) {
+ *chain = cfg->updateCallback;
+ chain = &cfg->updateCallback->next;
+ }
+
for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
if (cfg->overflowCallback[i]) {
*chain = cfg->overflowCallback[i];
@@ -421,10 +433,10 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
*chain = NULL;
}
// enable or disable IRQ
- TIM_ITConfig(tim, TIM_IT_Update, cfg->overflowCallbackActive ? ENABLE : DISABLE);
+ TIM_ITConfig((TIM_TypeDef *)tim, TIM_IT_Update, cfg->overflowCallbackActive ? ENABLE : DISABLE);
}
-// config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive
+// config edge and overflow callback for channel. Try to avoid per-channel overflowCallback, it is a bit expensive
void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback)
{
uint8_t timerIndex = lookupTimerIndex(timHw->tim);
@@ -444,6 +456,16 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
}
+void timerConfigUpdateCallback(const TIM_TypeDef *tim, timerOvrHandlerRec_t *updateCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ timerConfig[timerIndex].updateCallback = updateCallback;
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], tim);
+}
+
// configure callbacks for pair of channels (1+2 or 3+4).
// Hi(2,4) and Lo(1,3) callbacks are specified, it is not important which timHw channel is used.
// This is intended for dual capture mode (each channel handles one transition)
@@ -694,6 +716,39 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
#endif
}
+static inline void timUpdateHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
+{
+ uint16_t capture;
+ unsigned tim_status;
+ tim_status = tim->SR & tim->DIER;
+ while (tim_status) {
+ // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+ // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+ unsigned bit = __builtin_clz(tim_status);
+ unsigned mask = ~(0x80000000 >> bit);
+ tim->SR = mask;
+ tim_status &= mask;
+ switch (bit) {
+ case __builtin_clz(TIM_IT_Update): {
+
+ if (timerConfig->forcedOverflowTimerValue != 0) {
+ capture = timerConfig->forcedOverflowTimerValue - 1;
+ timerConfig->forcedOverflowTimerValue = 0;
+ } else {
+ capture = tim->ARR;
+ }
+
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while (cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ break;
+ }
+ }
+ }
+}
+
// handler for shared interrupts when both timers need to check status bits
#define _TIM_IRQ_HANDLER2(name, i, j) \
void name(void) \
@@ -708,6 +763,12 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
} struct dummy
+#define _TIM_IRQ_HANDLER_UPDATE_ONLY(name, i) \
+ void name(void) \
+ { \
+ timUpdateHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
+ } struct dummy
+
#if USED_TIMERS & TIM_N(1)
_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
# if defined(STM32F10X)
@@ -740,6 +801,23 @@ _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
#if USED_TIMERS & TIM_N(5)
_TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
#endif
+
+#if USED_TIMERS & TIM_N(6)
+# if !(defined(USE_PID_AUDIO) && (defined(STM32H7) || defined(STM32F7)))
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM6_IRQHandler, 6);
+# endif
+#endif
+#if USED_TIMERS & TIM_N(7)
+// The USB VCP_HAL driver conflicts with TIM7, see TIMx_IRQHandler in usbd_cdc_interface.h
+# if !(defined(USE_VCP) && (defined(STM32F4) || defined(STM32G4) || defined(STM32H7)))
+# if defined(STM32G4)
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM7_DAC_IRQHandler, 7);
+# else
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM7_IRQHandler, 7);
+# endif
+# endif
+#endif
+
#if USED_TIMERS & TIM_N(8)
_TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
# if defined(STM32F10X_XL)
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index f00043cc46..16de57a7f3 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -240,7 +240,20 @@ typedef enum {
TYPE_TIMER
} channelType_t;
-void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz); // This interface should be replaced.
+//
+// Legacy API
+//
+void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz);
+
+//
+// Initialisation
+//
+void timerInit(void);
+void timerStart(void);
+
+//
+// per-channel
+//
void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples);
@@ -261,13 +274,18 @@ void timerChClearCCFlag(const timerHardware_t* timHw);
void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq);
-void timerInit(void);
-void timerStart(void);
+//
+// per-timer
+//
+
void timerForceOverflow(TIM_TypeDef *tim);
+void timerConfigUpdateCallback(const TIM_TypeDef *tim, timerOvrHandlerRec_t *updateCallback);
+
uint32_t timerClock(TIM_TypeDef *tim);
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration
+void timerReconfigureTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz);
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
uint8_t timerInputIrq(TIM_TypeDef *tim);
diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c
index 901fe9c519..5740a445db 100644
--- a/src/main/drivers/timer_hal.c
+++ b/src/main/drivers/timer_hal.c
@@ -57,8 +57,14 @@
#define TIM_IT_CCx(ch) (TIM_IT_CC1 << ((ch) / 4))
typedef struct timerConfig_s {
+ // per-timer
+ timerOvrHandlerRec_t *updateCallback;
+
+ // per-channel
timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
+
+ // state
timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
uint32_t forcedOverflowTimerValue;
} timerConfig_t;
@@ -338,27 +344,37 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
return &timerHandle[timerIndex].Handle;
}
+void timerReconfigureTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
+{
+ TIM_HandleTypeDef* handle = timerFindTimerHandle(tim);
+ if (handle == NULL) return;
+
+ handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+ handle->Init.Prescaler = (timerClock(tim) / hz) - 1;
+
+ TIM_Base_SetConfig(handle->Instance, &handle->Init);
+}
+
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
{
- uint8_t timerIndex = lookupTimerIndex(tim);
- if (timerIndex >= USED_TIMER_COUNT) {
- return;
- }
- if (timerHandle[timerIndex].Handle.Instance == tim) {
+ TIM_HandleTypeDef* handle = timerFindTimerHandle(tim);
+ if (handle == NULL) return;
+
+ if (handle->Instance == tim) {
// already configured
return;
}
- timerHandle[timerIndex].Handle.Instance = tim;
+ handle->Instance = tim;
- timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
- timerHandle[timerIndex].Handle.Init.Prescaler = (timerClock(tim) / hz) - 1;
+ handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+ handle->Init.Prescaler = (timerClock(tim) / hz) - 1;
- timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
- timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
+ handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ handle->Init.CounterMode = TIM_COUNTERMODE_UP;
+ handle->Init.RepetitionCounter = 0x0000;
- HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle);
+ HAL_TIM_Base_Init(handle);
if (tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8
#if !(defined(STM32H7) || defined(STM32G4))
|| tim == TIM9
@@ -367,7 +383,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
TIM_ClockConfigTypeDef sClockSourceConfig;
memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig));
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
- if (HAL_TIM_ConfigClockSource(&timerHandle[timerIndex].Handle, &sClockSourceConfig) != HAL_OK) {
+ if (HAL_TIM_ConfigClockSource(handle, &sClockSourceConfig) != HAL_OK) {
return;
}
}
@@ -375,11 +391,10 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
TIM_MasterConfigTypeDef sMasterConfig;
memset(&sMasterConfig, 0, sizeof(sMasterConfig));
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
- if (HAL_TIMEx_MasterConfigSynchronization(&timerHandle[timerIndex].Handle, &sMasterConfig) != HAL_OK) {
+ if (HAL_TIMEx_MasterConfigSynchronization(handle, &sMasterConfig) != HAL_OK) {
return;
}
}
-
}
// old interface for PWM inputs. It should be replaced
@@ -460,7 +475,7 @@ void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *
// update overflow callback list
// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now)
-static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim)
+static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, const TIM_TypeDef *tim)
{
uint8_t timerIndex = lookupTimerIndex(tim);
if (timerIndex >= USED_TIMER_COUNT) {
@@ -469,6 +484,12 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim)
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+
+ if (cfg->updateCallback) {
+ *chain = cfg->updateCallback;
+ chain = &cfg->updateCallback->next;
+ }
+
for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
if (cfg->overflowCallback[i]) {
*chain = cfg->overflowCallback[i];
@@ -503,6 +524,16 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
}
+void timerConfigUpdateCallback(const TIM_TypeDef *tim, timerOvrHandlerRec_t *updateCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ timerConfig[timerIndex].updateCallback = updateCallback;
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], tim);
+}
+
// configure callbacks for pair of channels (1+2 or 3+4).
// Hi(2,4) and Lo(1,3) callbacks are specified, it is not important which timHw channel is used.
// This is intended for dual capture mode (each channel handles one transition)
@@ -784,6 +815,39 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
#endif
}
+static inline void timUpdateHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
+{
+ uint16_t capture;
+ unsigned tim_status;
+ tim_status = tim->SR & tim->DIER;
+ while (tim_status) {
+ // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+ // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+ unsigned bit = __builtin_clz(tim_status);
+ unsigned mask = ~(0x80000000 >> bit);
+ tim->SR = mask;
+ tim_status &= mask;
+ switch (bit) {
+ case __builtin_clz(TIM_IT_UPDATE): {
+
+ if (timerConfig->forcedOverflowTimerValue != 0) {
+ capture = timerConfig->forcedOverflowTimerValue - 1;
+ timerConfig->forcedOverflowTimerValue = 0;
+ } else {
+ capture = tim->ARR;
+ }
+
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while (cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ break;
+ }
+ }
+ }
+}
+
// handler for shared interrupts when both timers need to check status bits
#define _TIM_IRQ_HANDLER2(name, i, j) \
void name(void) \
@@ -798,6 +862,12 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
timCCxHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
} struct dummy
+#define _TIM_IRQ_HANDLER_UPDATE_ONLY(name, i) \
+ void name(void) \
+ { \
+ timUpdateHandler(TIM ## i, &timerConfig[TIMER_INDEX(i)]); \
+ } struct dummy
+
#if USED_TIMERS & TIM_N(1)
_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
# if defined(STM32H7)
@@ -830,6 +900,22 @@ _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
_TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
#endif
+#if USED_TIMERS & TIM_N(6)
+# if !(defined(USE_PID_AUDIO) && (defined(STM32H7) || defined(STM32F7)))
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM6_DAC_IRQHandler, 6);
+# endif
+#endif
+#if USED_TIMERS & TIM_N(7)
+// The USB VCP_HAL driver conflicts with TIM7, see TIMx_IRQHandler in usbd_cdc_interface.h
+# if !(defined(USE_VCP) && (defined(STM32F4) || defined(STM32G4) || defined(STM32H7) || defined(STM32F7)))
+# if defined(STM32G4)
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM7_DAC_IRQHandler, 7);
+# else
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM7_IRQHandler, 7);
+# endif
+# endif
+#endif
+
#if USED_TIMERS & TIM_N(8)
_TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
# if defined(STM32G4)
diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c
index 5121973ddf..32055ad3e0 100644
--- a/src/main/drivers/timer_stm32f7xx.c
+++ b/src/main/drivers/timer_stm32f7xx.c
@@ -38,8 +38,8 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn},
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn},
{ .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn},
- { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0},
- { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0},
+ { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = TIM6_DAC_IRQn},
+ { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = TIM7_IRQn},
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn},
{ .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn},
{ .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn},
diff --git a/src/main/drivers/timer_stm32g4xx.c b/src/main/drivers/timer_stm32g4xx.c
index 318cac7adb..f4c2bd8f24 100644
--- a/src/main/drivers/timer_stm32g4xx.c
+++ b/src/main/drivers/timer_stm32g4xx.c
@@ -39,8 +39,8 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM3, .rcc = RCC_APB11(TIM3), .inputIrq = TIM3_IRQn},
{ .TIMx = TIM4, .rcc = RCC_APB11(TIM4), .inputIrq = TIM4_IRQn},
{ .TIMx = TIM5, .rcc = RCC_APB11(TIM5), .inputIrq = TIM5_IRQn},
- { .TIMx = TIM6, .rcc = RCC_APB11(TIM6), .inputIrq = 0},
- { .TIMx = TIM7, .rcc = RCC_APB11(TIM7), .inputIrq = 0},
+ { .TIMx = TIM6, .rcc = RCC_APB11(TIM6), .inputIrq = TIM6_DAC_IRQn},
+ { .TIMx = TIM7, .rcc = RCC_APB11(TIM7), .inputIrq = TIM7_DAC_IRQn},
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn},
{ .TIMx = TIM15, .rcc = RCC_APB2(TIM15), .inputIrq = TIM1_BRK_TIM15_IRQn},
{ .TIMx = TIM16, .rcc = RCC_APB2(TIM16), .inputIrq = TIM1_UP_TIM16_IRQn},
diff --git a/src/main/drivers/timer_stm32h7xx.c b/src/main/drivers/timer_stm32h7xx.c
index ed66afc2dd..89b3ef7dd4 100644
--- a/src/main/drivers/timer_stm32h7xx.c
+++ b/src/main/drivers/timer_stm32h7xx.c
@@ -38,8 +38,8 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM3, .rcc = RCC_APB1L(TIM3), .inputIrq = TIM3_IRQn},
{ .TIMx = TIM4, .rcc = RCC_APB1L(TIM4), .inputIrq = TIM4_IRQn},
{ .TIMx = TIM5, .rcc = RCC_APB1L(TIM5), .inputIrq = TIM5_IRQn},
- { .TIMx = TIM6, .rcc = RCC_APB1L(TIM6), .inputIrq = 0},
- { .TIMx = TIM7, .rcc = RCC_APB1L(TIM7), .inputIrq = 0},
+ { .TIMx = TIM6, .rcc = RCC_APB1L(TIM6), .inputIrq = TIM6_DAC_IRQn},
+ { .TIMx = TIM7, .rcc = RCC_APB1L(TIM7), .inputIrq = TIM7_IRQn},
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn},
{ .TIMx = TIM12, .rcc = RCC_APB1L(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn},
{ .TIMx = TIM13, .rcc = RCC_APB1L(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn},
diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h
index a8e15f128c..fa770e03fd 100644
--- a/src/main/pg/pg_ids.h
+++ b/src/main/pg/pg_ids.h
@@ -151,7 +151,8 @@
#define PG_PULLDOWN_CONFIG 552
#define PG_MODE_ACTIVATION_CONFIG 553
#define PG_DYN_NOTCH_CONFIG 554
-#define PG_BETAFLIGHT_END 554
+#define PG_RX_EXPRESSLRS_SPI_CONFIG 555
+#define PG_BETAFLIGHT_END 555
// OSD configuration (subject to change)
diff --git a/src/main/pg/rx_spi_expresslrs.c b/src/main/pg/rx_spi_expresslrs.c
new file mode 100644
index 0000000000..aeb86b0d85
--- /dev/null
+++ b/src/main/pg/rx_spi_expresslrs.c
@@ -0,0 +1,43 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#if defined(USE_RX_EXPRESSLRS)
+
+#include "drivers/io.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "rx_spi_expresslrs.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(rxExpressLrsSpiConfig_t, rxExpressLrsSpiConfig, PG_RX_EXPRESSLRS_SPI_CONFIG, 0);
+
+PG_RESET_TEMPLATE(rxExpressLrsSpiConfig_t, rxExpressLrsSpiConfig,
+ .resetIoTag = IO_TAG(RX_EXPRESSLRS_SPI_RESET_PIN),
+ .busyIoTag = IO_TAG(RX_EXPRESSLRS_SPI_BUSY_PIN),
+ .UID = {0, 0, 0, 0, 0, 0},
+ .switchMode = 0,
+ .domain = 0,
+ .rateIndex = 0,
+ .modelId = 0xFF,
+);
+#endif
diff --git a/src/main/pg/rx_spi_expresslrs.h b/src/main/pg/rx_spi_expresslrs.h
new file mode 100644
index 0000000000..2ebc667efb
--- /dev/null
+++ b/src/main/pg/rx_spi_expresslrs.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "drivers/io_types.h"
+
+#include "pg/pg.h"
+
+typedef struct rxExpressLrsSpiConfig_s {
+ ioTag_t resetIoTag;
+ ioTag_t busyIoTag;
+ uint8_t UID[6];
+ uint8_t switchMode;
+ uint8_t domain;
+ uint8_t rateIndex;
+ uint8_t modelId;
+} rxExpressLrsSpiConfig_t;
+
+PG_DECLARE(rxExpressLrsSpiConfig_t, rxExpressLrsSpiConfig);
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 7c5ca8682f..59afb2f5ab 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -275,12 +275,12 @@ static void handleCrsfLinkStatisticsTxFrame(const crsfLinkStatisticsTx_t* statsP
{
const crsfLinkStatisticsTx_t stats = *statsPtr;
lastLinkStatisticsFrameUs = currentTimeUs;
- int16_t rssiDbm = -1 * stats.uplink_RSSI;
if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
const uint16_t rssiPercentScaled = scaleRange(stats.uplink_RSSI_percentage, 0, 100, 0, RSSI_MAX_VALUE);
setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
}
#ifdef USE_RX_RSSI_DBM
+ int16_t rssiDbm = -1 * stats.uplink_RSSI;
if (rxConfig()->crsf_use_rx_snr) {
rssiDbm = stats.uplink_SNR;
}
diff --git a/src/main/rx/expresslrs.c b/src/main/rx/expresslrs.c
new file mode 100644
index 0000000000..93cb731279
--- /dev/null
+++ b/src/main/rx/expresslrs.c
@@ -0,0 +1,1094 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ *
+ * Authors:
+ * Phobos- - Original port.
+ * Dominic Clifton/Hydra - Timer-based timeout implementation.
+ * Phobos- - Port of v2.0
+ */
+
+#include
+#include "platform.h"
+
+#ifdef USE_RX_EXPRESSLRS
+
+#include "build/debug.h"
+#include "build/debug_pin.h"
+
+#include "common/maths.h"
+#include "common/filter.h"
+
+#include "drivers/io.h"
+#include "drivers/rx/rx_spi.h"
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+#include "drivers/rx/rx_sx127x.h"
+#include "drivers/rx/rx_sx1280.h"
+#include "drivers/rx/expresslrs_driver.h"
+
+#include "config/config.h"
+#include "config/feature.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/rx_spi.h"
+#include "pg/rx_spi_expresslrs.h"
+
+#include "rx/rx.h"
+#include "rx/rx_spi.h"
+#include "rx/rx_spi_common.h"
+
+#include "rx/expresslrs.h"
+#include "rx/expresslrs_common.h"
+#include "rx/expresslrs_impl.h"
+#include "rx/expresslrs_telemetry.h"
+
+STATIC_UNIT_TESTED elrsReceiver_t receiver;
+static const uint8_t BindingUID[6] = {0,1,2,3,4,5}; // Special binding UID values
+static uint16_t crcInitializer = 0;
+static uint8_t bindingRateIndex = 0;
+static bool connectionHasModelMatch = false;
+static uint8_t txPower = 0;
+static uint8_t wideSwitchIndex = 0;
+
+static simpleLowpassFilter_t rssiFilter;
+
+static void rssiFilterReset(void)
+{
+ simpleLPFilterInit(&rssiFilter, 3, 5);
+}
+
+#define PACKET_HANDLING_TO_TOCK_ISR_DELAY_US 250
+
+//
+// Event pair recorder
+//
+
+typedef enum {
+ EPR_FIRST,
+ EPR_SECOND,
+} eprEvent_e;
+
+#define EPR_EVENT_COUNT 2
+
+typedef struct eprState_s {
+ uint32_t eventAtUs[EPR_EVENT_COUNT];
+ bool eventRecorded[EPR_EVENT_COUNT];
+} eprState_t;
+
+eprState_t eprState = {0};
+
+static void expressLrsEPRRecordEvent(eprEvent_e event, uint32_t currentTimeUs)
+{
+ eprState.eventAtUs[event] = currentTimeUs;
+ eprState.eventRecorded[event] = true;
+}
+
+static bool expressLrsEPRHaveBothEvents(void)
+{
+ bool bothEventsRecorded = eprState.eventRecorded[EPR_SECOND] && eprState.eventRecorded[EPR_FIRST];
+ return bothEventsRecorded;
+}
+
+static int32_t expressLrsEPRGetResult(void)
+{
+ if (!expressLrsEPRHaveBothEvents()) {
+ return 0;
+ }
+
+ return (int32_t)(eprState.eventAtUs[EPR_SECOND] - eprState.eventAtUs[EPR_FIRST]);
+}
+
+static void expressLrsEPRReset(void)
+{
+ memset(&eprState, 0, sizeof(eprState_t));
+}
+
+
+//
+// Phase Lock
+//
+
+
+#define EPR_INTERNAL EPR_FIRST
+#define EPR_EXTERNAL EPR_SECOND
+
+typedef struct phaseLockState_s {
+ simpleLowpassFilter_t offsetFilter;
+ simpleLowpassFilter_t offsetDxFilter;
+
+ int32_t rawOffsetUs;
+ int32_t previousRawOffsetUs;
+
+ int32_t offsetUs;
+ int32_t offsetDeltaUs;
+ int32_t previousOffsetUs;
+} phaseLockState_t;
+
+static phaseLockState_t pl;
+
+static void expressLrsPhaseLockReset(void)
+{
+ simpleLPFilterInit(&pl.offsetFilter, 2, 5);
+ simpleLPFilterInit(&pl.offsetDxFilter, 4, 5);
+
+ expressLrsEPRReset();
+}
+
+static uint8_t nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
+
+static uint8_t telemetryBurstCount = 1;
+static uint8_t telemetryBurstMax = 1;
+static bool telemBurstValid = false;
+
+// Maximum ms between LINK_STATISTICS packets for determining burst max
+#define TELEM_MIN_LINK_INTERVAL 512U
+
+#ifdef USE_MSP_OVER_TELEMETRY
+static uint8_t mspBuffer[ELRS_MSP_BUFFER_SIZE];
+#endif
+
+//
+// Stick unpacking
+//
+
+static void setRssiChannelData(uint16_t *rcData)
+{
+ rcData[ELRS_LQ_CHANNEL] = scaleRange(receiver.uplinkLQ, 0, 100, 988, 2011);
+ rcData[ELRS_RSSI_CHANNEL] = scaleRange(constrain(receiver.rssiFiltered, receiver.rfPerfParams->sensitivity, -50), receiver.rfPerfParams->sensitivity, -50, 988, 2011);
+}
+
+static void unpackAnalogChannelData(uint16_t *rcData, const uint8_t *payload)
+{
+ rcData[0] = convertAnalog((payload[0] << 3) | ((payload[4] & 0xC0) >> 5));
+ rcData[1] = convertAnalog((payload[1] << 3) | ((payload[4] & 0x30) >> 3));
+ rcData[2] = convertAnalog((payload[2] << 3) | ((payload[4] & 0x0C) >> 1));
+ rcData[3] = convertAnalog((payload[3] << 3) | ((payload[4] & 0x03) << 1));
+}
+
+/**
+ * Hybrid switches uses 10 bits for each analog channel,
+ * 2 bits for the low latency switch[0]
+ * 3 bits for the round-robin switch index and 2 bits for the value
+ * 4 analog channels, 1 low latency switch and round robin switch data = 47 bits (1 free)
+ *
+ * sets telemetry status bit
+ */
+static void unpackChannelDataHybridSwitch8(uint16_t *rcData, const uint8_t *payload)
+{
+ unpackAnalogChannelData(rcData, payload);
+
+ const uint8_t switchByte = payload[5];
+
+ // The low latency switch
+ rcData[4] = convertSwitch1b((switchByte & 0x40) >> 6);
+
+ // The round-robin switch, switchIndex is actually index-1
+ // to leave the low bit open for switch 7 (sent as 0b11x)
+ // where x is the high bit of switch 7
+ uint8_t switchIndex = (switchByte & 0x38) >> 3;
+ uint16_t switchValue = convertSwitch3b(switchByte & 0x07);
+
+ switch (switchIndex) {
+ case 0:
+ rcData[5] = switchValue;
+ break;
+ case 1:
+ rcData[6] = switchValue;
+ break;
+ case 2:
+ rcData[7] = switchValue;
+ break;
+ case 3:
+ rcData[8] = switchValue;
+ break;
+ case 4:
+ rcData[9] = switchValue;
+ break;
+ case 5:
+ rcData[10] = switchValue;
+ break;
+ case 6:
+ FALLTHROUGH;
+ case 7:
+ rcData[11] = convertSwitchNb(switchByte & 0x0F, 15); //4-bit
+ break;
+ default:
+ break;
+ }
+
+ setRssiChannelData(rcData);
+}
+
+/**
+ * HybridWide switches decoding of over the air data
+ *
+ * Hybrid switches uses 10 bits for each analog channel,
+ * 1 bits for the low latency switch[0]
+ * 6 or 7 bits for the round-robin switch
+ * 1 bit for the TelemetryStatus, which may be in every packet or just idx 7
+ * depending on TelemetryRatio
+ *
+ * Output: crsf.PackedRCdataOut, crsf.LinkStatistics.uplink_TX_Power
+ * Returns: TelemetryStatus bit
+ */
+static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload)
+{
+ unpackAnalogChannelData(rcData, payload);
+ const uint8_t switchByte = payload[5];
+
+ // The low latency switch
+ rcData[4] = convertSwitch1b((switchByte & 0x80) >> 7);
+
+ // The round-robin switch, 6-7 bits with the switch index implied by the nonce. Some logic moved to processRFPacket
+ if (wideSwitchIndex >= 7) {
+ txPower = switchByte & 0x3F;
+ } else {
+ uint8_t bins;
+ uint16_t switchValue;
+ if (tlmRatioEnumToValue(receiver.modParams->tlmInterval) < 8) {
+ bins = 63;
+ switchValue = switchByte & 0x3F; // 6-bit
+ } else {
+ bins = 127;
+ switchValue = switchByte & 0x7F; // 7-bit
+ }
+
+ switchValue = convertSwitchNb(switchValue, bins);
+ rcData[5 + wideSwitchIndex] = switchValue;
+ }
+
+ setRssiChannelData(rcData);
+}
+
+static void startReceiving(void)
+{
+ dbgPinLo(1);
+ receiver.startReceiving();
+}
+
+static uint8_t minLqForChaos(void)
+{
+ // Determine the most number of CRC-passing packets we could receive on
+ // a single channel out of 100 packets that fill the LQcalc span.
+ // The LQ must be GREATER THAN this value, not >=
+ // The amount of time we coexist on the same channel is
+ // 100 divided by the total number of packets in a FHSS loop (rounded up)
+ // and there would be 4x packets received each time it passes by so
+ // FHSShopInterval * ceil(100 / FHSShopInterval * numfhss) or
+ // FHSShopInterval * trunc((100 + (FHSShopInterval * numfhss) - 1) / (FHSShopInterval * numfhss))
+ // With a interval of 4 this works out to: 2.4=4, FCC915=4, AU915=8, EU868=8, EU/AU433=36
+ const uint32_t numfhss = getFHSSNumEntries();
+ const uint8_t interval = receiver.modParams->fhssHopInterval;
+ return interval * ((interval * numfhss + 99) / (interval * numfhss));
+}
+
+static void setRFLinkRate(const uint8_t index)
+{
+#if defined(USE_RX_SX1280) && defined(USE_RX_SX127X)
+ receiver.modParams = (rxExpressLrsSpiConfig()->domain == ISM2400) ? &airRateConfig[1][index] : &airRateConfig[0][index];
+ receiver.rfPerfParams = (rxExpressLrsSpiConfig()->domain == ISM2400) ? &rfPerfConfig[1][index] : &rfPerfConfig[0][index];
+#else
+ receiver.modParams = &airRateConfig[0][index];
+ receiver.rfPerfParams = &rfPerfConfig[0][index];
+#endif
+ receiver.currentFreq = getInitialFreq(receiver.freqOffset);
+ // Wait for (11/10) 110% of time it takes to cycle through all freqs in FHSS table (in ms)
+ receiver.cycleIntervalMs = ((uint32_t)11U * getFHSSNumEntries() * receiver.modParams->fhssHopInterval * receiver.modParams->interval) / (10U * 1000U);
+
+ receiver.config(receiver.modParams->bw, receiver.modParams->sf, receiver.modParams->cr, receiver.currentFreq, receiver.modParams->preambleLen, receiver.UID[5] & 0x01);
+
+ expressLrsUpdateTimerInterval(receiver.modParams->interval);
+
+ rssiFilterReset();
+ receiver.nextRateIndex = index; // presumably we just handled this
+ telemBurstValid = false;
+
+#ifdef USE_RX_LINK_QUALITY_INFO
+ rxSetRfMode((uint8_t)RATE_4HZ - (uint8_t)receiver.modParams->enumRate);
+#endif
+}
+
+static bool handleFHSS(void)
+{
+ uint8_t modresultFHSS = (receiver.nonceRX + 1) % receiver.modParams->fhssHopInterval;
+
+ if ((receiver.modParams->fhssHopInterval == 0) || receiver.alreadyFHSS == true || receiver.inBindingMode || (modresultFHSS != 0) || (receiver.connectionState == ELRS_DISCONNECTED)) {
+ return false;
+ }
+
+ receiver.alreadyFHSS = true;
+ receiver.currentFreq = FHSSgetNextFreq(receiver.freqOffset);
+ receiver.setFrequency(receiver.currentFreq);
+
+ uint8_t modresultTLM = (receiver.nonceRX + 1) % (tlmRatioEnumToValue(receiver.modParams->tlmInterval));
+
+ if (modresultTLM != 0 || receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) { // if we are about to send a tlm response don't bother going back to rx
+ startReceiving();
+ }
+ return true;
+}
+
+static bool handleSendTelemetryResponse(void)
+{
+ uint8_t packet[8];
+
+ uint8_t *data;
+ uint8_t maxLength;
+ uint8_t packageIndex;
+ uint8_t modresult = (receiver.nonceRX + 1) % tlmRatioEnumToValue(receiver.modParams->tlmInterval);
+
+ if ((receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM)
+ || (receiver.alreadyTLMresp == true) || (modresult != 0)) {
+ return false; // don't bother sending tlm if disconnected or TLM is off
+ }
+
+ receiver.alreadyTLMresp = true;
+ packet[0] = ELRS_TLM_PACKET;
+
+ if (nextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || !isTelemetrySenderActive()) {
+ packet[1] = ELRS_TELEMETRY_TYPE_LINK;
+ packet[2] = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; //diversity not supported
+ packet[3] = connectionHasModelMatch << 7;
+ packet[4] = receiver.snr;
+ packet[5] = receiver.uplinkLQ;
+#ifdef USE_MSP_OVER_TELEMETRY
+ packet[6] = getCurrentMspConfirm() ? 1 : 0;
+#else
+ packet[6] = 0;
+#endif
+ nextTelemetryType = ELRS_TELEMETRY_TYPE_DATA;
+ // Start the count at 1 because the next will be DATA and doing +1 before checking
+ // against Max below is for some reason 10 bytes more code
+ telemetryBurstCount = 1;
+ } else {
+ if (telemetryBurstCount < telemetryBurstMax) {
+ telemetryBurstCount++;
+ } else {
+ nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
+ }
+
+ getCurrentTelemetryPayload(&packageIndex, &maxLength, &data);
+ packet[1] = (packageIndex << ELRS_TELEMETRY_SHIFT) + ELRS_TELEMETRY_TYPE_DATA;
+ packet[2] = maxLength > 0 ? *data : 0;
+ packet[3] = maxLength >= 1 ? *(data + 1) : 0;
+ packet[4] = maxLength >= 2 ? *(data + 2) : 0;
+ packet[5] = maxLength >= 3 ? *(data + 3) : 0;
+ packet[6] = maxLength >= 4 ? *(data + 4) : 0;
+ }
+
+ uint16_t crc = calcCrc14(packet, 7, crcInitializer);
+ packet[0] |= (crc >> 6) & 0xFC;
+ packet[7] = crc & 0xFF;
+
+ dbgPinHi(1);
+ receiver.transmitData(packet, ELRS_RX_TX_BUFF_SIZE);
+ return true;
+}
+
+static void updatePhaseLock(void)
+{
+ if (receiver.connectionState != ELRS_DISCONNECTED && expressLrsEPRHaveBothEvents()) {
+ pl.rawOffsetUs = expressLrsEPRGetResult();
+
+ pl.offsetUs = simpleLPFilterUpdate(&pl.offsetFilter, pl.rawOffsetUs);
+ pl.offsetDeltaUs = simpleLPFilterUpdate(&pl.offsetDxFilter, pl.rawOffsetUs - pl.previousRawOffsetUs);
+
+ if (receiver.timerState == ELRS_TIM_LOCKED && lqPeriodIsSet()) {
+ if (receiver.nonceRX % 8 == 0) { //limit rate of freq offset adjustment slightly
+ if (pl.offsetUs > 0) {
+ expressLrsTimerIncreaseFrequencyOffset();
+ } else if (pl.offsetUs < 0) {
+ expressLrsTimerDecreaseFrequencyOffset();
+ }
+ }
+ }
+
+ if (receiver.connectionState != ELRS_CONNECTED) {
+ expressLrsUpdatePhaseShift(pl.rawOffsetUs >> 1);
+ } else {
+ expressLrsUpdatePhaseShift(pl.offsetUs >> 2);
+ }
+
+ pl.previousOffsetUs = pl.offsetUs;
+ pl.previousRawOffsetUs = pl.rawOffsetUs;
+
+ expressLrsTimerDebug();
+
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 0, pl.rawOffsetUs);
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 1, pl.offsetUs);
+ }
+
+ expressLrsEPRReset();
+}
+
+//hwTimerCallbackTick
+void expressLrsOnTimerTickISR(void) // this is 180 out of phase with the other callback, occurs mid-packet reception
+{
+ updatePhaseLock();
+ receiver.nonceRX += 1;
+
+ // Save the LQ value before the inc() reduces it by 1
+ receiver.uplinkLQ = lqGet();
+ // Only advance the LQI period counter if we didn't send Telemetry this period
+ if (!receiver.alreadyTLMresp) {
+ lqNewPeriod();
+ }
+
+ receiver.alreadyTLMresp = false;
+ receiver.alreadyFHSS = false;
+}
+
+//hwTimerCallbackTock
+void expressLrsOnTimerTockISR(void)
+{
+ uint32_t currentTimeUs = micros();
+
+ expressLrsEPRRecordEvent(EPR_INTERNAL, currentTimeUs);
+
+ receiver.fhssRequired = true; //Rest of the code is moved to expressLrsDataReceived to avoid race condition
+}
+
+static uint16_t lostConnectionCounter = 0;
+
+void lostConnection(void)
+{
+ lostConnectionCounter++;
+
+ receiver.rfModeCycleMultiplier = 1;
+ receiver.connectionState = ELRS_DISCONNECTED; //set lost connection
+ receiver.timerState = ELRS_TIM_DISCONNECTED;
+ expressLrsTimerResetFrequencyOffset();
+ receiver.freqOffset = 0;
+ pl.offsetUs = 0;
+ pl.offsetDeltaUs = 0;
+ pl.rawOffsetUs = 0;
+ pl.previousRawOffsetUs = 0;
+ receiver.gotConnectionMs = 0;
+ receiver.uplinkLQ = 0;
+ lqReset();
+ expressLrsPhaseLockReset();
+ receiver.alreadyTLMresp = false;
+ receiver.alreadyFHSS = false;
+
+ if (!receiver.inBindingMode) {
+ //while (micros() - expressLrsEPRGetResult() > 250); // time it just after the tock() TODO this currently breaks and is blocking, not a fan of this.
+ expressLrsTimerStop();
+ setRFLinkRate(receiver.nextRateIndex); // also sets to initialFreq
+ startReceiving();
+ }
+}
+
+static void tentativeConnection(const uint32_t timeStampMs)
+{
+ receiver.connectionState = ELRS_TENTATIVE;
+ connectionHasModelMatch = false;
+ receiver.timerState = ELRS_TIM_DISCONNECTED;
+ receiver.freqOffset = 0;
+ pl.offsetUs = 0;
+ pl.previousRawOffsetUs = 0;
+ expressLrsPhaseLockReset(); //also resets PFD
+ receiver.rfModeCycledAtMs = timeStampMs; // give another 3 sec for lock to occur
+
+ // The caller MUST call hwTimer.resume(). It is not done here because
+ // the timer ISR will fire immediately and preempt any other code
+}
+
+static void gotConnection(const uint32_t timeStampMs)
+{
+ if (receiver.connectionState == ELRS_CONNECTED) {
+ return; // Already connected
+ }
+
+ receiver.lockRFmode = true; // currently works as if LOCK_ON_FIRST_CONNECTION was enabled
+
+ receiver.connectionState = ELRS_CONNECTED; //we got a packet, therefore no lost connection
+ receiver.timerState = ELRS_TIM_TENTATIVE;
+ receiver.gotConnectionMs = timeStampMs;
+
+ if (rxExpressLrsSpiConfig()->rateIndex != receiver.rateIndex) {
+ rxExpressLrsSpiConfigMutable()->rateIndex = receiver.rateIndex;
+ receiver.configChanged = true;
+ }
+}
+
+//setup radio
+static void initializeReceiver(void)
+{
+ FHSSrandomiseFHSSsequence(receiver.UID, rxExpressLrsSpiConfig()->domain);
+ lqReset();
+ receiver.nonceRX = 0;
+ receiver.freqOffset = 0;
+ receiver.configChanged = false;
+ receiver.rssi = 0;
+ receiver.rssiFiltered = 0;
+ receiver.snr = 0;
+ receiver.uplinkLQ = 0;
+ receiver.rateIndex = receiver.inBindingMode ? bindingRateIndex : rxExpressLrsSpiConfig()->rateIndex;
+ setRFLinkRate(receiver.rateIndex);
+
+ receiver.alreadyFHSS = false;
+ receiver.alreadyTLMresp = false;
+ receiver.lockRFmode = false;
+ receiver.timerState = ELRS_TIM_DISCONNECTED;
+ receiver.connectionState = ELRS_DISCONNECTED;
+
+ uint32_t timeStampMs = millis();
+
+ receiver.rfModeCycledAtMs = timeStampMs;
+ receiver.configCheckedAtMs = timeStampMs;
+ receiver.statsUpdatedAtMs = timeStampMs;
+ receiver.gotConnectionMs = timeStampMs;
+ receiver.lastSyncPacketMs = timeStampMs;
+ receiver.lastValidPacketMs = timeStampMs;
+
+ receiver.rfModeCycleMultiplier = 1;
+}
+
+static void unpackBindPacket(uint8_t *packet)
+{
+ rxExpressLrsSpiConfigMutable()->UID[2] = packet[3];
+ rxExpressLrsSpiConfigMutable()->UID[3] = packet[4];
+ rxExpressLrsSpiConfigMutable()->UID[4] = packet[5];
+ rxExpressLrsSpiConfigMutable()->UID[5] = packet[6];
+
+ receiver.UID = rxExpressLrsSpiConfigMutable()->UID;
+ crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5];
+ receiver.inBindingMode = false;
+
+ initializeReceiver();
+
+ receiver.configChanged = true; //after initialize as it sets it to false
+ startReceiving();
+}
+
+/**
+ * Process the assembled MSP packet in mspBuffer[]
+ **/
+static void processRFMspPacket(uint8_t *packet)
+{
+ // Always examine MSP packets for bind information if in bind mode
+ // [1] is the package index, first packet of the MSP
+ if (receiver.inBindingMode && packet[1] == 1 && packet[2] == ELRS_MSP_BIND) {
+ unpackBindPacket(packet); //onELRSBindMSP
+ return;
+ }
+
+#ifdef USE_MSP_OVER_TELEMETRY
+ // Must be fully connected to process MSP, prevents processing MSP
+ // during sync, where packets can be received before connection
+ if (receiver.connectionState != ELRS_CONNECTED) {
+ return;
+ }
+
+ bool currentMspConfirmValue = getCurrentMspConfirm();
+ receiveMspData(packet[1], packet + 2);
+ if (currentMspConfirmValue != getCurrentMspConfirm()) {
+ nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
+ }
+ if (hasFinishedMspData()) {
+ if (mspBuffer[ELRS_MSP_COMMAND_INDEX] == ELRS_MSP_SET_RX_CONFIG && mspBuffer[ELRS_MSP_COMMAND_INDEX + 1] == ELRS_MSP_MODEL_ID) { //mspReceiverComplete
+ if (rxExpressLrsSpiConfig()->modelId != mspBuffer[9]) { //UpdateModelMatch
+ rxExpressLrsSpiConfigMutable()->modelId = mspBuffer[9];
+ receiver.configChanged = true;
+ receiver.connectionState = ELRS_DISCONNECT_PENDING;
+ }
+ } else if (connectionHasModelMatch) {
+ processMspPacket(mspBuffer);
+ }
+
+ mspReceiverUnlock();
+ }
+#endif
+}
+
+static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
+{
+ // Verify the first two of three bytes of the binding ID, which should always match
+ if (packet[4] != receiver.UID[3] || packet[5] != receiver.UID[4]) {
+ return false;
+ }
+
+ // The third byte will be XORed with inverse of the ModelId if ModelMatch is on
+ // Only require the first 18 bits of the UID to match to establish a connection
+ // but the last 6 bits must modelmatch before sending any data to the FC
+ if ((packet[6] & ~ELRS_MODELMATCH_MASK) != (receiver.UID[5] & ~ELRS_MODELMATCH_MASK)) {
+ return false;
+ }
+
+ receiver.lastSyncPacketMs = timeStampMs;
+
+ // Will change the packet air rate in loop() if this changes
+ receiver.nextRateIndex = (packet[3] & 0xC0) >> 6;
+ uint8_t tlmRateIn = (packet[3] & 0x38) >> 3;
+ uint8_t switchEncMode = ((packet[3] & 0x06) >> 1) - 1; //spi implementation uses 0 based index for hybrid
+
+ // Update switch mode encoding immediately
+ if (switchEncMode != rxExpressLrsSpiConfig()->switchMode) {
+ rxExpressLrsSpiConfigMutable()->switchMode = switchEncMode;
+ receiver.configChanged = true;
+ }
+
+ // Update TLM ratio
+ if (receiver.modParams->tlmInterval != tlmRateIn) {
+ receiver.modParams->tlmInterval = tlmRateIn;
+ telemBurstValid = false;
+ }
+
+ // modelId = 0xff indicates modelMatch is disabled, the XOR does nothing in that case
+ uint8_t modelXor = (~rxExpressLrsSpiConfig()->modelId) & ELRS_MODELMATCH_MASK;
+ bool modelMatched = packet[6] == (receiver.UID[5] ^ modelXor);
+
+ if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != packet[2] || FHSSgetCurrIndex() != packet[1] || connectionHasModelMatch != modelMatched) {
+ FHSSsetCurrIndex(packet[1]);
+ receiver.nonceRX = packet[2];
+
+ tentativeConnection(timeStampMs);
+ connectionHasModelMatch = modelMatched;
+
+ if (!expressLrsTimerIsRunning()) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+static rx_spi_received_e processRFPacket(uint8_t *payload)
+{
+ uint8_t packet[ELRS_RX_TX_BUFF_SIZE];
+
+ receiver.receiveData(packet, ELRS_RX_TX_BUFF_SIZE);
+
+ uint32_t timeStampUs = micros();
+
+ elrsPacketType_e type = packet[0] & 0x03;
+ uint16_t inCRC = (((uint16_t)(packet[0] & 0xFC)) << 6 ) | packet[7];
+
+ // For SM_HYBRID the CRC only has the packet type in byte 0
+ // For SM_HYBRID_WIDE the FHSS slot is added to the CRC in byte 0 on RC_DATA_PACKETs
+ if (type != ELRS_RC_DATA_PACKET || rxExpressLrsSpiConfig()->switchMode != SM_HYBRID_WIDE) {
+ packet[0] = type;
+ } else {
+ uint8_t nonceFHSSresult = receiver.nonceRX % receiver.modParams->fhssHopInterval;
+ packet[0] = type | (nonceFHSSresult << 2);
+ }
+ uint16_t calculatedCRC = calcCrc14(packet, 7, crcInitializer);
+
+ if (inCRC != calculatedCRC) {
+ return RX_SPI_RECEIVED_NONE;
+ }
+
+ expressLrsEPRRecordEvent(EPR_EXTERNAL, timeStampUs + PACKET_HANDLING_TO_TOCK_ISR_DELAY_US);
+
+ bool shouldStartTimer = false;
+ uint32_t timeStampMs = millis();
+
+ receiver.lastValidPacketMs = timeStampMs;
+
+ switch(type) {
+ case ELRS_RC_DATA_PACKET:
+ // Must be fully connected to process RC packets, prevents processing RC
+ // during sync, where packets can be received before connection
+ if (receiver.connectionState == ELRS_CONNECTED && connectionHasModelMatch) {
+ if (rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) {
+ wideSwitchIndex = hybridWideNonceToSwitchIndex(receiver.nonceRX);
+ if ((tlmRatioEnumToValue(receiver.modParams->tlmInterval) < 8) || wideSwitchIndex == 7) {
+ confirmCurrentTelemetryPayload((packet[6] & 0x40) >> 6);
+ }
+ } else {
+ confirmCurrentTelemetryPayload(packet[6] & (1 << 7));
+ }
+ memcpy(payload, &packet[1], 6); // stick data handling is done in expressLrsSetRcDataFromPayload
+ }
+ break;
+ case ELRS_MSP_DATA_PACKET:
+ processRFMspPacket(packet);
+ break;
+ case ELRS_TLM_PACKET:
+ //not implemented
+ break;
+ case ELRS_SYNC_PACKET:
+ shouldStartTimer = processRFSyncPacket(packet, timeStampMs) && !receiver.inBindingMode;
+ break;
+ default:
+ return RX_SPI_RECEIVED_NONE;
+ }
+
+ // Store the LQ/RSSI/Antenna
+ receiver.getRFlinkInfo(&receiver.rssi, &receiver.snr);
+ // Received a packet, that's the definition of LQ
+ lqIncrease();
+ // Extend sync duration since we've received a packet at this rate
+ // but do not extend it indefinitely
+ receiver.rfModeCycleMultiplier = ELRS_MODE_CYCLE_MULTIPLIER_SLOW; //RFModeCycleMultiplierSlow
+
+ if (shouldStartTimer) {
+ expressLrsTimerResume();
+ }
+
+ receiver.fhssRequired = true;
+
+ return RX_SPI_RECEIVED_DATA;
+}
+
+static void updateTelemetryBurst(void)
+{
+ if (telemBurstValid) {
+ return;
+ }
+ telemBurstValid = true;
+
+ uint32_t hz = rateEnumToHz(receiver.modParams->enumRate);
+ uint32_t ratiodiv = tlmRatioEnumToValue(receiver.modParams->tlmInterval);
+ telemetryBurstMax = TELEM_MIN_LINK_INTERVAL * hz / ratiodiv / 1000U;
+
+ // Reserve one slot for LINK telemetry
+ if (telemetryBurstMax > 1) {
+ --telemetryBurstMax;
+ } else {
+ telemetryBurstMax = 1;
+ }
+
+ // Notify the sender to adjust its expected throughput
+ updateTelemetryRate(hz, ratiodiv, telemetryBurstMax);
+}
+
+/* If not connected will rotate through the RF modes looking for sync
+ * and blink LED
+ */
+static void cycleRfMode(const uint32_t timeStampMs)
+{
+ if (receiver.connectionState == ELRS_CONNECTED || receiver.inBindingMode) {
+ return;
+ }
+ // Actually cycle the RF mode if not LOCK_ON_FIRST_CONNECTION
+ if (receiver.lockRFmode == false && (timeStampMs - receiver.rfModeCycledAtMs) > (receiver.cycleIntervalMs * receiver.rfModeCycleMultiplier)) {
+ receiver.rfModeCycledAtMs = timeStampMs;
+ receiver.lastSyncPacketMs = timeStampMs; // reset this variable
+ receiver.rateIndex = (receiver.rateIndex + 1) % ELRS_RATE_MAX;
+ setRFLinkRate(receiver.rateIndex); // switch between rates
+ receiver.statsUpdatedAtMs = timeStampMs;
+ lqReset();
+ startReceiving();
+
+ // Switch to FAST_SYNC if not already in it (won't be if was just connected)
+ receiver.rfModeCycleMultiplier = 1;
+ } // if time to switch RF mode
+}
+
+#ifdef USE_RX_SX1280
+static inline void configureReceiverForSX1280(void)
+{
+ receiver.init = (elrsRxInitFnPtr) sx1280Init;
+ receiver.config = (elrsRxConfigFnPtr) sx1280Config;
+ receiver.startReceiving = (elrsRxStartReceivingFnPtr) sx1280StartReceiving;
+ receiver.rxISR = (elrsRxISRFnPtr) sx1280ISR;
+ receiver.transmitData = (elrsRxTransmitDataFnPtr) sx1280TransmitData;
+ receiver.receiveData = (elrsRxReceiveDataFnPtr) sx1280ReceiveData;
+ receiver.getRFlinkInfo = (elrsRxGetRFlinkInfoFnPtr) sx1280GetLastPacketStats;
+ receiver.setFrequency = (elrsRxSetFrequencyFnPtr) sx1280SetFrequencyReg;
+ receiver.handleFreqCorrection = (elrsRxHandleFreqCorrectionFnPtr) sx1280AdjustFrequency;
+}
+#endif
+
+#ifdef USE_RX_SX127X
+static inline void configureReceiverForSX127x(void)
+{
+ receiver.init = (elrsRxInitFnPtr) sx127xInit;
+ receiver.config = (elrsRxConfigFnPtr) sx127xConfig;
+ receiver.startReceiving = (elrsRxStartReceivingFnPtr) sx127xStartReceiving;
+ receiver.rxISR = (elrsRxISRFnPtr) sx127xISR;
+ receiver.transmitData = (elrsRxTransmitDataFnPtr) sx127xTransmitData;
+ receiver.receiveData = (elrsRxReceiveDataFnPtr) sx127xReceiveData;
+ receiver.getRFlinkInfo = (elrsRxGetRFlinkInfoFnPtr) sx127xGetLastPacketStats;
+ receiver.setFrequency = (elrsRxSetFrequencyFnPtr) sx127xSetFrequencyReg;
+ receiver.handleFreqCorrection = (elrsRxHandleFreqCorrectionFnPtr) sx127xAdjustFrequency;
+}
+#endif
+
+//setup
+bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
+{
+ if (!rxSpiExtiConfigured()) {
+ return false;
+ }
+
+ rxSpiCommonIOInit(rxConfig);
+
+ rxRuntimeState->channelCount = ELRS_MAX_CHANNELS;
+
+ extiConfig->ioConfig = IOCFG_IPD;
+ extiConfig->trigger = BETAFLIGHT_EXTI_TRIGGER_RISING;
+
+ if (rxExpressLrsSpiConfig()->resetIoTag) {
+ receiver.resetPin = IOGetByTag(rxExpressLrsSpiConfig()->resetIoTag);
+ } else {
+ receiver.resetPin = IO_NONE;
+ }
+
+ if (rxExpressLrsSpiConfig()->busyIoTag) {
+ receiver.busyPin = IOGetByTag(rxExpressLrsSpiConfig()->busyIoTag);
+ } else {
+ receiver.busyPin = IO_NONE;
+ }
+
+ switch (rxExpressLrsSpiConfig()->domain) {
+#ifdef USE_RX_SX127X
+ case AU433:
+ FALLTHROUGH;
+ case AU915:
+ FALLTHROUGH;
+ case EU433:
+ FALLTHROUGH;
+ case EU868:
+ FALLTHROUGH;
+ case IN866:
+ FALLTHROUGH;
+ case FCC915:
+ configureReceiverForSX127x();
+ bindingRateIndex = ELRS_BINDING_RATE_900;
+ break;
+#endif
+#ifdef USE_RX_SX1280
+ case ISM2400:
+ configureReceiverForSX1280();
+ bindingRateIndex = ELRS_BINDING_RATE_24;
+ break;
+#endif
+ default:
+ return false;
+ }
+
+ if (!receiver.init(receiver.resetPin, receiver.busyPin)) {
+ return false;
+ }
+
+ if (rssiSource == RSSI_SOURCE_NONE) {
+ rssiSource = RSSI_SOURCE_RX_PROTOCOL;
+ }
+
+ if (linkQualitySource == LQ_SOURCE_NONE) {
+ linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
+ }
+
+ if (rxExpressLrsSpiConfig()->UID[0] || rxExpressLrsSpiConfig()->UID[1]
+ || rxExpressLrsSpiConfig()->UID[2] || rxExpressLrsSpiConfig()->UID[3]
+ || rxExpressLrsSpiConfig()->UID[4] || rxExpressLrsSpiConfig()->UID[5]) {
+ receiver.inBindingMode = false;
+ receiver.UID = rxExpressLrsSpiConfig()->UID;
+ crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5];
+ } else {
+ receiver.inBindingMode = true;
+ receiver.UID = BindingUID;
+ crcInitializer = 0;
+ }
+
+ expressLrsPhaseLockReset();
+
+ expressLrsInitialiseTimer(RX_EXPRESSLRS_TIMER_INSTANCE, &receiver.timerUpdateCb);
+ expressLrsTimerStop();
+
+ generateCrc14Table();
+ initializeReceiver();
+
+ initTelemetry();
+#ifdef USE_MSP_OVER_TELEMETRY
+ setMspDataToReceive(ELRS_MSP_BUFFER_SIZE, mspBuffer, ELRS_MSP_BYTES_PER_CALL);
+#endif
+
+ // Timer IRQs must only be enabled after the receiver is configured otherwise race conditions occur.
+ expressLrsTimerEnableIRQs();
+
+ startReceiving();
+
+ return true;
+}
+
+static void handleConnectionStateUpdate(const uint32_t timeStampMs)
+{
+ if ((receiver.connectionState != ELRS_DISCONNECTED) && (receiver.modParams->index != receiver.nextRateIndex)) { // forced change
+ lostConnection();
+ receiver.lastSyncPacketMs = timeStampMs; // reset this variable to stop rf mode switching and add extra time
+ receiver.rfModeCycledAtMs = timeStampMs; // reset this variable to stop rf mode switching and add extra time
+ setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
+#ifdef USE_RX_RSSI_DBM
+ setRssiDbmDirect(-130, RSSI_SOURCE_RX_PROTOCOL);
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+ setLinkQualityDirect(0);
+#endif
+ }
+
+ if (receiver.connectionState == ELRS_TENTATIVE && ((timeStampMs - receiver.lastSyncPacketMs) > receiver.rfPerfParams->rxLockTimeoutMs)) {
+ lostConnection();
+ receiver.rfModeCycledAtMs = timeStampMs;
+ receiver.lastSyncPacketMs = timeStampMs;
+ }
+
+ cycleRfMode(timeStampMs);
+
+ uint32_t localLastValidPacket = receiver.lastValidPacketMs; // Required to prevent race condition due to LastValidPacket getting updated from ISR
+ if ((receiver.connectionState == ELRS_DISCONNECT_PENDING) || // check if we lost conn.
+ ((receiver.connectionState == ELRS_CONNECTED) && ((int32_t)receiver.rfPerfParams->disconnectTimeoutMs < (int32_t)(timeStampMs - localLastValidPacket)))) {
+ lostConnection();
+ }
+
+ if ((receiver.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) { //detects when we are connected
+ gotConnection(timeStampMs);
+ }
+
+ if ((receiver.timerState == ELRS_TIM_TENTATIVE) && ((timeStampMs - receiver.gotConnectionMs) > ELRS_CONSIDER_CONNECTION_GOOD_MS) && (ABS(pl.offsetDeltaUs) <= 5)) {
+ receiver.timerState = ELRS_TIM_LOCKED;
+ }
+}
+
+static void handleConfigUpdate(const uint32_t timeStampMs)
+{
+ if ((timeStampMs - receiver.configCheckedAtMs) > ELRS_CONFIG_CHECK_MS) {
+ receiver.configCheckedAtMs = timeStampMs;
+ if (receiver.configChanged) {
+ lostConnection();
+ writeEEPROM();
+ receiver.configChanged = false;
+ }
+ }
+}
+
+static void handleLinkStatsUpdate(const uint32_t timeStampMs)
+{
+ if ((timeStampMs - receiver.statsUpdatedAtMs) > ELRS_LINK_STATS_CHECK_MS) {
+
+ receiver.statsUpdatedAtMs = timeStampMs;
+
+ if (receiver.connectionState != ELRS_CONNECTED) {
+ setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
+#ifdef USE_RX_RSSI_DBM
+ setRssiDbmDirect(-130, RSSI_SOURCE_RX_PROTOCOL);
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+ setLinkQualityDirect(0);
+#endif
+ } else {
+ receiver.rssiFiltered = simpleLPFilterUpdate(&rssiFilter, receiver.rssi);
+ uint16_t rssiScaled = scaleRange(constrain(receiver.rssiFiltered, receiver.rfPerfParams->sensitivity, -50), receiver.rfPerfParams->sensitivity, -50, 0, 1023);
+ setRssi(rssiScaled, RSSI_SOURCE_RX_PROTOCOL);
+#ifdef USE_RX_RSSI_DBM
+ setRssiDbm(receiver.rssiFiltered, RSSI_SOURCE_RX_PROTOCOL);
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+ setLinkQualityDirect(receiver.uplinkLQ);
+#endif
+#ifdef USE_RX_LINK_UPLINK_POWER
+ rxSetUplinkTxPwrMw(txPowerIndexToValue(txPower));
+#endif
+ }
+ }
+}
+
+static void handleTelemetryUpdate(void)
+{
+ if (receiver.connectionState != ELRS_CONNECTED || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM)) {
+ return;
+ }
+
+ uint8_t *nextPayload = 0;
+ uint8_t nextPlayloadSize = 0;
+ if (!isTelemetrySenderActive() && getNextTelemetryPayload(&nextPlayloadSize, &nextPayload)) {
+ setTelemetryDataToTransmit(nextPlayloadSize, nextPayload, ELRS_TELEMETRY_BYTES_PER_CALL);
+ }
+ updateTelemetryBurst();
+}
+
+void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ if (rcData && payload) {
+ rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE ? unpackChannelDataHybridWide(rcData, payload) : unpackChannelDataHybridSwitch8(rcData, payload);
+ }
+}
+
+static void enterBindingMode(void)
+{
+ if ((receiver.connectionState == ELRS_CONNECTED) || receiver.inBindingMode) {
+ // Don't enter binding if:
+ // - we're already connected
+ // - we're already binding
+ return;
+ }
+
+ // Set UID to special binding values
+ receiver.UID = BindingUID;
+ crcInitializer = 0;
+ receiver.inBindingMode = true;
+
+ setRFLinkRate(bindingRateIndex);
+ startReceiving();
+}
+
+rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
+{
+ rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
+ uint32_t isrTimeStampUs;
+
+ if (rxSpiCheckBindRequested(true)) {
+ enterBindingMode();
+ }
+
+ uint8_t irqReason = receiver.rxISR(&isrTimeStampUs);
+ if (irqReason == ELRS_DIO_TX_DONE) {
+ startReceiving();
+ } else if (irqReason == ELRS_DIO_RX_DONE) {
+ result = processRFPacket(payload);
+ }
+
+ if (receiver.fhssRequired) {
+ receiver.fhssRequired = false;
+ bool didFHSS = handleFHSS();
+ bool tlmSent = handleSendTelemetryResponse();
+
+ if (!didFHSS && !tlmSent && lqPeriodIsSet() && rxExpressLrsSpiConfig()->domain != ISM2400) {
+ receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset
+ }
+ }
+
+ handleTelemetryUpdate();
+
+ const uint32_t timeStampMs = millis();
+
+ handleConnectionStateUpdate(timeStampMs);
+ handleConfigUpdate(timeStampMs);
+ handleLinkStatsUpdate(timeStampMs);
+
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 0, lostConnectionCounter);
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 1, receiver.rssiFiltered);
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 2, receiver.snr);
+ DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 3, receiver.uplinkLQ);
+
+ receiver.inBindingMode ? rxSpiLedBlinkBind() : rxSpiLedBlinkRxLoss(result);
+
+ return result;
+}
+
+#endif /* USE_RX_EXPRESSLRS */
diff --git a/src/main/rx/expresslrs.h b/src/main/rx/expresslrs.h
new file mode 100644
index 0000000000..c6ae3b874c
--- /dev/null
+++ b/src/main/rx/expresslrs.h
@@ -0,0 +1,36 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#pragma once
+
+#include
+#include
+
+#include "drivers/timer.h"
+#include "rx/expresslrs_common.h"
+
+bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
+void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e expressLrsDataReceived(uint8_t *payload);
diff --git a/src/main/rx/expresslrs_common.c b/src/main/rx/expresslrs_common.c
new file mode 100644
index 0000000000..2516d4fa79
--- /dev/null
+++ b/src/main/rx/expresslrs_common.c
@@ -0,0 +1,640 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#include
+#include "platform.h"
+
+#ifdef USE_RX_EXPRESSLRS
+
+#include "build/build_config.h"
+#include "common/utils.h"
+#include "common/maths.h"
+#include "rx/crsf.h"
+#include "rx/expresslrs_common.h"
+#include "drivers/rx/rx_sx127x.h"
+#include "drivers/rx/rx_sx1280.h"
+
+STATIC_UNIT_TESTED uint16_t crc14tab[ELRS_CRC_LEN] = {0};
+
+static uint8_t volatile FHSSptr = 0;
+STATIC_UNIT_TESTED uint8_t FHSSsequence[ELRS_NR_SEQUENCE_ENTRIES] = {0};
+static const uint32_t *FHSSfreqs;
+static uint8_t numFreqs = 0; // The number of FHSS frequencies in the table
+static uint8_t seqCount = 0;
+static uint8_t syncChannel = 0;
+
+#define MS_TO_US(ms) (ms * 1000)
+
+// Regarding failsafe timeout values:
+// @CapnBry - Higher rates shorter timeout. Usually it runs 1-1.5 seconds with complete sync 500Hz.
+// 250Hz is 2-5s. 150Hz 2.5s. 50Hz stays in sync all 5 seconds of my test.
+// The failsafe timeout values come from the ELRS project's ExpressLRS_AirRateConfig definitions.
+elrsModSettings_t airRateConfig[][ELRS_RATE_MAX] = {
+#ifdef USE_RX_SX127X
+ {
+ {0, RATE_200HZ, SX127x_BW_500_00_KHZ, SX127x_SF_6, SX127x_CR_4_7, 5000, TLM_RATIO_1_64, 4, 8},
+ {1, RATE_100HZ, SX127x_BW_500_00_KHZ, SX127x_SF_7, SX127x_CR_4_7, 10000, TLM_RATIO_1_64, 4, 8},
+ {2, RATE_50HZ, SX127x_BW_500_00_KHZ, SX127x_SF_8, SX127x_CR_4_7, 20000, TLM_RATIO_NO_TLM, 4, 10},
+ {3, RATE_25HZ, SX127x_BW_500_00_KHZ, SX127x_SF_9, SX127x_CR_4_7, 40000, TLM_RATIO_NO_TLM, 2, 10}
+ },
+#endif
+#ifdef USE_RX_SX1280
+ {
+ {0, RATE_500HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF5, SX1280_LORA_CR_LI_4_6, 2000, TLM_RATIO_1_128, 4, 12},
+ {1, RATE_250HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF6, SX1280_LORA_CR_LI_4_7, 4000, TLM_RATIO_1_64, 4, 14},
+ {2, RATE_150HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF7, SX1280_LORA_CR_LI_4_7, 6666, TLM_RATIO_1_32, 4, 12},
+ {3, RATE_50HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF9, SX1280_LORA_CR_LI_4_6, 20000, TLM_RATIO_NO_TLM, 2, 12}
+ },
+#endif
+#if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
+ {{0}},
+#endif
+};
+
+elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX] = {
+#ifdef USE_RX_SX127X
+ {
+ {0, RATE_200HZ, -112, 4380, 3000, 2500, 600, 5000},
+ {1, RATE_100HZ, -117, 8770, 3500, 2500, 600, 5000},
+ {2, RATE_50HZ, -120, 17540, 4000, 2500, 600, 5000},
+ {3, RATE_25HZ, -123, 17540, 6000, 4000, 0, 5000}
+ },
+#endif
+#ifdef USE_RX_SX1280
+ {
+ {0, RATE_500HZ, -105, 1665, 2500, 2500, 3, 5000},
+ {1, RATE_250HZ, -108, 3300, 3000, 2500, 6, 5000},
+ {2, RATE_150HZ, -112, 5871, 3500, 2500, 10, 5000},
+ {3, RATE_50HZ, -117, 18443, 4000, 2500, 0, 5000}
+ },
+#endif
+#if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
+ {{0}},
+#endif
+};
+
+#ifdef USE_RX_SX127X
+const uint32_t FHSSfreqsAU433[] = {
+ FREQ_HZ_TO_REG_VAL_900(433420000),
+ FREQ_HZ_TO_REG_VAL_900(433920000),
+ FREQ_HZ_TO_REG_VAL_900(434420000)};
+
+const uint32_t FHSSfreqsAU915[] = {
+ FREQ_HZ_TO_REG_VAL_900(915500000),
+ FREQ_HZ_TO_REG_VAL_900(916100000),
+ FREQ_HZ_TO_REG_VAL_900(916700000),
+ FREQ_HZ_TO_REG_VAL_900(917300000),
+
+ FREQ_HZ_TO_REG_VAL_900(917900000),
+ FREQ_HZ_TO_REG_VAL_900(918500000),
+ FREQ_HZ_TO_REG_VAL_900(919100000),
+ FREQ_HZ_TO_REG_VAL_900(919700000),
+
+ FREQ_HZ_TO_REG_VAL_900(920300000),
+ FREQ_HZ_TO_REG_VAL_900(920900000),
+ FREQ_HZ_TO_REG_VAL_900(921500000),
+ FREQ_HZ_TO_REG_VAL_900(922100000),
+
+ FREQ_HZ_TO_REG_VAL_900(922700000),
+ FREQ_HZ_TO_REG_VAL_900(923300000),
+ FREQ_HZ_TO_REG_VAL_900(923900000),
+ FREQ_HZ_TO_REG_VAL_900(924500000),
+
+ FREQ_HZ_TO_REG_VAL_900(925100000),
+ FREQ_HZ_TO_REG_VAL_900(925700000),
+ FREQ_HZ_TO_REG_VAL_900(926300000),
+ FREQ_HZ_TO_REG_VAL_900(926900000)};
+
+/* Frequency bands taken from https://wetten.overheid.nl/BWBR0036378/2016-12-28#Bijlagen
+ * Note: these frequencies fall in the license free H-band, but in combination with 500kHz
+ * LoRa modem bandwidth used by ExpressLRS (EU allows up to 125kHz modulation BW only) they
+ * will never pass RED certification and they are ILLEGAL to use.
+ *
+ * Therefore we simply maximize the usage of available spectrum so laboratory testing of the software won't disturb existing
+ * 868MHz ISM band traffic too much.
+ */
+const uint32_t FHSSfreqsEU868[] = {
+ FREQ_HZ_TO_REG_VAL_900(863275000), // band H1, 863 - 865MHz, 0.1% duty cycle or CSMA techniques, 25mW EIRP
+ FREQ_HZ_TO_REG_VAL_900(863800000),
+ FREQ_HZ_TO_REG_VAL_900(864325000),
+ FREQ_HZ_TO_REG_VAL_900(864850000),
+ FREQ_HZ_TO_REG_VAL_900(865375000), // Band H2, 865 - 868.6MHz, 1.0% dutycycle or CSMA, 25mW EIRP
+ FREQ_HZ_TO_REG_VAL_900(865900000),
+ FREQ_HZ_TO_REG_VAL_900(866425000),
+ FREQ_HZ_TO_REG_VAL_900(866950000),
+ FREQ_HZ_TO_REG_VAL_900(867475000),
+ FREQ_HZ_TO_REG_VAL_900(868000000),
+ FREQ_HZ_TO_REG_VAL_900(868525000), // Band H3, 868.7-869.2MHz, 0.1% dutycycle or CSMA, 25mW EIRP
+ FREQ_HZ_TO_REG_VAL_900(869050000),
+ FREQ_HZ_TO_REG_VAL_900(869575000)};
+
+/**
+ * India currently delicensed the 865-867 MHz band with a maximum of 1W Transmitter power,
+ * 4Watts Effective Radiated Power and 200Khz carrier bandwidth as per
+ * https://dot.gov.in/sites/default/files/Delicensing%20in%20865-867%20MHz%20band%20%5BGSR%20564%20%28E%29%5D_0.pdf .
+ * There is currently no mention of Direct-sequence spread spectrum,
+ * So these frequencies are a subset of Regulatory_Domain_EU_868 frequencies.
+ */
+const uint32_t FHSSfreqsIN866[] = {
+ FREQ_HZ_TO_REG_VAL_900(865375000),
+ FREQ_HZ_TO_REG_VAL_900(865900000),
+ FREQ_HZ_TO_REG_VAL_900(866425000),
+ FREQ_HZ_TO_REG_VAL_900(866950000)};
+
+/* Frequency band G, taken from https://wetten.overheid.nl/BWBR0036378/2016-12-28#Bijlagen
+ * Note: As is the case with the 868Mhz band, these frequencies only comply to the license free portion
+ * of the spectrum, nothing else. As such, these are likely illegal to use.
+ */
+const uint32_t FHSSfreqsEU433[] = {
+ FREQ_HZ_TO_REG_VAL_900(433100000),
+ FREQ_HZ_TO_REG_VAL_900(433925000),
+ FREQ_HZ_TO_REG_VAL_900(434450000)};
+
+/* Very definitely not fully checked. An initial pass at increasing the hops
+*/
+const uint32_t FHSSfreqsFCC915[] = {
+ FREQ_HZ_TO_REG_VAL_900(903500000),
+ FREQ_HZ_TO_REG_VAL_900(904100000),
+ FREQ_HZ_TO_REG_VAL_900(904700000),
+ FREQ_HZ_TO_REG_VAL_900(905300000),
+
+ FREQ_HZ_TO_REG_VAL_900(905900000),
+ FREQ_HZ_TO_REG_VAL_900(906500000),
+ FREQ_HZ_TO_REG_VAL_900(907100000),
+ FREQ_HZ_TO_REG_VAL_900(907700000),
+
+ FREQ_HZ_TO_REG_VAL_900(908300000),
+ FREQ_HZ_TO_REG_VAL_900(908900000),
+ FREQ_HZ_TO_REG_VAL_900(909500000),
+ FREQ_HZ_TO_REG_VAL_900(910100000),
+
+ FREQ_HZ_TO_REG_VAL_900(910700000),
+ FREQ_HZ_TO_REG_VAL_900(911300000),
+ FREQ_HZ_TO_REG_VAL_900(911900000),
+ FREQ_HZ_TO_REG_VAL_900(912500000),
+
+ FREQ_HZ_TO_REG_VAL_900(913100000),
+ FREQ_HZ_TO_REG_VAL_900(913700000),
+ FREQ_HZ_TO_REG_VAL_900(914300000),
+ FREQ_HZ_TO_REG_VAL_900(914900000),
+
+ FREQ_HZ_TO_REG_VAL_900(915500000), // as per AU..
+ FREQ_HZ_TO_REG_VAL_900(916100000),
+ FREQ_HZ_TO_REG_VAL_900(916700000),
+ FREQ_HZ_TO_REG_VAL_900(917300000),
+
+ FREQ_HZ_TO_REG_VAL_900(917900000),
+ FREQ_HZ_TO_REG_VAL_900(918500000),
+ FREQ_HZ_TO_REG_VAL_900(919100000),
+ FREQ_HZ_TO_REG_VAL_900(919700000),
+
+ FREQ_HZ_TO_REG_VAL_900(920300000),
+ FREQ_HZ_TO_REG_VAL_900(920900000),
+ FREQ_HZ_TO_REG_VAL_900(921500000),
+ FREQ_HZ_TO_REG_VAL_900(922100000),
+
+ FREQ_HZ_TO_REG_VAL_900(922700000),
+ FREQ_HZ_TO_REG_VAL_900(923300000),
+ FREQ_HZ_TO_REG_VAL_900(923900000),
+ FREQ_HZ_TO_REG_VAL_900(924500000),
+
+ FREQ_HZ_TO_REG_VAL_900(925100000),
+ FREQ_HZ_TO_REG_VAL_900(925700000),
+ FREQ_HZ_TO_REG_VAL_900(926300000),
+ FREQ_HZ_TO_REG_VAL_900(926900000)};
+#endif
+#ifdef USE_RX_SX1280
+const uint32_t FHSSfreqsISM2400[] = {
+ FREQ_HZ_TO_REG_VAL_24(2400400000),
+ FREQ_HZ_TO_REG_VAL_24(2401400000),
+ FREQ_HZ_TO_REG_VAL_24(2402400000),
+ FREQ_HZ_TO_REG_VAL_24(2403400000),
+ FREQ_HZ_TO_REG_VAL_24(2404400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2405400000),
+ FREQ_HZ_TO_REG_VAL_24(2406400000),
+ FREQ_HZ_TO_REG_VAL_24(2407400000),
+ FREQ_HZ_TO_REG_VAL_24(2408400000),
+ FREQ_HZ_TO_REG_VAL_24(2409400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2410400000),
+ FREQ_HZ_TO_REG_VAL_24(2411400000),
+ FREQ_HZ_TO_REG_VAL_24(2412400000),
+ FREQ_HZ_TO_REG_VAL_24(2413400000),
+ FREQ_HZ_TO_REG_VAL_24(2414400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2415400000),
+ FREQ_HZ_TO_REG_VAL_24(2416400000),
+ FREQ_HZ_TO_REG_VAL_24(2417400000),
+ FREQ_HZ_TO_REG_VAL_24(2418400000),
+ FREQ_HZ_TO_REG_VAL_24(2419400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2420400000),
+ FREQ_HZ_TO_REG_VAL_24(2421400000),
+ FREQ_HZ_TO_REG_VAL_24(2422400000),
+ FREQ_HZ_TO_REG_VAL_24(2423400000),
+ FREQ_HZ_TO_REG_VAL_24(2424400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2425400000),
+ FREQ_HZ_TO_REG_VAL_24(2426400000),
+ FREQ_HZ_TO_REG_VAL_24(2427400000),
+ FREQ_HZ_TO_REG_VAL_24(2428400000),
+ FREQ_HZ_TO_REG_VAL_24(2429400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2430400000),
+ FREQ_HZ_TO_REG_VAL_24(2431400000),
+ FREQ_HZ_TO_REG_VAL_24(2432400000),
+ FREQ_HZ_TO_REG_VAL_24(2433400000),
+ FREQ_HZ_TO_REG_VAL_24(2434400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2435400000),
+ FREQ_HZ_TO_REG_VAL_24(2436400000),
+ FREQ_HZ_TO_REG_VAL_24(2437400000),
+ FREQ_HZ_TO_REG_VAL_24(2438400000),
+ FREQ_HZ_TO_REG_VAL_24(2439400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2440400000),
+ FREQ_HZ_TO_REG_VAL_24(2441400000),
+ FREQ_HZ_TO_REG_VAL_24(2442400000),
+ FREQ_HZ_TO_REG_VAL_24(2443400000),
+ FREQ_HZ_TO_REG_VAL_24(2444400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2445400000),
+ FREQ_HZ_TO_REG_VAL_24(2446400000),
+ FREQ_HZ_TO_REG_VAL_24(2447400000),
+ FREQ_HZ_TO_REG_VAL_24(2448400000),
+ FREQ_HZ_TO_REG_VAL_24(2449400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2450400000),
+ FREQ_HZ_TO_REG_VAL_24(2451400000),
+ FREQ_HZ_TO_REG_VAL_24(2452400000),
+ FREQ_HZ_TO_REG_VAL_24(2453400000),
+ FREQ_HZ_TO_REG_VAL_24(2454400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2455400000),
+ FREQ_HZ_TO_REG_VAL_24(2456400000),
+ FREQ_HZ_TO_REG_VAL_24(2457400000),
+ FREQ_HZ_TO_REG_VAL_24(2458400000),
+ FREQ_HZ_TO_REG_VAL_24(2459400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2460400000),
+ FREQ_HZ_TO_REG_VAL_24(2461400000),
+ FREQ_HZ_TO_REG_VAL_24(2462400000),
+ FREQ_HZ_TO_REG_VAL_24(2463400000),
+ FREQ_HZ_TO_REG_VAL_24(2464400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2465400000),
+ FREQ_HZ_TO_REG_VAL_24(2466400000),
+ FREQ_HZ_TO_REG_VAL_24(2467400000),
+ FREQ_HZ_TO_REG_VAL_24(2468400000),
+ FREQ_HZ_TO_REG_VAL_24(2469400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2470400000),
+ FREQ_HZ_TO_REG_VAL_24(2471400000),
+ FREQ_HZ_TO_REG_VAL_24(2472400000),
+ FREQ_HZ_TO_REG_VAL_24(2473400000),
+ FREQ_HZ_TO_REG_VAL_24(2474400000),
+
+ FREQ_HZ_TO_REG_VAL_24(2475400000),
+ FREQ_HZ_TO_REG_VAL_24(2476400000),
+ FREQ_HZ_TO_REG_VAL_24(2477400000),
+ FREQ_HZ_TO_REG_VAL_24(2478400000),
+ FREQ_HZ_TO_REG_VAL_24(2479400000)};
+#endif
+
+void generateCrc14Table(void)
+{
+ uint16_t crc;
+ for (uint16_t i = 0; i < ELRS_CRC_LEN; i++) {
+ crc = i << (14 - 8);
+ for (uint8_t j = 0; j < 8; j++) {
+ crc = (crc << 1) ^ ((crc & 0x2000) ? ELRS_CRC14_POLY : 0);
+ }
+ crc14tab[i] = crc;
+ }
+}
+
+uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc)
+{
+ while (len--) {
+ crc = (crc << 8) ^ crc14tab[((crc >> 6) ^ (uint16_t) *data++) & 0x00FF];
+ }
+ return crc & 0x3FFF;
+}
+
+static void initializeFHSSFrequencies(const elrsFreqDomain_e dom) {
+ switch (dom) {
+#ifdef USE_RX_SX127X
+ case AU433:
+ FHSSfreqs = FHSSfreqsAU433;
+ numFreqs = sizeof(FHSSfreqsAU433) / sizeof(uint32_t);
+ break;
+ case AU915:
+ FHSSfreqs = FHSSfreqsAU915;
+ numFreqs = sizeof(FHSSfreqsAU915) / sizeof(uint32_t);
+ break;
+ case EU433:
+ FHSSfreqs = FHSSfreqsEU433;
+ numFreqs = sizeof(FHSSfreqsEU433) / sizeof(uint32_t);
+ break;
+ case EU868:
+ FHSSfreqs = FHSSfreqsEU868;
+ numFreqs = sizeof(FHSSfreqsEU868) / sizeof(uint32_t);
+ break;
+ case IN866:
+ FHSSfreqs = FHSSfreqsIN866;
+ numFreqs = sizeof(FHSSfreqsIN866) / sizeof(uint32_t);
+ break;
+ case FCC915:
+ FHSSfreqs = FHSSfreqsFCC915;
+ numFreqs = sizeof(FHSSfreqsFCC915) / sizeof(uint32_t);
+ break;
+#endif
+#ifdef USE_RX_SX1280
+ case ISM2400:
+ FHSSfreqs = FHSSfreqsISM2400;
+ numFreqs = sizeof(FHSSfreqsISM2400) / sizeof(uint32_t);
+ break;
+#endif
+ default:
+ FHSSfreqs = NULL;
+ numFreqs = 0;
+ }
+}
+
+uint32_t getInitialFreq(const int32_t freqCorrection)
+{
+ return FHSSfreqs[syncChannel] - freqCorrection;
+}
+
+uint8_t getFHSSNumEntries(void)
+{
+ return numFreqs;
+}
+
+uint8_t FHSSgetCurrIndex(void)
+{
+ return FHSSptr;
+}
+
+void FHSSsetCurrIndex(const uint8_t value)
+{
+ FHSSptr = value % seqCount;
+}
+
+uint32_t FHSSgetNextFreq(const int32_t freqCorrection)
+{
+ FHSSptr = (FHSSptr + 1) % seqCount;
+ return FHSSfreqs[FHSSsequence[FHSSptr]] - freqCorrection;
+}
+
+static uint32_t seed = 0;
+
+// returns 0 <= x < max where max < 256
+static uint8_t rngN(const uint8_t max)
+{
+ const uint32_t m = 2147483648;
+ const uint32_t a = 214013;
+ const uint32_t c = 2531011;
+ seed = (a * seed + c) % m;
+ return (seed >> 16) % max;
+}
+
+/**
+Requirements:
+1. 0 every n hops
+2. No two repeated channels
+3. Equal occurance of each (or as even as possible) of each channel
+4. Pseudorandom
+
+Approach:
+ Fill the sequence array with the sync channel every FHSS_FREQ_CNT
+ Iterate through the array, and for each block, swap each entry in it with
+ another random entry, excluding the sync channel.
+
+*/
+void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom)
+{
+ seed = ((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5];
+
+ initializeFHSSFrequencies(dom);
+
+ seqCount = (256 / MAX(numFreqs, 1)) * numFreqs;
+
+ syncChannel = numFreqs / 2;
+
+ // initialize the sequence array
+ for (uint8_t i = 0; i < seqCount; i++) {
+ if (i % numFreqs == 0) {
+ FHSSsequence[i] = syncChannel;
+ } else if (i % numFreqs == syncChannel) {
+ FHSSsequence[i] = 0;
+ } else {
+ FHSSsequence[i] = i % numFreqs;
+ }
+ }
+
+ for (uint8_t i = 0; i < seqCount; i++) {
+ // if it's not the sync channel
+ if (i % numFreqs != 0) {
+ uint8_t offset = (i / numFreqs) * numFreqs; // offset to start of current block
+ uint8_t rand = rngN(numFreqs - 1) + 1; // random number between 1 and numFreqs
+
+ // switch this entry and another random entry in the same block
+ uint8_t temp = FHSSsequence[i];
+ FHSSsequence[i] = FHSSsequence[offset + rand];
+ FHSSsequence[offset + rand] = temp;
+ }
+ }
+}
+
+uint8_t tlmRatioEnumToValue(const elrsTlmRatio_e enumval)
+{
+ switch (enumval) {
+ case TLM_RATIO_NO_TLM:
+ return 1;
+ break;
+ case TLM_RATIO_1_2:
+ return 2;
+ break;
+ case TLM_RATIO_1_4:
+ return 4;
+ break;
+ case TLM_RATIO_1_8:
+ return 8;
+ break;
+ case TLM_RATIO_1_16:
+ return 16;
+ break;
+ case TLM_RATIO_1_32:
+ return 32;
+ break;
+ case TLM_RATIO_1_64:
+ return 64;
+ break;
+ case TLM_RATIO_1_128:
+ return 128;
+ break;
+ default:
+ return 0;
+ }
+}
+
+uint16_t rateEnumToHz(const elrsRfRate_e eRate)
+{
+ switch (eRate) {
+ case RATE_500HZ: return 500;
+ case RATE_250HZ: return 250;
+ case RATE_200HZ: return 200;
+ case RATE_150HZ: return 150;
+ case RATE_100HZ: return 100;
+ case RATE_50HZ: return 50;
+ case RATE_25HZ: return 25;
+ case RATE_4HZ: return 4;
+ default: return 1;
+ }
+}
+
+uint16_t txPowerIndexToValue(const uint8_t index)
+{
+ switch (index) {
+ case 0: return 0;
+ case 1: return 10;
+ case 2: return 25;
+ case 3: return 100;
+ case 4: return 500;
+ case 5: return 1000;
+ case 6: return 2000;
+ case 7: return 250;
+ case 8: return 50;
+ default: return 0;
+ }
+}
+
+#define ELRS_LQ_DEPTH 4 //100 % 32
+
+typedef struct linkQuality_s {
+ uint32_t array[ELRS_LQ_DEPTH];
+ uint8_t value;
+ uint8_t index;
+ uint32_t mask;
+} linkQuality_t;
+
+static linkQuality_t lq;
+
+void lqIncrease(void)
+{
+ if (lqPeriodIsSet()) {
+ return;
+ }
+ lq.array[lq.index] |= lq.mask;
+ lq.value += 1;
+}
+
+void lqNewPeriod(void)
+{
+ lq.mask <<= 1;
+ if (lq.mask == 0) {
+ lq.mask = (1 << 0);
+ lq.index += 1;
+ }
+
+ // At idx N / 32 and bit N % 32, wrap back to idx=0, bit=0
+ if ((lq.index == 3) && (lq.mask & (1 << ELRS_LQ_DEPTH))) {
+ lq.index = 0;
+ lq.mask = (1 << 0);
+ }
+
+ if ((lq.array[lq.index] & lq.mask) != 0) {
+ lq.array[lq.index] &= ~lq.mask;
+ lq.value -= 1;
+ }
+}
+
+uint8_t lqGet(void)
+{
+ return lq.value;
+}
+
+bool lqPeriodIsSet(void)
+{
+ return lq.array[lq.index] & lq.mask;
+}
+
+void lqReset(void)
+{
+ memset(&lq, 0, sizeof(lq));
+ lq.mask = (1 << 0);
+}
+
+uint16_t convertSwitch1b(const uint16_t val)
+{
+ return val ? 2000 : 1000;
+}
+
+// 3b to decode 7 pos switches
+uint16_t convertSwitch3b(const uint16_t val)
+{
+ switch (val) {
+ case 0:
+ return 1000;
+ case 1:
+ return 1275;
+ case 2:
+ return 1425;
+ case 3:
+ return 1575;
+ case 4:
+ return 1725;
+ case 5:
+ return 2000;
+ default:
+ return 1500;
+ }
+}
+
+uint16_t convertSwitchNb(const uint16_t val, const uint16_t max)
+{
+ return (val > max) ? 1500 : val * 1000 / max + 1000;
+}
+
+uint16_t convertAnalog(const uint16_t val)
+{
+ return CRSF_RC_CHANNEL_SCALE_LEGACY * val + 881;
+}
+
+uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce)
+{
+ // Returns the sequence (0 to 7, then 0 to 7 rotated left by 1):
+ // 0, 1, 2, 3, 4, 5, 6, 7,
+ // 1, 2, 3, 4, 5, 6, 7, 0
+ // Because telemetry can occur on every 2, 4, 8, 16, 32, 64, 128th packet
+ // this makes sure each of the 8 values is sent at least once every 16 packets
+ // regardless of the TLM ratio
+ // Index 7 also can never fall on a telemetry slot
+ return ((nonce & 0x07) + ((nonce >> 3) & 0x01)) % 8;
+}
+
+#endif /* USE_RX_EXPRESSLRS */
diff --git a/src/main/rx/expresslrs_common.h b/src/main/rx/expresslrs_common.h
new file mode 100644
index 0000000000..b31bdaadfc
--- /dev/null
+++ b/src/main/rx/expresslrs_common.h
@@ -0,0 +1,172 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#pragma once
+
+#include
+#include
+
+#include "drivers/io_types.h"
+#include "drivers/time.h"
+
+#define ELRS_CRC_LEN 256
+#define ELRS_CRC14_POLY 0x2E57
+
+#define ELRS_NR_SEQUENCE_ENTRIES 256
+
+#define ELRS_RX_TX_BUFF_SIZE 8
+
+#define ELRS_TELEMETRY_TYPE_LINK 0x01
+#define ELRS_TELEMETRY_TYPE_DATA 0x02
+#define ELRS_MSP_BIND 0x09
+#define ELRS_MSP_MODEL_ID 0x0A
+#define ELRS_MSP_SET_RX_CONFIG 45
+
+#define ELRS_MODELMATCH_MASK 0x3F
+
+#define FREQ_HZ_TO_REG_VAL_900(freq) ((uint32_t)(freq / SX127x_FREQ_STEP))
+#define FREQ_HZ_TO_REG_VAL_24(freq) ((uint32_t)(freq / SX1280_FREQ_STEP))
+
+#define ELRS_RATE_MAX 4
+#define ELRS_BINDING_RATE_24 3
+#define ELRS_BINDING_RATE_900 2
+
+#define ELRS_MAX_CHANNELS 16
+#define ELRS_RSSI_CHANNEL 15
+#define ELRS_LQ_CHANNEL 14
+
+#define ELRS_CONFIG_CHECK_MS 200
+#define ELRS_LINK_STATS_CHECK_MS 100
+#define ELRS_CONSIDER_CONNECTION_GOOD_MS 1000
+
+#define ELRS_MODE_CYCLE_MULTIPLIER_SLOW 10
+
+typedef enum {
+#ifdef USE_RX_SX127X
+ AU433,
+ AU915,
+ EU433,
+ EU868,
+ IN866,
+ FCC915,
+#endif
+#ifdef USE_RX_SX1280
+ ISM2400,
+#endif
+#if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
+ NONE,
+#endif
+} elrsFreqDomain_e;
+
+typedef enum {
+ SM_HYBRID = 0,
+ SM_HYBRID_WIDE = 1
+} elrsSwitchMode_e;
+
+typedef enum {
+ TLM_RATIO_NO_TLM = 0,
+ TLM_RATIO_1_128 = 1,
+ TLM_RATIO_1_64 = 2,
+ TLM_RATIO_1_32 = 3,
+ TLM_RATIO_1_16 = 4,
+ TLM_RATIO_1_8 = 5,
+ TLM_RATIO_1_4 = 6,
+ TLM_RATIO_1_2 = 7,
+} elrsTlmRatio_e;
+
+typedef enum {
+ RATE_500HZ = 0,
+ RATE_250HZ = 1,
+ RATE_200HZ = 2,
+ RATE_150HZ = 3,
+ RATE_100HZ = 4,
+ RATE_50HZ = 5,
+ RATE_25HZ = 6,
+ RATE_4HZ = 7,
+} elrsRfRate_e; // Max value of 16 since only 4 bits have been assigned in the sync package.
+
+typedef struct elrsModSettings_s {
+ uint8_t index;
+ elrsRfRate_e enumRate; // Max value of 16 since only 4 bits have been assigned in the sync package.
+ uint8_t bw;
+ uint8_t sf;
+ uint8_t cr;
+ uint32_t interval; // interval in us seconds that corresponds to that frequency
+ elrsTlmRatio_e tlmInterval; // every X packets is a response TLM packet, should be a power of 2
+ uint8_t fhssHopInterval; // every X packets we hop to a new frequency. Max value of 16 since only 4 bits have been assigned in the sync package.
+ uint8_t preambleLen;
+} elrsModSettings_t;
+
+typedef struct elrsRfPerfParams_s {
+ int8_t index;
+ elrsRfRate_e enumRate; // Max value of 16 since only 4 bits have been assigned in the sync package.
+ int32_t sensitivity; // expected RF sensitivity based on
+ uint32_t toa; // time on air in microseconds
+ uint32_t disconnectTimeoutMs; // Time without a packet before receiver goes to disconnected (ms)
+ uint32_t rxLockTimeoutMs; // Max time to go from tentative -> connected state on receiver (ms)
+ uint32_t syncPktIntervalDisconnected; // how often to send the SYNC_PACKET packet (ms) when there is no response from RX
+ uint32_t syncPktIntervalConnected; // how often to send the SYNC_PACKET packet (ms) when there we have a connection
+} elrsRfPerfParams_t;
+
+typedef bool (*elrsRxInitFnPtr)(IO_t resetPin, IO_t busyPin);
+typedef void (*elrsRxConfigFnPtr)(const uint8_t bw, const uint8_t sf, const uint8_t cr, const uint32_t freq, const uint8_t preambleLen, const bool iqInverted);
+typedef void (*elrsRxStartReceivingFnPtr)(void);
+typedef uint8_t (*elrsRxISRFnPtr)(timeUs_t *timeStamp);
+typedef void (*elrsRxTransmitDataFnPtr)(const uint8_t *data, const uint8_t length);
+typedef void (*elrsRxReceiveDataFnPtr)(uint8_t *data, const uint8_t length);
+typedef void (*elrsRxGetRFlinkInfoFnPtr)(int8_t *rssi, int8_t *snr);
+typedef void (*elrsRxSetFrequencyFnPtr)(const uint32_t freq);
+typedef void (*elrsRxHandleFreqCorrectionFnPtr)(int32_t offset, const uint32_t freq);
+
+extern elrsModSettings_t airRateConfig[][ELRS_RATE_MAX];
+extern elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX];
+
+void generateCrc14Table(void);
+uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc);
+
+uint32_t getInitialFreq(const int32_t freqCorrection);
+uint8_t getFHSSNumEntries(void);
+uint8_t FHSSgetCurrIndex(void);
+void FHSSsetCurrIndex(const uint8_t value);
+uint32_t FHSSgetNextFreq(const int32_t freqCorrection);
+void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom);
+uint8_t tlmRatioEnumToValue(const elrsTlmRatio_e enumval);
+uint16_t rateEnumToHz(const elrsRfRate_e eRate);
+uint16_t txPowerIndexToValue(const uint8_t index);
+
+//
+// Link Quality
+//
+void lqReset(void);
+void lqNewPeriod(void);
+bool lqPeriodIsSet(void);
+void lqIncrease(void);
+uint8_t lqGet(void);
+
+uint16_t convertSwitch1b(const uint16_t val);
+uint16_t convertSwitch3b(const uint16_t val);
+uint16_t convertSwitchNb(const uint16_t val, const uint16_t max);
+uint16_t convertAnalog(const uint16_t val);
+uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce);
diff --git a/src/main/rx/expresslrs_impl.h b/src/main/rx/expresslrs_impl.h
new file mode 100644
index 0000000000..eb480c4a60
--- /dev/null
+++ b/src/main/rx/expresslrs_impl.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+// expresslrs packet header types
+// 00 -> standard 4 channel data packet
+// 01 -> switch data packet
+// 11 -> tlm packet
+// 10 -> sync packet with hop data
+typedef enum {
+ ELRS_RC_DATA_PACKET = 0x00,
+ ELRS_MSP_DATA_PACKET = 0x01,
+ ELRS_SYNC_PACKET = 0x02,
+ ELRS_TLM_PACKET = 0x03,
+} elrsPacketType_e;
+
+typedef enum {
+ ELRS_DIO_UNKNOWN = 0,
+ ELRS_DIO_RX_DONE = 1,
+ ELRS_DIO_TX_DONE = 2
+} dioReason_e;
+
+typedef enum {
+ ELRS_CONNECTED,
+ ELRS_TENTATIVE,
+ ELRS_DISCONNECTED,
+ ELRS_DISCONNECT_PENDING // used on modelmatch change to drop the connection
+} connectionState_e;
+
+typedef enum {
+ ELRS_TIM_DISCONNECTED = 0,
+ ELRS_TIM_TENTATIVE = 1,
+ ELRS_TIM_LOCKED = 2
+} timerState_e;
+
+typedef struct elrsReceiver_s {
+
+ IO_t resetPin;
+ IO_t busyPin;
+
+ int32_t freqOffset;
+ uint32_t currentFreq;
+
+ volatile uint8_t nonceRX; // nonce that we THINK we are up to.
+
+ elrsModSettings_t *modParams;
+ elrsRfPerfParams_t *rfPerfParams;
+
+ const uint8_t *UID;
+
+ int8_t rssi;
+ int8_t snr;
+ int8_t rssiFiltered;
+
+ uint8_t uplinkLQ;
+
+ bool alreadyFHSS;
+ bool alreadyTLMresp;
+ bool lockRFmode;
+
+ timerState_e timerState;
+ connectionState_e connectionState;
+
+ uint8_t rfModeCycleMultiplier;
+ uint16_t cycleIntervalMs;
+ uint32_t rfModeCycledAtMs;
+ uint8_t rateIndex;
+ uint8_t nextRateIndex;
+
+ uint32_t gotConnectionMs;
+ uint32_t lastSyncPacketMs;
+ uint32_t lastValidPacketMs;
+
+ uint32_t configCheckedAtMs;
+ bool configChanged;
+
+ bool inBindingMode;
+ volatile bool fhssRequired;
+
+ uint32_t statsUpdatedAtMs;
+
+ elrsRxInitFnPtr init;
+ elrsRxConfigFnPtr config;
+ elrsRxStartReceivingFnPtr startReceiving;
+ elrsRxISRFnPtr rxISR;
+ elrsRxTransmitDataFnPtr transmitData;
+ elrsRxReceiveDataFnPtr receiveData;
+ elrsRxGetRFlinkInfoFnPtr getRFlinkInfo;
+ elrsRxSetFrequencyFnPtr setFrequency;
+ elrsRxHandleFreqCorrectionFnPtr handleFreqCorrection;
+
+ timerOvrHandlerRec_t timerUpdateCb;
+} elrsReceiver_t;
+
diff --git a/src/main/rx/expresslrs_telemetry.c b/src/main/rx/expresslrs_telemetry.c
new file mode 100644
index 0000000000..f1a40eb8c3
--- /dev/null
+++ b/src/main/rx/expresslrs_telemetry.c
@@ -0,0 +1,377 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ *
+ * Authors:
+ * Phobos- - Original port.
+ */
+
+#include
+#include "platform.h"
+
+#ifdef USE_RX_EXPRESSLRS
+
+#include "config/feature.h"
+#include "fc/runtime_config.h"
+
+#include "msp/msp_protocol.h"
+
+#include "rx/crsf_protocol.h"
+#include "rx/expresslrs_telemetry.h"
+
+#include "telemetry/crsf.h"
+#include "telemetry/telemetry.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+static uint8_t tlmBuffer[CRSF_FRAME_SIZE_MAX];
+
+typedef enum {
+ CRSF_FRAME_GPS_INDEX = 0,
+ CRSF_FRAME_BATTERY_SENSOR_INDEX,
+ CRSF_FRAME_ATTITUDE_INDEX,
+ CRSF_FRAME_FLIGHT_MODE_INDEX,
+ CRSF_FRAME_PAYLOAD_TYPES_COUNT //should be last
+} frameTypeIndex_e;
+
+static crsfFrameType_e payloadTypes[] = {
+ CRSF_FRAMETYPE_GPS,
+ CRSF_FRAMETYPE_BATTERY_SENSOR,
+ CRSF_FRAMETYPE_ATTITUDE,
+ CRSF_FRAMETYPE_FLIGHT_MODE
+};
+
+STATIC_UNIT_TESTED uint8_t tlmSensors = 0;
+STATIC_UNIT_TESTED uint8_t currentPayloadIndex;
+
+static uint8_t *data;
+static uint8_t length;
+static uint8_t bytesPerCall;
+static uint8_t currentOffset;
+static uint8_t currentPackage;
+static bool waitUntilTelemetryConfirm;
+static uint16_t waitCount;
+static uint16_t maxWaitCount;
+static volatile stubbornSenderState_e senderState;
+
+static void telemetrySenderResetState(void)
+{
+ data = 0;
+ currentOffset = 0;
+ currentPackage = 0;
+ length = 0;
+ waitUntilTelemetryConfirm = true;
+ waitCount = 0;
+ // 80 corresponds to UpdateTelemetryRate(ANY, 2, 1), which is what the TX uses in boost mode
+ maxWaitCount = 80;
+ senderState = ELRS_SENDER_IDLE;
+ bytesPerCall = 1;
+}
+
+/***
+ * Queues a message to send, will abort the current message if one is currently being transmitted
+ ***/
+void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc)
+{
+ if (lengthToTransmit / bpc >= ELRS_TELEMETRY_MAX_PACKAGES) {
+ return;
+ }
+
+ length = lengthToTransmit;
+ data = dataToTransmit;
+ currentOffset = 0;
+ currentPackage = 1;
+ waitCount = 0;
+ bytesPerCall = bpc;
+ senderState = (senderState == ELRS_SENDER_IDLE) ? ELRS_SENDING : ELRS_RESYNC_THEN_SEND;
+}
+
+bool isTelemetrySenderActive(void)
+{
+ return senderState != ELRS_SENDER_IDLE;
+}
+
+void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData)
+{
+ switch (senderState) {
+ case ELRS_RESYNC:
+ case ELRS_RESYNC_THEN_SEND:
+ *packageIndex = ELRS_TELEMETRY_MAX_PACKAGES;
+ *count = 0;
+ *currentData = 0;
+ break;
+ case ELRS_SENDING:
+ *currentData = data + currentOffset;
+ *packageIndex = currentPackage;
+ if (bytesPerCall > 1) {
+ if (currentOffset + bytesPerCall <= length) {
+ *count = bytesPerCall;
+ } else {
+ *count = length - currentOffset;
+ }
+ } else {
+ *count = 1;
+ }
+ break;
+ default:
+ *count = 0;
+ *currentData = 0;
+ *packageIndex = 0;
+ }
+}
+
+void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
+{
+ stubbornSenderState_e nextSenderState = senderState;
+
+ switch (senderState) {
+ case ELRS_SENDING:
+ if (telemetryConfirmValue != waitUntilTelemetryConfirm) {
+ waitCount++;
+ if (waitCount > maxWaitCount) {
+ waitUntilTelemetryConfirm = !telemetryConfirmValue;
+ nextSenderState = ELRS_RESYNC;
+ }
+ break;
+ }
+
+ currentOffset += bytesPerCall;
+ currentPackage++;
+ waitUntilTelemetryConfirm = !waitUntilTelemetryConfirm;
+ waitCount = 0;
+
+ if (currentOffset >= length) {
+ nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM;
+ }
+
+ break;
+
+ case ELRS_RESYNC:
+ case ELRS_RESYNC_THEN_SEND:
+ case ELRS_WAIT_UNTIL_NEXT_CONFIRM:
+ if (telemetryConfirmValue == waitUntilTelemetryConfirm) {
+ nextSenderState = (senderState == ELRS_RESYNC_THEN_SEND) ? ELRS_SENDING : ELRS_SENDER_IDLE;
+ waitUntilTelemetryConfirm = !telemetryConfirmValue;
+ } else if (senderState == ELRS_WAIT_UNTIL_NEXT_CONFIRM) { // switch to resync if tx does not confirm value fast enough
+ waitCount++;
+ if (waitCount > maxWaitCount) {
+ waitUntilTelemetryConfirm = !telemetryConfirmValue;
+ nextSenderState = ELRS_RESYNC;
+ }
+ }
+
+ break;
+
+ case ELRS_SENDER_IDLE:
+ break;
+ }
+
+ senderState = nextSenderState;
+}
+
+#ifdef USE_MSP_OVER_TELEMETRY
+static uint8_t *mspData;
+static volatile bool finishedData;
+static volatile uint8_t mspLength;
+static volatile uint8_t mspBytesPerCall;
+static volatile uint8_t mspCurrentOffset;
+static volatile uint8_t mspCurrentPackage;
+static volatile bool mspConfirm;
+
+STATIC_UNIT_TESTED volatile bool mspReplyPending;
+STATIC_UNIT_TESTED volatile bool deviceInfoReplyPending;
+
+void mspReceiverResetState(void) {
+ mspData = 0;
+ mspBytesPerCall = 1;
+ mspCurrentOffset = 0;
+ mspCurrentPackage = 0;
+ mspLength = 0;
+ mspConfirm = false;
+ mspReplyPending = false;
+ deviceInfoReplyPending = false;
+}
+
+bool getCurrentMspConfirm(void)
+{
+ return mspConfirm;
+}
+
+void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc)
+{
+ mspLength = maxLength;
+ mspData = dataToReceive;
+ mspCurrentPackage = 1;
+ mspCurrentOffset = 0;
+ finishedData = false;
+ mspBytesPerCall = bpc;
+}
+
+void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData)
+{
+ if (packageIndex == 0 && mspCurrentPackage > 1) {
+ finishedData = true;
+ mspConfirm = !mspConfirm;
+ return;
+ }
+
+ if (packageIndex == ELRS_MSP_MAX_PACKAGES) {
+ mspConfirm = !mspConfirm;
+ mspCurrentPackage = 1;
+ mspCurrentOffset = 0;
+ finishedData = false;
+ return;
+ }
+
+ if (finishedData) {
+ return;
+ }
+
+ if (packageIndex == mspCurrentPackage) {
+ for (uint8_t i = 0; i < mspBytesPerCall; i++) {
+ mspData[mspCurrentOffset++] = *(receiveData + i);
+ }
+
+ mspCurrentPackage++;
+ mspConfirm = !mspConfirm;
+ return;
+ }
+
+ return;
+}
+
+bool hasFinishedMspData(void)
+{
+ return finishedData;
+}
+
+void mspReceiverUnlock(void)
+{
+ if (finishedData) {
+ mspCurrentPackage = 1;
+ mspCurrentOffset = 0;
+ finishedData = false;
+ }
+}
+
+static uint8_t mspFrameSize = 0;
+
+static void bufferMspResponse(uint8_t *payload, const uint8_t payloadSize)
+{
+ mspFrameSize = getCrsfMspFrame(tlmBuffer, payload, payloadSize);
+}
+
+void processMspPacket(uint8_t *packet)
+{
+ switch (packet[2]) {
+ case CRSF_FRAMETYPE_DEVICE_PING:
+ deviceInfoReplyPending = true;
+ break;
+ case CRSF_FRAMETYPE_MSP_REQ:
+ FALLTHROUGH;
+ case CRSF_FRAMETYPE_MSP_WRITE: //TODO: MSP_EEPROM_WRITE command is disabled.
+ if (packet[ELRS_MSP_COMMAND_INDEX] != MSP_EEPROM_WRITE && bufferCrsfMspFrame(&packet[ELRS_MSP_PACKET_OFFSET], CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
+ handleCrsfMspFrameBuffer(&bufferMspResponse);
+ mspReplyPending = true;
+ }
+ break;
+ default:
+ break;
+ }
+}
+#endif
+
+/*
+ * Called when the telemetry ratio or air rate changes, calculate
+ * the new threshold for how many times the telemetryConfirmValue
+ * can be wrong in a row before giving up and going to RESYNC
+ */
+void updateTelemetryRate(const uint16_t airRate, const uint8_t tlmRatio, const uint8_t tlmBurst)
+{
+ // consipicuously unused airRate parameter, the wait count is strictly based on number
+ // of packets, not time between the telemetry packets, or a wall clock timeout
+ UNUSED(airRate);
+ // The expected number of packet periods between telemetry packets
+ uint32_t packsBetween = tlmRatio * (1 + tlmBurst) / tlmBurst;
+ maxWaitCount = packsBetween * ELRS_TELEMETRY_MAX_MISSED_PACKETS;
+}
+
+void initTelemetry(void)
+{
+ if (!featureIsEnabled(FEATURE_TELEMETRY)) {
+ return;
+ }
+
+ if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
+ tlmSensors |= BIT(CRSF_FRAME_ATTITUDE_INDEX);
+ }
+ if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
+ || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
+ tlmSensors |= BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
+ }
+ if (telemetryIsSensorEnabled(SENSOR_MODE)) {
+ tlmSensors |= BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
+ }
+#ifdef USE_GPS
+ if (featureIsEnabled(FEATURE_GPS)
+ && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
+ tlmSensors |= BIT(CRSF_FRAME_GPS_INDEX);
+ }
+#endif
+
+ telemetrySenderResetState();
+#ifdef USE_MSP_OVER_TELEMETRY
+ mspReceiverResetState();
+#endif
+}
+
+bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData)
+{
+#ifdef USE_MSP_OVER_TELEMETRY
+ if (deviceInfoReplyPending) {
+ *nextPayloadSize = getCrsfFrame(tlmBuffer, CRSF_FRAMETYPE_DEVICE_INFO);
+ *payloadData = tlmBuffer;
+ deviceInfoReplyPending = false;
+ return true;
+ } else if (mspReplyPending) {
+ *nextPayloadSize = mspFrameSize;
+ *payloadData = tlmBuffer;
+ mspReplyPending = false;
+ return true;
+ } else
+#endif
+ if (tlmSensors & BIT(currentPayloadIndex)) {
+ *nextPayloadSize = getCrsfFrame(tlmBuffer, payloadTypes[currentPayloadIndex]);
+ *payloadData = tlmBuffer;
+ currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
+ return true;
+ } else {
+ currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
+ *nextPayloadSize = 0;
+ *payloadData = 0;
+ return false;
+ }
+}
+
+#endif
diff --git a/src/main/rx/expresslrs_telemetry.h b/src/main/rx/expresslrs_telemetry.h
new file mode 100644
index 0000000000..8027d2981c
--- /dev/null
+++ b/src/main/rx/expresslrs_telemetry.h
@@ -0,0 +1,60 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+#define ELRS_TELEMETRY_SHIFT 2
+#define ELRS_TELEMETRY_BYTES_PER_CALL 5
+#define ELRS_TELEMETRY_MAX_PACKAGES (255 >> ELRS_TELEMETRY_SHIFT)
+#define ELRS_TELEMETRY_MAX_MISSED_PACKETS 20
+
+#define ELRS_MSP_BYTES_PER_CALL 5
+#define ELRS_MSP_BUFFER_SIZE 65
+#define ELRS_MSP_MAX_PACKAGES ((ELRS_MSP_BUFFER_SIZE / ELRS_MSP_BYTES_PER_CALL) + 1)
+#define ELRS_MSP_PACKET_OFFSET 5
+#define ELRS_MSP_COMMAND_INDEX 7
+
+typedef enum {
+ ELRS_SENDER_IDLE = 0,
+ ELRS_SENDING,
+ ELRS_WAIT_UNTIL_NEXT_CONFIRM,
+ ELRS_RESYNC,
+ ELRS_RESYNC_THEN_SEND, // perform a RESYNC then go to SENDING
+} stubbornSenderState_e;
+
+void initTelemetry(void);
+bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData);
+
+void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc);
+bool isTelemetrySenderActive(void);
+void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData);
+void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue);
+void updateTelemetryRate(const uint16_t airRate, const uint8_t tlmRatio, const uint8_t tlmBurst);
+
+void mspReceiverResetState(void);
+bool getCurrentMspConfirm(void);
+void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc);
+void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData);
+bool hasFinishedMspData(void);
+void mspReceiverUnlock(void);
+void processMspPacket(uint8_t *packet);
diff --git a/src/main/rx/rx_bind.c b/src/main/rx/rx_bind.c
index c0729b22ed..a1fa029d56 100644
--- a/src/main/rx/rx_bind.c
+++ b/src/main/rx/rx_bind.c
@@ -76,7 +76,10 @@ static bool doRxBind(bool doBind)
#ifdef USE_RX_SPEKTRUM
case RX_SPI_CYRF6936_DSM:
#endif
-#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM)
+#ifdef USE_RX_EXPRESSLRS
+ case RX_SPI_EXPRESSLRS:
+#endif
+#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM) || defined(USE_RX_EXPRESSLRS)
if (doBind) {
rxSpiBind();
}
diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c
index c72def4940..c6cab6fc64 100644
--- a/src/main/rx/rx_spi.c
+++ b/src/main/rx/rx_spi.c
@@ -51,7 +51,7 @@
#include "rx/a7105_flysky.h"
#include "rx/cc2500_sfhss.h"
#include "rx/cyrf6936_spektrum.h"
-
+#include "rx/expresslrs.h"
uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
@@ -174,6 +174,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
protocolDataReceived = spektrumSpiDataReceived;
protocolSetRcDataFromPayload = spektrumSpiSetRcDataFromPayload;
break;
+#endif
+#ifdef USE_RX_EXPRESSLRS
+ case RX_SPI_EXPRESSLRS:
+ protocolInit = expressLrsSpiInit;
+ protocolDataReceived = expressLrsDataReceived;
+ protocolSetRcDataFromPayload = expressLrsSetRcDataFromPayload;
+ break;
#endif
default:
return false;
diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h
index 1db7902158..b19f2b20a4 100644
--- a/src/main/rx/rx_spi.h
+++ b/src/main/rx/rx_spi.h
@@ -48,6 +48,7 @@ typedef enum {
RX_SPI_REDPINE,
RX_SPI_FRSKY_X_V2,
RX_SPI_FRSKY_X_LBT_V2,
+ RX_SPI_EXPRESSLRS,
RX_SPI_PROTOCOL_COUNT
} rx_spi_protocol_e;
diff --git a/src/main/target/BETAFPVF4SX1280/target.c b/src/main/target/BETAFPVF4SX1280/target.c
new file mode 100644
index 0000000000..4beb644fb3
--- /dev/null
+++ b/src/main/target/BETAFPVF4SX1280/target.c
@@ -0,0 +1,42 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+
+ DEF_TIM( TIM3, CH4, PB1, TIM_USE_LED, 0, 0 ), // LED Strip
+ DEF_TIM( TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0 ), // M1
+ DEF_TIM( TIM2, CH1, PA0, TIM_USE_MOTOR, 0, 0 ), // M2
+ DEF_TIM( TIM2, CH3, PB10,TIM_USE_MOTOR, 0, 0 ), // M3
+ DEF_TIM( TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0 ), // M4
+
+ DEF_TIM( TIM1, CH1, PA8, TIM_USE_MOTOR, 0, 0 ), //S5
+ DEF_TIM( TIM1, CH3, PA10,TIM_USE_MOTOR, 0, 0 ), //S6
+};
+
+
diff --git a/src/main/target/BETAFPVF4SX1280/target.h b/src/main/target/BETAFPVF4SX1280/target.h
new file mode 100644
index 0000000000..5758ec5f05
--- /dev/null
+++ b/src/main/target/BETAFPVF4SX1280/target.h
@@ -0,0 +1,149 @@
+
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "BHER"
+#define USBD_PRODUCT_STRING "BETAFPVF4SX1280"
+
+#define LED0_PIN PC14
+
+#define USE_BEEPER
+#define BEEPER_PIN PA14
+#define BEEPER_INVERTED
+
+#define USE_UART
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL1_TX_PIN PA8
+#define SOFTSERIAL1_RX_PIN PA8
+
+#define USE_UART1
+#define UART1_RX_PIN PA10
+#define UART1_TX_PIN PA9
+
+#define USE_UART2
+#define UART2_RX_PIN PA3
+#define UART2_TX_PIN PA2
+
+#define USE_VCP
+
+#define SERIAL_PORT_COUNT 4
+
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+#define SPI1_NSS_PIN PA4
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+#define SPI2_NSS_PIN PB12
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PB3
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+
+#define USE_GYRO
+#define USE_ACC
+
+//MPU6000
+#define USE_GYRO_SPI_MPU6000
+#define USE_ACC_SPI_MPU6000
+
+// ICM-20689
+#define USE_ACC_SPI_ICM20689
+#define USE_GYRO_SPI_ICM20689
+
+#define USE_EXTI
+#define USE_GYRO_EXTI
+#define GYRO_1_EXTI_PIN PB6
+#define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define GYRO_1_CS_PIN SPI1_NSS_PIN
+#define GYRO_1_SPI_INSTANCE SPI1
+#define GYRO_1_ALIGN CW90_DEG
+
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
+
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE SPI2
+#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN
+
+#define USE_RX_SPI
+#define RX_SPI_INSTANCE SPI3
+#define RX_SPI_LED_INVERTED
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
+#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_EXPRESSLRS
+
+#define USE_RX_EXPRESSLRS
+#define USE_RX_EXPRESSLRS_TELEMETRY
+#define USE_RX_SX1280
+#define RX_CHANNELS_AETR
+#define RX_SPI_BIND_PIN PB2
+#define RX_NSS_PIN PA15
+#define RX_SPI_LED_PIN PC15
+#define RX_SPI_EXTI_PIN PC13
+#define RX_EXPRESSLRS_SPI_RESET_PIN PB9
+#define RX_EXPRESSLRS_SPI_BUSY_PIN PA13
+#define RX_EXPRESSLRS_TIMER_INSTANCE TIM5
+
+
+#define ADC1_DMA_OPT 0
+
+#define USE_ADC
+#define USE_ADC_INTERNAL
+
+#define ADC1_INSTANCE ADC1
+
+#define VBAT_ADC_PIN PA1
+#define CURRENT_METER_ADC_PIN PB0
+
+#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
+
+#define VBAT_SCALE_DEFAULT 110
+#define CURRENT_METER_SCALE_DEFAULT 800
+
+#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP |FEATURE_SOFTSERIAL)
+
+#define USE_ESCSERIAL
+#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
+#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_OFF
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define USABLE_TIMER_CHANNEL_COUNT 7
+
+#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
+
+
+
diff --git a/src/main/target/BETAFPVF4SX1280/target.mk b/src/main/target/BETAFPVF4SX1280/target.mk
new file mode 100644
index 0000000000..1d6ed16a8a
--- /dev/null
+++ b/src/main/target/BETAFPVF4SX1280/target.mk
@@ -0,0 +1,17 @@
+F411_TARGETS += $(TARGET)
+HSE_VALUE = 8000000
+FEATURES += VCP ONBOARDFLASH
+
+TARGET_SRC = \
+ drivers/accgyro/accgyro_spi_icm20689.c\
+ drivers/barometer/barometer_bmp280.c \
+ drivers/accgyro/accgyro_spi_mpu6000.c \
+ drivers/max7456.c \
+ drivers/vtx_rtc6705.c \
+ drivers/vtx_rtc6705_soft_spi.c \
+ drivers/rx/expresslrs_driver.c \
+ drivers/rx/rx_sx127x.c \
+ drivers/rx/rx_sx1280.c \
+ rx/expresslrs_telemetry.c \
+ rx/expresslrs_common.c \
+ rx/expresslrs.c
diff --git a/src/main/target/CRAZYBEEF4SX1280/config.c b/src/main/target/CRAZYBEEF4SX1280/config.c
new file mode 100644
index 0000000000..08ee6386cc
--- /dev/null
+++ b/src/main/target/CRAZYBEEF4SX1280/config.c
@@ -0,0 +1,36 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+#include "io/serial.h"
+#include "pg/piniobox.h"
+#include "target.h"
+
+#define USE_TARGET_CONFIG
+
+void targetConfiguration(void)
+{
+ pinioBoxConfigMutable()->permanentId[0] = 40;
+ pinioBoxConfigMutable()->permanentId[1] = 41;
+
+}
+
diff --git a/src/main/target/CRAZYBEEF4SX1280/target.c b/src/main/target/CRAZYBEEF4SX1280/target.c
new file mode 100644
index 0000000000..0f19e3a7c1
--- /dev/null
+++ b/src/main/target/CRAZYBEEF4SX1280/target.c
@@ -0,0 +1,43 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+ DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM/RX2
+
+ DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), // S1_OUT - DMA1_ST1
+ DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // S2_OUT - DMA1_ST0
+ DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // S3_OUT - DMA1_ST3
+ DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // S4_OUT - DMA1_ST7
+
+ DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0), // 2812LED - DMA1_ST2
+
+ DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0 ), // TX2
+ DEF_TIM(TIM1, CH2, PA9, TIM_USE_PWM, 0, 0 ), // TX1
+ DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM, 0, 0 ), // RX1
+};
diff --git a/src/main/target/CRAZYBEEF4SX1280/target.h b/src/main/target/CRAZYBEEF4SX1280/target.h
new file mode 100644
index 0000000000..c895b8c167
--- /dev/null
+++ b/src/main/target/CRAZYBEEF4SX1280/target.h
@@ -0,0 +1,147 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#define USE_TARGET_CONFIG
+
+#define TARGET_BOARD_IDENTIFIER "HAMO"
+#define USBD_PRODUCT_STRING "CRAZYBEEF4SX1280"
+
+#define LED0_PIN PC13
+
+#define USE_BEEPER
+#define BEEPER_PIN PC15
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define GYRO_1_CS_PIN PA4
+#define GYRO_1_SPI_INSTANCE SPI1
+#define GYRO_1_ALIGN CW90_DEG
+
+#define USE_EXTI
+#define USE_GYRO_EXTI
+#define GYRO_1_EXTI_PIN PA1
+#define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define USE_GYRO
+#define USE_GYRO_SPI_MPU6000
+
+#define USE_ACC
+#define USE_ACC_SPI_MPU6000
+
+
+// *************** OSD/FLASH *****************************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE SPI2
+#define MAX7456_SPI_CS_PIN PB12
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define FLASH_CS_PIN PA14
+#define FLASH_SPI_INSTANCE SPI2
+
+// *************** SPI RX **********************
+#define USE_RX_SPI
+#define USE_RX_EXPRESSLRS
+#define USE_RX_SX1280
+//#define USE_RX_SX127X
+
+#define USE_SPI_DEVICE_3
+#define RX_SPI_INSTANCE SPI3
+#define SPI3_SCK_PIN PB3
+#define SPI3_MISO_PIN PB4
+#define SPI3_MOSI_PIN PB5
+
+#define RX_NSS_PIN PA15
+#define RX_SPI_EXTI_PIN PC14
+#define RX_SPI_BIND_PIN PB2
+#define RX_SPI_LED_PIN PB9
+#define RX_EXPRESSLRS_SPI_RESET_PIN PA8
+#define RX_EXPRESSLRS_SPI_BUSY_PIN PA13
+#define RX_EXPRESSLRS_TIMER_INSTANCE TIM3
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
+#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_EXPRESSLRS
+
+
+// *************** UART *****************************
+#define USE_VCP
+// #define USB_DETECT_PIN PC15
+// #define USE_USB_DETECT
+
+#define USE_UART1
+#define UART1_RX_PIN PA10
+#define UART1_TX_PIN PA9
+
+#define USE_UART2
+#define UART2_RX_PIN PA3
+#define UART2_TX_PIN PA2
+
+#define SERIAL_PORT_COUNT 3
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define ADC1_DMA_OPT 0 // DMA 2 Stream 0 Channel 0
+
+#define VBAT_ADC_PIN PB0
+#define CURRENT_METER_ADC_PIN PB1
+
+#define USE_ESCSERIAL
+
+#define USE_LED_STRIP
+
+#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
+#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_OFF
+
+#define USE_PINIO
+//#define PINIO1_PIN PB5 // VTX switcher
+//#define PINIO2_PIN PA15 // Camera switcher
+#define USE_PINIOBOX
+
+#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_RX_SPI)
+#define CURRENT_METER_SCALE_DEFAULT 1175
+#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define USABLE_TIMER_CHANNEL_COUNT 9
+#define USED_TIMERS (TIM_N(1)|TIM_N(2)|TIM_N(4)|TIM_N(5)|TIM_N(9))
+
diff --git a/src/main/target/CRAZYBEEF4SX1280/target.mk b/src/main/target/CRAZYBEEF4SX1280/target.mk
new file mode 100644
index 0000000000..0ab7b4dc4c
--- /dev/null
+++ b/src/main/target/CRAZYBEEF4SX1280/target.mk
@@ -0,0 +1,15 @@
+F411_TARGETS += $(TARGET)
+FEATURES += VCP ONBOARDFLASH
+
+TARGET_SRC = \
+ drivers/accgyro/accgyro_spi_mpu6000.c \
+ drivers/max7456.c \
+ drivers/vtx_rtc6705.c \
+ drivers/vtx_rtc6705_soft_spi.c \
+ drivers/rx/expresslrs_driver.c \
+ drivers/rx/rx_sx127x.c \
+ drivers/rx/rx_sx1280.c \
+ rx/expresslrs_telemetry.c \
+ rx/expresslrs_common.c \
+ rx/expresslrs.c
+
diff --git a/src/main/target/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h
index 37442292a4..d66ac3b596 100644
--- a/src/main/target/STM32_UNIFIED/target.h
+++ b/src/main/target/STM32_UNIFIED/target.h
@@ -333,4 +333,9 @@
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define USE_RANGEFINDER_TF
+
+#define USE_RX_EXPRESSLRS
+#define USE_RX_SX1280
+#define USE_RX_SX127X
+
#endif
diff --git a/src/main/target/STM32_UNIFIED/target.mk b/src/main/target/STM32_UNIFIED/target.mk
index 8a6b59c139..3cfe8b5af4 100644
--- a/src/main/target/STM32_UNIFIED/target.mk
+++ b/src/main/target/STM32_UNIFIED/target.mk
@@ -54,6 +54,12 @@ TARGET_SRC = \
rx/cc2500_redpine.c \
rx/a7105_flysky.c \
rx/cyrf6936_spektrum.c \
+ drivers/rx/expresslrs_driver.c \
+ rx/expresslrs.c \
+ rx/expresslrs_common.c \
+ rx/expresslrs_telemetry.c \
drivers/rx/rx_cc2500.c \
drivers/rx/rx_a7105.c \
- drivers/rx/rx_cyrf6936.c
+ drivers/rx/rx_cyrf6936.c \
+ drivers/rx/rx_sx127x.c \
+ drivers/rx/rx_sx1280.c \
diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h
index b3919713e3..df0ce74181 100644
--- a/src/main/target/common_defaults_post.h
+++ b/src/main/target/common_defaults_post.h
@@ -334,6 +334,22 @@
#define RX_CC2500_SPI_ANT_SEL_PIN NONE
#endif
#endif
+
+#if defined(USE_RX_EXPRESSLRS)
+#if !defined(RX_EXPRESSLRS_SPI_RESET_PIN)
+#define RX_EXPRESSLRS_SPI_RESET_PIN NONE
+#endif
+
+#if !defined(RX_EXPRESSLRS_SPI_BUSY_PIN)
+#define RX_EXPRESSLRS_SPI_BUSY_PIN NONE
+#endif
+
+#if !defined(RX_EXPRESSLRS_TIMER_INSTANCE)
+#define RX_EXPRESSLRS_TIMER_INSTANCE NULL
+#endif
+
+#endif
+
#endif
// gyro hardware
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index fb38258ebb..450aaca111 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -108,6 +108,9 @@
#undef USE_TELEMETRY_CRSF
#undef USE_CRSF_LINK_STATISTICS
#undef USE_CRSF_V3
+#endif
+
+#if !defined(USE_RX_EXPRESSLRS) && !defined(USE_SERIALRX_CRSF)
#undef USE_RX_RSSI_DBM
#endif
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 1525837319..1b27f696c4 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -138,14 +138,12 @@ bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
}
}
-static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize);
-
-static bool handleCrsfMspFrameBuffer()
+bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn)
{
static bool replyPending = false;
if (replyPending) {
if (crsfRxIsTelemetryBufEmpty()) {
- replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
+ replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
}
return replyPending;
}
@@ -157,7 +155,7 @@ static bool handleCrsfMspFrameBuffer()
const uint8_t mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
if (crsfRxIsTelemetryBufEmpty()) {
- replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
+ replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
} else {
replyPending = true;
}
@@ -740,7 +738,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
// Send ad-hoc response frames as soon as possible
#if defined(USE_MSP_OVER_TELEMETRY)
if (mspReplyPending) {
- mspReplyPending = handleCrsfMspFrameBuffer();
+ mspReplyPending = handleCrsfMspFrameBuffer(&crsfSendMspResponse);
crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
return;
}
@@ -800,7 +798,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
}
}
-#if defined(UNIT_TEST)
+#if defined(UNIT_TEST) || defined(USE_RX_EXPRESSLRS)
static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
{
crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
@@ -812,7 +810,7 @@ static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
return frameSize;
}
-STATIC_UNIT_TESTED int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
{
sbuf_t crsfFrameBuf;
sbuf_t *sbuf = &crsfFrameBuf;
@@ -833,10 +831,32 @@ STATIC_UNIT_TESTED int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
case CRSF_FRAMETYPE_GPS:
crsfFrameGps(sbuf);
break;
+#endif
+#if defined(USE_MSP_OVER_TELEMETRY)
+ case CRSF_FRAMETYPE_DEVICE_INFO:
+ crsfFrameDeviceInfo(sbuf);
+ break;
#endif
}
const int frameSize = crsfFinalizeBuf(sbuf, frame);
return frameSize;
}
+
+#if defined(USE_MSP_OVER_TELEMETRY)
+int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
+{
+ sbuf_t crsfFrameBuf;
+ sbuf_t *sbuf = &crsfFrameBuf;
+
+ crsfInitializeFrame(sbuf);
+ sbufWriteU8(sbuf, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+ sbufWriteU8(sbuf, CRSF_FRAMETYPE_MSP_RESP);
+ sbufWriteU8(sbuf, CRSF_ADDRESS_RADIO_TRANSMITTER);
+ sbufWriteU8(sbuf, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+ sbufWriteData(sbuf, payload, payloadSize);
+ const int frameSize = crsfFinalizeBuf(sbuf, frame);
+ return frameSize;
+}
+#endif
#endif
#endif
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 67c9694e91..c52f958cfd 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -26,7 +26,7 @@
#include "common/time.h"
#include "rx/crsf_protocol.h"
-
+#include "telemetry/msp_shared.h"
void initCrsfTelemetry(void);
uint32_t getCrsfDesiredSpeed(void);
@@ -43,6 +43,8 @@ void crsfProcessDisplayPortCmd(uint8_t *frameStart);
#if defined(USE_MSP_OVER_TELEMETRY)
void initCrsfMspBuffer(void);
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
+bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn);
+int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize);
#endif
#if defined(USE_CRSF_V3)
void speedNegotiationProcess(uint32_t currentTime);
diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h
index f92fde3f08..4ceb8e6826 100644
--- a/src/main/telemetry/msp_shared.h
+++ b/src/main/telemetry/msp_shared.h
@@ -26,7 +26,6 @@
// type of function to send MSP response chunk over telemetry.
typedef void (*mspResponseFnPtr)(uint8_t *payload, const uint8_t payloadSize);
-
void initSharedMsp(void);
// receives telemetry payload with msp and handles it.
diff --git a/src/test/Makefile b/src/test/Makefile
index 1546ca1dce..a2c105ed99 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -415,8 +415,37 @@ rx_spi_spektrum_unittest_SRC := \
$(USER_DIR)/rx/cyrf6936_spektrum.c
rx_spi_spektrum_unittest_DEFINES := \
- USE_RX_SPI \
- USE_RX_SPEKTRUM
+ USE_RX_SPI= \
+ USE_RX_SPEKTRUM=
+
+rx_spi_expresslrs_unittest_SRC := \
+ $(USER_DIR)/pg/rx_spi_expresslrs.c \
+ $(USER_DIR)/rx/expresslrs_common.c \
+ $(USER_DIR)/rx/expresslrs.c \
+
+rx_spi_expresslrs_unittest_DEFINES := \
+ USE_RX_SPI= \
+ USE_RX_EXPRESSLRS= \
+ USE_RX_SX1280= \
+ USE_RX_SX127X= \
+
+rx_spi_expresslrs_telemetry_unittest_SRC := \
+ $(USER_DIR)/rx/crsf.c \
+ $(USER_DIR)/telemetry/crsf.c \
+ $(USER_DIR)/common/crc.c \
+ $(USER_DIR)/common/maths.c \
+ $(USER_DIR)/common/streambuf.c \
+ $(USER_DIR)/common/gps_conversion.c \
+ $(USER_DIR)/common/printf.c \
+ $(USER_DIR)/common/typeconversion.c \
+ $(USER_DIR)/rx/expresslrs_telemetry.c \
+ $(USER_DIR)/build/atomic.c \
+ $(USER_DIR)/telemetry/msp_shared.c \
+
+rx_spi_expresslrs_telemetry_unittest_DEFINES := \
+ USE_RX_EXPRESSLRS= \
+ USE_GPS= \
+ USE_MSP_OVER_TELEMETRY= \
# Please tweak the following variable definitions as needed by your
# project, except GTEST_HEADERS, which you can use in your own targets
diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc
index f9c2d8b01c..ccb4e80510 100644
--- a/src/test/unit/rx_crsf_unittest.cc
+++ b/src/test/unit/rx_crsf_unittest.cc
@@ -357,7 +357,7 @@ const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NU
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
serialPort_t *telemetrySharedPort = NULL;
void crsfScheduleDeviceInfoResponse(void) {};
-void crsfScheduleMspResponse(void) {};
+void crsfScheduleMspResponse(uint8_t ) {};
bool bufferMspFrame(uint8_t *, int) {return true;}
bool isBatteryVoltageAvailable(void) { return true; }
bool isAmperageAvailable(void) { return true; }
diff --git a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc
new file mode 100644
index 0000000000..ccdf5154e7
--- /dev/null
+++ b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc
@@ -0,0 +1,466 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#include
+#include
+
+#include
+
+extern "C" {
+ #include "platform.h"
+
+ #include "build/version.h"
+ #include "common/printf.h"
+
+ #include "drivers/io.h"
+
+ #include "pg/pg.h"
+ #include "pg/pg_ids.h"
+ #include "pg/rx.h"
+
+ #include "msp/msp.h"
+ #include "msp/msp_serial.h"
+
+ #include "telemetry/telemetry.h"
+ #include "telemetry/msp_shared.h"
+ #include "rx/crsf_protocol.h"
+ #include "rx/expresslrs_telemetry.h"
+ #include "flight/imu.h"
+
+ #include "sensors/battery.h"
+ #include "sensors/sensors.h"
+ #include "sensors/acceleration.h"
+
+ #include "config/config.h"
+
+ #include "io/gps.h"
+
+ #include "msp/msp_protocol.h"
+
+ extern uint8_t tlmSensors;
+ extern uint8_t currentPayloadIndex;
+
+ extern volatile bool mspReplyPending;
+ extern volatile bool deviceInfoReplyPending;
+
+ bool airMode;
+
+ uint16_t testBatteryVoltage = 0;
+ int32_t testAmperage = 0;
+ int32_t testmAhDrawn = 0;
+
+ PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
+ PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+//make clean test_rx_spi_expresslrs_telemetry_unittest
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestInit)
+{
+ initTelemetry();
+ EXPECT_EQ(tlmSensors, 15);
+}
+
+static void testSetDataToTransmit(uint8_t payloadSize, uint8_t *payload)
+{
+ uint8_t *data;
+ uint8_t maxLength;
+ uint8_t packageIndex;
+ bool confirmValue = true;
+
+ setTelemetryDataToTransmit(payloadSize, payload, ELRS_TELEMETRY_BYTES_PER_CALL);
+
+ for (int j = 0; j < (1 + ((payloadSize - 1) / ELRS_TELEMETRY_BYTES_PER_CALL)); j++) {
+ getCurrentTelemetryPayload(&packageIndex, &maxLength, &data);
+ EXPECT_LE(maxLength, 5);
+ EXPECT_EQ(1 + j, packageIndex);
+ for(int i = 0; i < maxLength; i++) {
+ EXPECT_EQ(payload[i + j * ELRS_TELEMETRY_BYTES_PER_CALL], data[i]);
+ }
+ confirmCurrentTelemetryPayload(confirmValue);
+ confirmValue = !confirmValue;
+ }
+
+ getCurrentTelemetryPayload(&packageIndex, &maxLength, &data);
+ EXPECT_EQ(0, packageIndex);
+ EXPECT_EQ(true, isTelemetrySenderActive());
+ confirmCurrentTelemetryPayload(!confirmValue);
+ EXPECT_EQ(true, isTelemetrySenderActive());
+ confirmCurrentTelemetryPayload(confirmValue);
+ EXPECT_EQ(false, isTelemetrySenderActive());
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestGps)
+{
+ initTelemetry();
+ currentPayloadIndex = 0;
+
+ gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
+ gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
+ gpsSol.llh.altCm = 2345 * 100; // altitude in cm / 100 + 1000m offset, so CRSF value should be 3345
+ gpsSol.groundSpeed = 1630; // speed in cm/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
+ gpsSol.numSat = 9;
+ gpsSol.groundCourse = 1479; // degrees * 10
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_EQ(currentPayloadIndex, 1);
+
+ int32_t lattitude = payload[3] << 24 | payload[4] << 16 | payload[5] << 8 | payload[6];
+ EXPECT_EQ(560000000, lattitude);
+ int32_t longitude = payload[7] << 24 | payload[8] << 16 | payload[9] << 8 | payload[10];
+ EXPECT_EQ(1630000000, longitude);
+ uint16_t groundSpeed = payload[11] << 8 | payload[12];
+ EXPECT_EQ(587, groundSpeed);
+ uint16_t GPSheading = payload[13] << 8 | payload[14];
+ EXPECT_EQ(14790, GPSheading);
+ uint16_t altitude = payload[15] << 8 | payload[16];
+ EXPECT_EQ(3345, altitude);
+ uint8_t satelliteCount = payload[17];
+ EXPECT_EQ(9, satelliteCount);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestBattery)
+{
+ initTelemetry();
+ currentPayloadIndex = 1;
+
+ testBatteryVoltage = 330; // 3.3V = 3300 mv
+ testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
+ testmAhDrawn = 1234;
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_EQ(currentPayloadIndex, 2);
+
+ uint16_t voltage = payload[3] << 8 | payload[4]; // mV * 100
+ EXPECT_EQ(33, voltage);
+ uint16_t current = payload[5] << 8 | payload[6]; // mA * 100
+ EXPECT_EQ(296, current);
+ uint32_t capacity = payload[7] << 16 | payload[8] << 8 | payload[9]; // mAh
+ EXPECT_EQ(1234, capacity);
+ uint16_t remaining = payload[10]; // percent
+ EXPECT_EQ(67, remaining);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestAttitude)
+{
+ initTelemetry();
+ currentPayloadIndex = 2;
+
+ attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad
+ attitude.values.roll = 1495; // 2.609267231731523 rad
+ attitude.values.yaw = -1799; //3.139847324337799 rad
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_EQ(currentPayloadIndex, 3);
+
+ int16_t pitch = payload[3] << 8 | payload[4]; // rad / 10000
+ EXPECT_EQ(11833, pitch);
+ int16_t roll = payload[5] << 8 | payload[6];
+ EXPECT_EQ(26092, roll);
+ int16_t yaw = payload[7] << 8 | payload[8];
+ EXPECT_EQ(-31398, yaw);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestFlightMode)
+{
+ initTelemetry();
+ currentPayloadIndex = 3;
+
+ airMode = false;
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_EQ(currentPayloadIndex, 0);
+
+ EXPECT_EQ('W', payload[3]);
+ EXPECT_EQ('A', payload[4]);
+ EXPECT_EQ('I', payload[5]);
+ EXPECT_EQ('T', payload[6]);
+ EXPECT_EQ('*', payload[7]);
+ EXPECT_EQ(0, payload[8]);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVersionRequest)
+{
+ uint8_t request[15] = {238,12,122,200,234,48,0,1,1,0,0,0,0,128, 0};
+ uint8_t response[12] = {200,10,123,234,200,48,3,1,0,1,44,42};
+ uint8_t data1[6] = {1, request[0], request[1], request[2], request[3], request[4]};
+ uint8_t data2[6] = {2, request[5], request[6], request[7], request[8], request[9]};
+ uint8_t data3[6] = {3, request[10], request[11], request[12], request[13], request[14]};
+ uint8_t mspBuffer[15] = {0};
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ EXPECT_EQ(CRSF_ADDRESS_CRSF_TRANSMITTER, request[0]);
+ EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, request[2]);
+ EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, request[3]);
+ EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, request[4]);
+
+ initTelemetry();
+ initSharedMsp();
+
+ setMspDataToReceive(15, mspBuffer, ELRS_MSP_BYTES_PER_CALL);
+ receiveMspData(data1[0], data1 + 1);
+ receiveMspData(data2[0], data2 + 1);
+ receiveMspData(data3[0], data3 + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(0, 0);
+ EXPECT_TRUE(hasFinishedMspData());
+
+ processMspPacket(mspBuffer);
+ EXPECT_TRUE(mspReplyPending);
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+
+ EXPECT_EQ(payload[1] + 2, payloadSize);
+ EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
+ EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
+ EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
+
+ for (int i = 0; i < 12; i++) {
+ EXPECT_EQ(response[i], payload[i]);
+ }
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspPidRequest)
+{
+ uint8_t pidRequest[15] = {0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69, 0x00};
+ uint8_t data1[6] = {1, pidRequest[0], pidRequest[1], pidRequest[2], pidRequest[3], pidRequest[4]};
+ uint8_t data2[6] = {2, pidRequest[5], pidRequest[6], pidRequest[7], pidRequest[8], pidRequest[9]};
+ uint8_t data3[6] = {3, pidRequest[10], pidRequest[11], pidRequest[12], pidRequest[13], pidRequest[14]};
+ uint8_t mspBuffer[15] = {0};
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ initTelemetry();
+ initSharedMsp();
+
+ setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL);
+ receiveMspData(data1[0], data1 + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(data2[0], data2 + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(data3[0], data3 + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(0, 0);
+ EXPECT_TRUE(hasFinishedMspData());
+ for (int i = 0; i < 15; i ++) {
+ EXPECT_EQ(mspBuffer[i], pidRequest[i]);
+ }
+ EXPECT_FALSE(mspReplyPending);
+
+ processMspPacket(mspBuffer);
+ EXPECT_TRUE(mspReplyPending);
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_FALSE(mspReplyPending);
+
+ EXPECT_EQ(payloadSize, payload[1] + 2);
+ EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
+ EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
+ EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
+ EXPECT_EQ(0x31, payload[5]); //0x30 + 1 since it is second request, see msp_shared.c:L204
+ EXPECT_EQ(0x1E, payload[6]);
+ EXPECT_EQ(0x70, payload[7]);
+ for (int i = 1; i <= 30; i++) {
+ EXPECT_EQ(i, payload[i + 7]);
+ }
+ EXPECT_EQ(0x1E, payload[37]);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVtxRequest)
+{
+ uint8_t vtxRequest[15] = {0x00,0x0C,0x7C,0xC8,0xEA,0x30,0x04,0x59,0x18,0x00,0x01,0x00,0x44,0x5E, 0x00};
+ uint8_t data1[6] = {1, vtxRequest[0], vtxRequest[1], vtxRequest[2], vtxRequest[3], vtxRequest[4]};
+ uint8_t data2[6] = {2, vtxRequest[5], vtxRequest[6], vtxRequest[7], vtxRequest[8], vtxRequest[9]};
+ uint8_t data3[6] = {3, vtxRequest[10], vtxRequest[11], vtxRequest[12], vtxRequest[13], vtxRequest[14]};
+ uint8_t mspBuffer[15] = {0};
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ initTelemetry();
+ initSharedMsp();
+
+ setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL);
+ receiveMspData(data1[0], data1 + 1);
+ receiveMspData(data2[0], data2 + 1);
+ receiveMspData(data3[0], data3 + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(0, 0);
+ EXPECT_TRUE(hasFinishedMspData());
+
+ processMspPacket(mspBuffer);
+ EXPECT_TRUE(mspReplyPending);
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+
+ EXPECT_EQ(payloadSize, payload[1] + 2);
+ EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
+ EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
+ EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
+ EXPECT_EQ(0x32, payload[5]); //0x30 + 2 since it is third request, see msp_shared.c:L204
+ EXPECT_EQ(0x00, payload[6]);
+ EXPECT_EQ(0x59, payload[7]);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+TEST(RxSpiExpressLrsTelemetryUnitTest, TestDeviceInfoResp)
+{
+ uint8_t mspBuffer[15] = {0};
+
+ uint8_t *payload = 0;
+ uint8_t payloadSize = 0;
+
+ uint8_t pingData[4] = {1, CRSF_ADDRESS_CRSF_TRANSMITTER, 1, CRSF_FRAMETYPE_DEVICE_PING};
+
+ initTelemetry();
+ initSharedMsp();
+
+ setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL);
+ receiveMspData(pingData[0], pingData + 1);
+ EXPECT_FALSE(hasFinishedMspData());
+ receiveMspData(0, 0);
+ EXPECT_TRUE(hasFinishedMspData());
+
+ EXPECT_FALSE(deviceInfoReplyPending);
+
+ processMspPacket(mspBuffer);
+ EXPECT_TRUE(deviceInfoReplyPending);
+
+ getNextTelemetryPayload(&payloadSize, &payload);
+ EXPECT_FALSE(deviceInfoReplyPending);
+
+ EXPECT_EQ(CRSF_FRAMETYPE_DEVICE_INFO, payload[2]);
+ EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
+ EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
+ EXPECT_EQ(0x01, payload[payloadSize - 2]);
+ EXPECT_EQ(0, payload[payloadSize - 3]);
+
+ testSetDataToTransmit(payloadSize, payload);
+}
+
+// STUBS
+
+extern "C" {
+
+ attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
+ gpsSolutionData_t gpsSol;
+ rssiSource_e rssiSource;
+ uint8_t armingFlags;
+ uint8_t stateFlags;
+ uint16_t flightModeFlags;
+
+ uint32_t microsISR(void) {return 0; }
+
+ void beeperConfirmationBeeps(uint8_t ) {}
+
+ uint8_t calculateBatteryPercentageRemaining(void) {return 67; }
+
+ int32_t getAmperage(void) {return testAmperage; }
+
+ uint16_t getBatteryVoltage(void) {return testBatteryVoltage; }
+
+ uint16_t getLegacyBatteryVoltage(void) {return (testBatteryVoltage + 5) / 10; }
+
+ uint16_t getBatteryAverageCellVoltage(void) {return 0; }
+
+ batteryState_e getBatteryState(void) {return BATTERY_OK; }
+
+ bool featureIsEnabled(uint32_t) {return true; }
+ bool telemetryIsSensorEnabled(sensor_e) {return true; }
+ bool sensors(uint32_t ) { return true; }
+
+ bool airmodeIsEnabled(void) {return airMode; }
+
+ bool isBatteryVoltageConfigured(void) { return true; }
+ bool isAmperageConfigured(void) { return true; }
+
+ const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) { return NULL;}
+ serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; }
+ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
+
+ int32_t getEstimatedAltitudeCm(void) { return gpsSol.llh.altCm; }
+
+ int32_t getMAhDrawn(void) { return testmAhDrawn; }
+
+ bool isArmingDisabled(void) { return false; }
+
+ mspDescriptor_t mspDescriptorAlloc(void) {return 0; }
+
+ uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE];
+
+ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
+
+ UNUSED(srcDesc);
+ UNUSED(mspPostProcessFn);
+
+ sbuf_t *dst = &reply->buf;
+ const uint8_t cmdMSP = cmd->cmd;
+ reply->cmd = cmd->cmd;
+
+ if (cmdMSP == 0x70) {
+ for (unsigned int ii=1; ii<=30; ii++) {
+ sbufWriteU8(dst, ii);
+ }
+ } else if (cmdMSP == 0xCA) {
+ return MSP_RESULT_ACK;
+ } else if (cmdMSP == 0x01) {
+ sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
+ sbufWriteU8(dst, API_VERSION_MAJOR);
+ sbufWriteU8(dst, API_VERSION_MINOR);
+ }
+
+ return MSP_RESULT_ACK;
+ }
+
+ timeUs_t rxFrameTimeUs(void) { return 0; }
+
+}
diff --git a/src/test/unit/rx_spi_expresslrs_unittest.cc b/src/test/unit/rx_spi_expresslrs_unittest.cc
new file mode 100644
index 0000000000..bcfea96ac1
--- /dev/null
+++ b/src/test/unit/rx_spi_expresslrs_unittest.cc
@@ -0,0 +1,470 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/*
+ * Based on https://github.com/ExpressLRS/ExpressLRS
+ * Thanks to AlessandroAU, original creator of the ExpressLRS project.
+ */
+
+#include
+#include
+
+#include
+
+extern "C" {
+ #include "platform.h"
+
+ #include "drivers/io.h"
+ #include "common/filter.h"
+
+ #include "pg/pg.h"
+ #include "pg/pg_ids.h"
+ #include "pg/rx_spi.h"
+ #include "pg/rx_spi_expresslrs.h"
+
+ #include "rx/rx_spi.h"
+ #include "rx/expresslrs.h"
+ #include "rx/expresslrs_impl.h"
+
+ #include "drivers/rx/rx_sx127x.h"
+ #include "drivers/rx/rx_sx1280.h"
+
+ extern uint8_t FHSSsequence[ELRS_NR_SEQUENCE_ENTRIES];
+ extern uint16_t crc14tab[ELRS_CRC_LEN];
+
+ extern elrsReceiver_t receiver;
+ static const elrsReceiver_t empty = elrsReceiver_t();
+
+ static rxRuntimeState_t config = rxRuntimeState_t();
+ static rxSpiExtiConfig_t extiConfig;
+ static const rxSpiConfig_t injectedConfig = {
+ .extiIoTag = IO_TAG(PA0),
+ };
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+//make clean test_rx_spi_expresslrs_unittest
+TEST(RxSpiExpressLrsUnitTest, TestCrc14)
+{
+ uint16_t expectedCrc14Tab[ELRS_CRC_LEN] = {
+ 0,28247,62201,40110,52133,42482,14684,22283,
+ 38730,63773,26035,3044,23791,12984,44566,49217,
+ 11924,16579,56429,45626,58673,35686,6088,31135,
+ 47582,55177,19239,9584,29307,7212,32898,61141,
+ 29567,7464,33158,61393,47322,54925,18979,9332,
+ 58421,35426,5836,30875,12176,16839,56681,45886,
+ 24043,13244,44818,49477,38478,63513,25783,2784,
+ 51873,42230,14424,22031,260,28499,62461,40362,
+ 51369,42750,14928,21511,780,27995,61941,40866,
+ 24547,12724,44314,49997,37958,64017,26303,2280,
+ 58941,34922,5316,31379,11672,17359,57185,45366,
+ 29047,7968,33678,60889,47826,54405,18475,9852,
+ 48086,54657,18735,10104,28787,7716,33418,60637,
+ 11420,17099,56933,45106,59193,35182,5568,31639,
+ 38210,64277,26555,2540,24295,12464,44062,49737,
+ 520,27743,61681,40614,51629,43002,15188,21763,
+ 37202,65285,25515,3580,23287,13472,43022,50777,
+ 1560,26703,62689,39606,52669,41962,16196,20755,
+ 49094,53649,19775,9064,29795,6708,34458,59597,
+ 10380,18139,55925,46114,58153,36222,4560,32647,
+ 57901,35962,4308,32387,10632,18399,56177,46374,
+ 30055,6960,34718,59849,48834,53397,19515,8812,
+ 52409,41710,15936,20503,1820,26955,62949,39858,
+ 23539,13732,43274,51037,36950,65025,25263,3320,
+ 23035,14252,43778,50517,37470,64521,24743,3824,
+ 52913,41190,15432,21023,1300,27459,63469,39354,
+ 30575,6456,34198,60353,48330,53917,20019,8292,
+ 57381,36466,4828,31883,11136,17879,55673,46894,
+ 10884,17619,55421,46634,57633,36726,5080,32143,
+ 48590,54169,20279,8544,30315,6204,33938,60101,
+ 1040,27207,63209,39102,53173,41442,15692,21275,
+ 37722,64781,24995,4084,22783,13992,43526,50257
+ };
+
+ generateCrc14Table();
+ for (int i = 0; i < ELRS_CRC_LEN; i++) {
+ EXPECT_EQ(expectedCrc14Tab[i], crc14tab[i]);
+ }
+}
+
+TEST(RxSpiExpressLrsUnitTest, TestFHSSTable)
+{
+ const uint8_t UID[6] = {1, 2, 3, 4, 5, 6};
+
+ const uint8_t expectedSequence[2][ELRS_NR_SEQUENCE_ENTRIES] = {
+ {
+ 40, 43, 39, 18, 52, 19, 36, 7, 1, 12,
+ 71, 5, 42, 46, 50, 28, 49, 33, 76, 51,
+ 60, 70, 47, 61, 0, 55, 72, 37, 53, 25,
+ 3, 11, 41, 13, 35, 27, 9, 75, 48, 77,
+ 73, 74, 69, 58, 14, 31, 10, 59, 66, 4,
+ 78, 17, 44, 54, 29, 57, 21, 64, 22, 67,
+ 62, 56, 15, 79, 6, 34, 23, 30, 32, 2,
+ 68, 8, 63, 65, 45, 20, 24, 26, 16, 38,
+ 40, 8, 52, 29, 57, 10, 6, 26, 19, 75,
+ 21, 24, 1, 9, 50, 32, 69, 67, 2, 59,
+ 28, 48, 77, 60, 41, 49, 68, 4, 5, 3,
+ 44, 78, 58, 31, 16, 62, 35, 45, 73, 11,
+ 33, 46, 42, 36, 64, 7, 34, 53, 17, 25,
+ 37, 38, 54, 55, 15, 76, 18, 43, 23, 12,
+ 39, 51, 22, 79, 74, 63, 27, 66, 65, 47,
+ 70, 0, 30, 61, 13, 56, 14, 72, 71, 20,
+ 40, 71, 68, 12, 57, 45, 10, 53, 21, 15,
+ 69, 26, 54, 55, 73, 47, 35, 77, 1, 31,
+ 20, 0, 38, 76, 5, 60, 6, 79, 3, 16,
+ 50, 17, 52, 62, 18, 46, 28, 39, 29, 51,
+ 43, 34, 49, 56, 32, 61, 74, 58, 25, 44,
+ 2, 19, 65, 4, 13, 67, 11, 30, 66, 64,
+ 36, 24, 75, 33, 59, 7, 41, 70, 48, 14,
+ 42, 37, 8, 23, 78, 63, 22, 9, 72, 27
+ },
+ {
+ 20, 37, 1, 3, 7, 26, 36, 29, 15, 35,
+ 33, 24, 10, 34, 13, 31, 22, 9, 28, 23,
+ 17, 38, 6, 27, 0, 32, 11, 5, 18, 25,
+ 2, 4, 12, 19, 16, 8, 30, 14, 21, 39,
+ 20, 2, 14, 7, 13, 33, 32, 28, 21, 11,
+ 25, 17, 22, 9, 3, 4, 0, 31, 35, 38,
+ 10, 34, 26, 39, 36, 6, 19, 16, 30, 27,
+ 15, 24, 18, 1, 23, 37, 29, 8, 12, 5,
+ 20, 19, 24, 29, 27, 2, 22, 14, 0, 3,
+ 23, 13, 12, 35, 4, 25, 38, 18, 33, 36,
+ 21, 16, 5, 31, 9, 32, 11, 1, 6, 7,
+ 10, 15, 26, 34, 39, 37, 28, 17, 30, 8,
+ 20, 7, 4, 24, 19, 16, 8, 13, 15, 10,
+ 14, 36, 34, 0, 17, 12, 28, 21, 39, 22,
+ 3, 2, 32, 33, 27, 6, 37, 18, 31, 38,
+ 23, 25, 26, 30, 9, 1, 35, 5, 11, 29,
+ 20, 1, 35, 22, 0, 10, 11, 27, 18, 37,
+ 21, 31, 9, 19, 30, 17, 5, 38, 29, 36,
+ 3, 2, 25, 34, 23, 6, 15, 4, 16, 26,
+ 12, 24, 14, 13, 39, 8, 32, 7, 28, 33,
+ 20, 36, 13, 5, 39, 37, 15, 8, 9, 4,
+ 22, 12, 1, 6, 32, 25, 17, 18, 27, 28,
+ 23, 19, 26, 3, 38, 16, 2, 34, 14, 30,
+ 10, 11, 7, 0, 35, 24, 21, 33, 31, 29
+ }
+ };
+
+ FHSSrandomiseFHSSsequence(UID, ISM2400);
+ for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) {
+ EXPECT_EQ(expectedSequence[0][i], FHSSsequence[i]);
+ }
+
+ FHSSrandomiseFHSSsequence(UID, FCC915);
+ for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) {
+ EXPECT_EQ(expectedSequence[1][i], FHSSsequence[i]);
+ }
+}
+
+TEST(RxSpiExpressLrsUnitTest, TestInitUnbound)
+{
+ const uint8_t bindUID[6] = {0, 1, 2, 3, 4, 5};
+
+ receiver = empty;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+
+ //check initialization of elrsReceiver_t
+ EXPECT_TRUE(receiver.inBindingMode);
+ EXPECT_EQ(IO_NONE, receiver.resetPin);
+ EXPECT_EQ(IO_NONE, receiver.busyPin);
+ for (int i = 0; i < 6; i++) {
+ EXPECT_EQ(bindUID[i], receiver.UID[i]);
+ }
+ EXPECT_EQ(0, receiver.nonceRX);
+ EXPECT_EQ(0, receiver.freqOffset);
+ EXPECT_EQ(0, receiver.lastValidPacketMs);
+
+ const uint32_t initialFrequencies[7] = {
+ FREQ_HZ_TO_REG_VAL_900(433920000),
+ FREQ_HZ_TO_REG_VAL_900(921500000),
+ FREQ_HZ_TO_REG_VAL_900(433925000),
+ FREQ_HZ_TO_REG_VAL_900(866425000),
+ FREQ_HZ_TO_REG_VAL_900(866425000),
+ FREQ_HZ_TO_REG_VAL_900(915500000),
+ FREQ_HZ_TO_REG_VAL_24(2440400000)
+ };
+
+ for (int i = 0; i < 7; i++) {
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->domain = i;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(initialFrequencies[i], receiver.currentFreq);
+ }
+
+ // for unbound we need to initialize in 50HZ mode
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->rateIndex = 1;
+ rxExpressLrsSpiConfigMutable()->domain = FCC915;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(airRateConfig[0][2].index, receiver.modParams->index);
+ EXPECT_EQ(airRateConfig[0][2].enumRate, receiver.modParams->enumRate);
+ EXPECT_EQ(airRateConfig[0][2].bw, receiver.modParams->bw);
+ EXPECT_EQ(airRateConfig[0][2].sf, receiver.modParams->sf);
+ EXPECT_EQ(airRateConfig[0][2].cr, receiver.modParams->cr);
+ EXPECT_EQ(airRateConfig[0][2].interval, receiver.modParams->interval);
+ EXPECT_EQ(airRateConfig[0][2].tlmInterval, receiver.modParams->tlmInterval);
+ EXPECT_EQ(airRateConfig[0][2].fhssHopInterval, receiver.modParams->fhssHopInterval);
+ EXPECT_EQ(airRateConfig[0][2].preambleLen, receiver.modParams->preambleLen);
+
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->rateIndex = 1;
+ rxExpressLrsSpiConfigMutable()->domain = ISM2400;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(airRateConfig[1][3].index, receiver.modParams->index);
+ EXPECT_EQ(airRateConfig[1][3].enumRate, receiver.modParams->enumRate);
+ EXPECT_EQ(airRateConfig[1][3].bw, receiver.modParams->bw);
+ EXPECT_EQ(airRateConfig[1][3].sf, receiver.modParams->sf);
+ EXPECT_EQ(airRateConfig[1][3].cr, receiver.modParams->cr);
+ EXPECT_EQ(airRateConfig[1][3].interval, receiver.modParams->interval);
+ EXPECT_EQ(airRateConfig[1][3].tlmInterval, receiver.modParams->tlmInterval);
+ EXPECT_EQ(airRateConfig[1][3].fhssHopInterval, receiver.modParams->fhssHopInterval);
+ EXPECT_EQ(airRateConfig[1][3].preambleLen, receiver.modParams->preambleLen);
+
+ //check switch mode
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->switchMode = SM_HYBRID;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(16, config.channelCount);
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->switchMode = SM_HYBRID_WIDE;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(16, config.channelCount);
+}
+
+TEST(RxSpiExpressLrsUnitTest, TestInitBound)
+{
+ const uint8_t validUID[6] = {0, 0, 1, 2, 3, 4};
+ receiver = empty;
+ memcpy(rxExpressLrsSpiConfigMutable()->UID, validUID, 6);
+
+ // check mod params
+ for (int i = 0; i < ELRS_RATE_MAX; i++) {
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->rateIndex = i;
+ rxExpressLrsSpiConfigMutable()->domain = FCC915;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(airRateConfig[0][i].index, receiver.modParams->index);
+ EXPECT_EQ(airRateConfig[0][i].enumRate, receiver.modParams->enumRate);
+ EXPECT_EQ(airRateConfig[0][i].bw, receiver.modParams->bw);
+ EXPECT_EQ(airRateConfig[0][i].sf, receiver.modParams->sf);
+ EXPECT_EQ(airRateConfig[0][i].cr, receiver.modParams->cr);
+ EXPECT_EQ(airRateConfig[0][i].interval, receiver.modParams->interval);
+ EXPECT_EQ(airRateConfig[0][i].tlmInterval, receiver.modParams->tlmInterval);
+ EXPECT_EQ(airRateConfig[0][i].fhssHopInterval, receiver.modParams->fhssHopInterval);
+ EXPECT_EQ(airRateConfig[0][i].preambleLen, receiver.modParams->preambleLen);
+
+ receiver = empty;
+ rxExpressLrsSpiConfigMutable()->rateIndex = i;
+ rxExpressLrsSpiConfigMutable()->domain = ISM2400;
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_EQ(airRateConfig[1][i].index, receiver.modParams->index);
+ EXPECT_EQ(airRateConfig[1][i].enumRate, receiver.modParams->enumRate);
+ EXPECT_EQ(airRateConfig[1][i].bw, receiver.modParams->bw);
+ EXPECT_EQ(airRateConfig[1][i].sf, receiver.modParams->sf);
+ EXPECT_EQ(airRateConfig[1][i].cr, receiver.modParams->cr);
+ EXPECT_EQ(airRateConfig[1][i].interval, receiver.modParams->interval);
+ EXPECT_EQ(airRateConfig[1][i].tlmInterval, receiver.modParams->tlmInterval);
+ EXPECT_EQ(airRateConfig[1][i].fhssHopInterval, receiver.modParams->fhssHopInterval);
+ EXPECT_EQ(airRateConfig[1][i].preambleLen, receiver.modParams->preambleLen);
+ }
+
+ expressLrsSpiInit(&injectedConfig, &config, &extiConfig);
+ EXPECT_FALSE(receiver.inBindingMode);
+ for (int i = 0; i < 6; i++) {
+ EXPECT_EQ(validUID[i], receiver.UID[i]);
+ }
+}
+
+TEST(RxSpiExpressLrsUnitTest, TestLQCalc)
+{
+ lqReset();
+ for (int i = 1; i <= 100; i++) {
+ lqNewPeriod();
+ lqIncrease();
+ EXPECT_EQ(i, lqGet());
+ }
+ lqNewPeriod();
+ lqIncrease();
+ EXPECT_EQ(100, lqGet());
+ for (int i = 99; i >= 0; i--) {
+ lqNewPeriod();
+ EXPECT_EQ(i, lqGet());
+ }
+ lqNewPeriod();
+ EXPECT_EQ(0, lqGet());
+ lqReset();
+ lqNewPeriod();
+ EXPECT_EQ(0, lqGet());
+ lqIncrease();
+ EXPECT_EQ(1, lqGet());
+}
+
+TEST(RxSpiExpressLrsUnitTest, Test1bSwitchDecode)
+{
+ EXPECT_EQ(1000, convertSwitch1b(0));
+ EXPECT_EQ(2000, convertSwitch1b(1));
+ EXPECT_EQ(2000, convertSwitch1b(2));
+ EXPECT_EQ(2000, convertSwitch1b(255));
+}
+
+TEST(RxSpiExpressLrsUnitTest, Test3bSwitchDecode)
+{
+ EXPECT_EQ(1000, convertSwitch3b(0));
+ EXPECT_EQ(1275, convertSwitch3b(1));
+ EXPECT_EQ(1425, convertSwitch3b(2));
+ EXPECT_EQ(1575, convertSwitch3b(3));
+ EXPECT_EQ(1725, convertSwitch3b(4));
+ EXPECT_EQ(2000, convertSwitch3b(5));
+ EXPECT_EQ(1500, convertSwitch3b(6));
+ EXPECT_EQ(1500, convertSwitch3b(7));
+ EXPECT_EQ(1500, convertSwitch3b(8));
+ EXPECT_EQ(1500, convertSwitch3b(123));
+ EXPECT_EQ(1500, convertSwitch3b(255));
+}
+
+TEST(RxSpiExpressLrsUnitTest, Test4bSwitchDecode)
+{
+ EXPECT_EQ(1000, convertSwitchNb(0, 15));
+ EXPECT_EQ(1066, convertSwitchNb(1, 15));
+ EXPECT_EQ(1133, convertSwitchNb(2, 15));
+ EXPECT_EQ(1200, convertSwitchNb(3, 15));
+ EXPECT_EQ(1266, convertSwitchNb(4, 15));
+ EXPECT_EQ(1333, convertSwitchNb(5, 15));
+ EXPECT_EQ(1400, convertSwitchNb(6, 15));
+ EXPECT_EQ(1466, convertSwitchNb(7, 15));
+ EXPECT_EQ(1533, convertSwitchNb(8, 15));
+ EXPECT_EQ(1600, convertSwitchNb(9, 15));
+ EXPECT_EQ(1666, convertSwitchNb(10, 15));
+ EXPECT_EQ(1733, convertSwitchNb(11, 15));
+ EXPECT_EQ(1800, convertSwitchNb(12, 15));
+ EXPECT_EQ(1866, convertSwitchNb(13, 15));
+ EXPECT_EQ(1933, convertSwitchNb(14, 15));
+ EXPECT_EQ(2000, convertSwitchNb(15, 15));
+ EXPECT_EQ(1500, convertSwitchNb(16, 15));
+ EXPECT_EQ(1500, convertSwitchNb(255, 15));
+}
+
+TEST(RxSpiExpressLrsUnitTest, TestAnalogDecode)
+{
+ EXPECT_EQ(988, convertAnalog(172));
+ EXPECT_EQ(1500, convertAnalog(992));
+ EXPECT_EQ(2012, convertAnalog(1811));
+}
+
+// STUBS
+
+extern "C" {
+
+ int16_t *debug;
+ uint8_t debugMode;
+
+ rssiSource_e rssiSource;
+ linkQualitySource_e linkQualitySource;
+ void setRssi(uint16_t , rssiSource_e ) {}
+ void setRssiDirect(uint16_t , rssiSource_e ) {}
+
+ uint32_t micros(void) { return 0; }
+ uint32_t millis(void) { return 0; }
+
+ bool IORead(IO_t ) { return true; }
+ IO_t IOGetByTag(ioTag_t ) { return (IO_t)1; }
+ void IOHi(IO_t ) {}
+ void IOLo(IO_t ) {}
+ void writeEEPROM(void) {}
+
+ void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
+ void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
+ void rxSpiLedBlinkBind(void) {}
+ bool rxSpiCheckBindRequested(bool)
+ {
+ return false;
+ }
+ bool rxSpiExtiConfigured(void) { return true; }
+
+ bool sx1280IsBusy(void) { return false; }
+ void sx1280Config(const sx1280LoraBandwidths_e , const sx1280LoraSpreadingFactors_e , const sx1280LoraCodingRates_e , const uint32_t , const uint8_t , const bool ) {}
+ void sx1280StartReceiving(void) {}
+ uint8_t sx1280ISR(uint32_t *timestamp)
+ {
+ *timestamp = 0;
+ return 0;
+ }
+ void sx1280TransmitData(const uint8_t *, const uint8_t ) {}
+ void sx1280ReceiveData(uint8_t *, const uint8_t ) {}
+ void sx1280SetFrequencyReg(const uint32_t ) {}
+ void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr)
+ {
+ *rssi = 0;
+ *snr = 0;
+ }
+ void sx1280AdjustFrequency(int32_t , const uint32_t ) {}
+ bool sx1280Init(IO_t , IO_t ) { return true; }
+
+ void sx127xConfig(const sx127xBandwidth_e , const sx127xSpreadingFactor_e , const sx127xCodingRate_e , const uint32_t , const uint8_t , const bool ) {}
+ void sx127xStartReceiving(void) {}
+ uint8_t sx127xISR(uint32_t *timestamp)
+ {
+ *timestamp = 0;
+ return 0;
+ }
+ void sx127xTransmitData(const uint8_t *, const uint8_t ) {}
+ void sx127xReceiveData(uint8_t *, const uint8_t ) {}
+ void sx127xSetFrequencyReg(const uint32_t ) {}
+ void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr)
+ {
+ *rssi = 0;
+ *snr = 0;
+ }
+ void sx127xAdjustFrequency(int32_t , const uint32_t ) {}
+ bool sx127xInit(IO_t , IO_t ) { return true; }
+
+ int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) {
+ long int a = ((long int) destTo - (long int) destFrom) * ((long int) x - (long int) srcFrom);
+ long int b = (long int) srcTo - (long int) srcFrom;
+ return (a / b) + destFrom;
+ }
+
+ void expressLrsInitialiseTimer(TIM_TypeDef *, elrsReceiver_t *) {}
+ void expressLrsTimerEnableIRQs(void) {}
+ void expressLrsUpdateTimerInterval(uint16_t ) {}
+ void expressLrsUpdatePhaseShift(int32_t ) {}
+ void expressLrsTimerIncreaseFrequencyOffset(void) {}
+ void expressLrsTimerDecreaseFrequencyOffset(void) {}
+ void expressLrsTimerResetFrequencyOffset(void) {}
+ void expressLrsTimerStop(void) {}
+ void expressLrsTimerResume(void) {}
+ bool expressLrsTimerIsRunning(void) { return true; }
+ void expressLrsTimerDebug(void) {}
+
+ int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *, int32_t ) { return 0; }
+ void simpleLPFilterInit(simpleLowpassFilter_t *, int32_t , int32_t ) {}
+ void dbgPinHi(int ) {}
+ void dbgPinLo(int ) {}
+
+ void initTelemetry(void) {}
+ bool getNextTelemetryPayload(uint8_t *, uint8_t **) { return false; }
+
+ void setTelemetryDataToTransmit(const uint8_t , uint8_t* , const uint8_t ) {}
+ bool isTelemetrySenderActive(void) { return false; }
+ void getCurrentTelemetryPayload(uint8_t *, uint8_t *, uint8_t **) {}
+ void confirmCurrentTelemetryPayload(const bool ) {}
+ void updateTelemetryRate(const uint16_t , const uint8_t , const uint8_t ) {}
+
+}
diff --git a/unified_targets/configs/CRAZYBEEF4SX1280.config b/unified_targets/configs/CRAZYBEEF4SX1280.config
new file mode 100644
index 0000000000..6237182dc8
--- /dev/null
+++ b/unified_targets/configs/CRAZYBEEF4SX1280.config
@@ -0,0 +1,13 @@
+board_name CRAZYBEEF4SX1280
+manufacturer_id HAMO
+
+resource RX_SPI_CS 1 A15
+resource RX_SPI_EXTI 1 C14
+resource RX_SPI_BIND 1 B02
+resource RX_SPI_LED 1 B09
+resource RX_SPI_EXPRESSLRS_RESET 1 A08
+resource RX_SPI_EXPRESSLRS_BUSY 1 A13
+
+set expresslrs_domain = ISM2400
+set expresslrs_rate_index = 0
+set expresslrs_switch_mode = HYBRID