diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md
index 0a9a32b48a..3bc2657cf2 100644
--- a/docs/development/Building in Ubuntu.md
+++ b/docs/development/Building in Ubuntu.md
@@ -1,13 +1,12 @@
# Building in Ubuntu
Building for Ubuntu platform is remarkably easy.
-This document is tested and based on the latest Ubuntu 20.04 LTS release and can also be used for WSL.
+This document is tested and based on the latest Ubuntu 20.04.03 LTS release and can also be used for WSL(2).
### Clone betaflight repository and install toolchain
- $ sudo apt update
- $ sudo apt upgrade
- $ sudo apt install build-essential
+ $ sudo apt update && apt upgrade
+ $ sudo apt install build-essential libblocksruntime-dev git curl clang
$ git clone https://github.com/betaflight/betaflight.git
$ cd betaflight
$ make arm_sdk_install
@@ -29,6 +28,13 @@ Make sure to remove `obj/` and `make clean`, before building again.
### Building Betaflight Configurator
+ $ sudo apt update && apt upgrade
+ $ sudo apt install libatomic1 npm
+ $ sudo npm install -g gulp-cli yarn
+ $ curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.38.0/install.sh
+ $ source ~/.bashrc
+ $ nvm install v14.18.2 (for exact version please check link below)
+
See [Betaflight Configurator Development](https://github.com/betaflight/betaflight-configurator#development) for how to build the Betaflight Configurator.
### Flashing a target with Betaflight Configurator on Ubuntu 20.04
diff --git a/make/source.mk b/make/source.mk
index c3628f54b8..3bed09d3e9 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -370,6 +370,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm20689.c \
+ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c
diff --git a/src/main/config/config.c b/src/main/config/config.c
index c5a2a76803..f016fe4ee8 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -736,6 +736,9 @@ void writeUnmodifiedConfigToEEPROM(void)
void writeEEPROM(void)
{
+#ifdef USE_RX_SPI
+ rxSpiStop(); // some rx spi protocols use hardware timer, which needs to be stopped before writing to eeprom
+#endif
systemConfigMutable()->configurationState = CONFIGURATION_STATE_CONFIGURED;
writeUnmodifiedConfigToEEPROM();
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
index 747e450816..067c2d422b 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -217,7 +217,7 @@ bool mpuAccReadSPI(accDev_t *acc)
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
- acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
+ acc->gyro->dev.txBuf[0] = acc->gyro->accDataReg | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 7, true, NULL},
@@ -294,7 +294,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
- gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80;
+ gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 7, true, NULL},
diff --git a/src/main/drivers/rx/rx_sx127x.c b/src/main/drivers/rx/rx_sx127x.c
index 51ab625211..aa85fd13aa 100644
--- a/src/main/drivers/rx/rx_sx127x.c
+++ b/src/main/drivers/rx/rx_sx127x.c
@@ -427,6 +427,8 @@ void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr)
{
*rssi = sx127xGetLastPacketRSSI();
*snr = sx127xGetLastPacketSNR();
+ int8_t negOffset = (*snr < 0) ? *snr : 0;
+ *rssi += negOffset;
}
uint8_t sx127xGetIrqFlags(void)
diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c
index f0c6354968..7d35d88f2e 100644
--- a/src/main/drivers/rx/rx_sx1280.c
+++ b/src/main/drivers/rx/rx_sx1280.c
@@ -458,6 +458,8 @@ void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr)
sx1280ReadCommandBurst(SX1280_RADIO_GET_PACKETSTATUS, status, 2);
*rssi = -(int8_t)(status[0] / 2);
*snr = ((int8_t) status[1]) / 4;
+ int8_t negOffset = (*snr < 0) ? *snr : 0;
+ *rssi += negOffset;
}
#endif /* USE_RX_SX1280 */
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 19021c65ce..ac0434883f 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -321,7 +321,7 @@ static void taskCameraControl(uint32_t currentTime)
task_t tasks[TASK_COUNT];
// Task ID data in .data (initialised data)
-task_attr_t task_attrs[TASK_COUNT] = {
+task_attribute_t task_attributes[TASK_COUNT] = {
[TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
@@ -432,7 +432,7 @@ task_t *getTask(unsigned taskId)
void tasksInit(void)
{
for (int i = 0; i < TASK_COUNT; i++) {
- tasks[i].attr = &task_attrs[i];
+ tasks[i].attribute = &task_attributes[i];
}
schedulerInit();
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index ca0292db40..fe54b453b8 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -500,7 +500,7 @@ static void saSendFrame(uint8_t *buf, int len)
switch (smartAudioSerialPort->identifier) {
case SERIAL_PORT_SOFTSERIAL1:
case SERIAL_PORT_SOFTSERIAL2:
- if(vtxSettingsConfig()->softserialAlt) {
+ if (vtxSettingsConfig()->softserialAlt) {
serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
}
break;
diff --git a/src/main/rx/expresslrs.c b/src/main/rx/expresslrs.c
index 0cd023bbbc..4991b06e5a 100644
--- a/src/main/rx/expresslrs.c
+++ b/src/main/rx/expresslrs.c
@@ -33,6 +33,7 @@
#ifdef USE_RX_EXPRESSLRS
+#include "build/atomic.h"
#include "build/debug.h"
#include "build/debug_pin.h"
@@ -40,6 +41,7 @@
#include "common/filter.h"
#include "drivers/io.h"
+#include "drivers/nvic.h"
#include "drivers/rx/rx_spi.h"
#include "drivers/system.h"
#include "drivers/time.h"
@@ -51,6 +53,8 @@
#include "config/config.h"
#include "config/feature.h"
+#include "fc/init.h"
+
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx_spi.h"
@@ -354,19 +358,23 @@ static bool handleFHSS(void)
return true;
}
-static bool handleSendTelemetryResponse(void)
+static bool shouldSendTelemetryResponse(void)
+{
+ 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
+ } else {
+ return true;
+ }
+}
+
+static void 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;
@@ -408,13 +416,13 @@ static bool handleSendTelemetryResponse(void)
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();
+ int32_t maxOffset = receiver.modParams->interval / 4;
+ pl.rawOffsetUs = constrain(expressLrsEPRGetResult(), -maxOffset, maxOffset);
pl.offsetUs = simpleLPFilterUpdate(&pl.offsetFilter, pl.rawOffsetUs);
pl.offsetDeltaUs = simpleLPFilterUpdate(&pl.offsetDxFilter, pl.rawOffsetUs - pl.previousRawOffsetUs);
@@ -552,6 +560,7 @@ static void initializeReceiver(void)
receiver.rateIndex = receiver.inBindingMode ? bindingRateIndex : rxExpressLrsSpiConfig()->rateIndex;
setRFLinkRate(receiver.rateIndex);
+ receiver.started = false;
receiver.alreadyFHSS = false;
receiver.alreadyTLMresp = false;
receiver.lockRFmode = false;
@@ -584,7 +593,6 @@ static void unpackBindPacket(uint8_t *packet)
initializeReceiver();
receiver.configChanged = true; //after initialize as it sets it to false
- startReceiving();
}
/**
@@ -679,14 +687,12 @@ static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
return false;
}
-static rx_spi_received_e processRFPacket(uint8_t *payload)
+static rx_spi_received_e processRFPacket(uint8_t *payload, uint32_t timeStampUs)
{
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];
@@ -925,8 +931,6 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta
// Timer IRQs must only be enabled after the receiver is configured otherwise race conditions occur.
expressLrsTimerEnableIRQs();
- startReceiving();
-
return true;
}
@@ -955,17 +959,21 @@ static void handleConnectionStateUpdate(const uint32_t 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)))) {
+ ((receiver.connectionState == ELRS_CONNECTED) && (receiver.rfPerfParams->disconnectTimeoutMs < (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.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) {
+ gotConnection(timeStampMs); // detects when we are connected
}
if ((receiver.timerState == ELRS_TIM_TENTATIVE) && ((timeStampMs - receiver.gotConnectionMs) > ELRS_CONSIDER_CONNECTION_GOOD_MS) && (ABS(pl.offsetDeltaUs) <= 5)) {
receiver.timerState = ELRS_TIM_LOCKED;
}
+
+ if ((receiver.connectionState == ELRS_CONNECTED) && (ABS(pl.offsetDeltaUs) > 10) && (pl.offsetUs >= 100) && (lqGet() <= minLqForChaos())) {
+ lostConnection(); // SPI: resync when we're in chaos territory
+ }
}
static void handleConfigUpdate(const uint32_t timeStampMs)
@@ -973,7 +981,6 @@ 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;
}
@@ -986,15 +993,7 @@ static void handleLinkStatsUpdate(const uint32_t timeStampMs)
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 {
+ if (receiver.connectionState == ELRS_CONNECTED) {
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);
@@ -1006,6 +1005,14 @@ static void handleLinkStatsUpdate(const uint32_t timeStampMs)
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
rxSetUplinkTxPwrMw(txPowerIndexToValue(txPower));
+#endif
+ } else {
+ 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
}
}
@@ -1050,10 +1057,16 @@ static void enterBindingMode(void)
startReceiving();
}
+static uint32_t isrTimeStampUs;
+
rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
{
rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
- uint32_t isrTimeStampUs;
+
+ if (!receiver.started && (systemState & SYSTEM_STATE_READY)) {
+ receiver.started = true;
+ startReceiving(); // delay receiving after initialization to ensure a clean connect
+ }
if (rxSpiCheckBindRequested(true)) {
enterBindingMode();
@@ -1063,15 +1076,30 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
if (irqReason == ELRS_DIO_TX_DONE) {
startReceiving();
} else if (irqReason == ELRS_DIO_RX_DONE) {
- result = processRFPacket(payload);
+ result = processRFPacket(payload, isrTimeStampUs);
}
if (receiver.fhssRequired) {
receiver.fhssRequired = false;
- bool didFHSS = handleFHSS();
- bool tlmSent = handleSendTelemetryResponse();
+ bool didFHSS = false;
+ bool tlmReq = false;
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) { // prevent from updating nonce in TICK
+ didFHSS = handleFHSS();
+ tlmReq = shouldSendTelemetryResponse();
+ }
- if (!didFHSS && !tlmSent && lqPeriodIsSet() && rxExpressLrsSpiConfig()->domain != ISM2400) {
+ if (tlmReq) {
+ // in case we miss a packet before TLM we still need to estimate processing time using %
+ uint32_t processingTime = (micros() - isrTimeStampUs) % receiver.modParams->interval;
+ if (processingTime < PACKET_HANDLING_TO_TOCK_ISR_DELAY_US && receiver.timerState == ELRS_TIM_LOCKED) {
+ handleSendTelemetryResponse();
+ } else {
+ receiver.alreadyTLMresp = true;
+ startReceiving();
+ }
+ }
+
+ if (rxExpressLrsSpiConfig()->domain != ISM2400 && !didFHSS && !tlmReq && lqPeriodIsSet()) {
receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset
}
}
@@ -1094,4 +1122,11 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
return result;
}
+void expressLrsStop(void)
+{
+ if (receiver.started) {
+ lostConnection();
+ }
+}
+
#endif /* USE_RX_EXPRESSLRS */
diff --git a/src/main/rx/expresslrs.h b/src/main/rx/expresslrs.h
index c6ae3b874c..5ed7cdb882 100644
--- a/src/main/rx/expresslrs.h
+++ b/src/main/rx/expresslrs.h
@@ -34,3 +34,4 @@
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);
+void expressLrsStop(void);
diff --git a/src/main/rx/expresslrs_impl.h b/src/main/rx/expresslrs_impl.h
index eb480c4a60..2655cf2c99 100644
--- a/src/main/rx/expresslrs_impl.h
+++ b/src/main/rx/expresslrs_impl.h
@@ -73,6 +73,7 @@ typedef struct elrsReceiver_s {
bool alreadyFHSS;
bool alreadyTLMresp;
bool lockRFmode;
+ bool started;
timerState_e timerState;
connectionState_e connectionState;
diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c
index c6cab6fc64..68f6a21f1b 100644
--- a/src/main/rx/rx_spi.c
+++ b/src/main/rx/rx_spi.c
@@ -57,15 +57,19 @@ uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received
+static void nullProtocolStop(void) {}
+
typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
+typedef void (*protocolStopFnPtr)(void);
static protocolInitFnPtr protocolInit;
static protocolDataReceivedFnPtr protocolDataReceived;
static protocolProcessFrameFnPtr protocolProcessFrame;
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
+static protocolStopFnPtr protocolStop = nullProtocolStop;
STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
@@ -180,6 +184,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
protocolInit = expressLrsSpiInit;
protocolDataReceived = expressLrsDataReceived;
protocolSetRcDataFromPayload = expressLrsSetRcDataFromPayload;
+ protocolStop = expressLrsStop;
break;
#endif
default:
@@ -270,4 +275,10 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
return ret;
}
+
+void rxSpiStop(void)
+{
+ protocolStop();
+}
+
#endif
diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h
index b19f2b20a4..990cdda214 100644
--- a/src/main/rx/rx_spi.h
+++ b/src/main/rx/rx_spi.h
@@ -95,3 +95,4 @@ typedef struct {
#define RC_CHANNEL_RTH RC_SPI_AUX6 // return to home
bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState);
+void rxSpiStop(void);
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index c1540c8cf7..5e7b291a56 100644
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -131,7 +131,7 @@ bool queueAdd(task_t *task)
return false;
}
for (int ii = 0; ii <= taskQueueSize; ++ii) {
- if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->attr->staticPriority < task->attr->staticPriority) {
+ if (taskQueueArray[ii] == NULL || taskQueueArray[ii]->attribute->staticPriority < task->attribute->staticPriority) {
memmove(&taskQueueArray[ii+1], &taskQueueArray[ii], sizeof(task) * (taskQueueSize - ii));
taskQueueArray[ii] = task;
++taskQueueSize;
@@ -207,10 +207,10 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
{
taskInfo->isEnabled = queueContains(getTask(taskId));
- taskInfo->desiredPeriodUs = getTask(taskId)->attr->desiredPeriodUs;
- taskInfo->staticPriority = getTask(taskId)->attr->staticPriority;
- taskInfo->taskName = getTask(taskId)->attr->taskName;
- taskInfo->subTaskName = getTask(taskId)->attr->subTaskName;
+ taskInfo->desiredPeriodUs = getTask(taskId)->attribute->desiredPeriodUs;
+ taskInfo->staticPriority = getTask(taskId)->attribute->staticPriority;
+ taskInfo->taskName = getTask(taskId)->attribute->taskName;
+ taskInfo->subTaskName = getTask(taskId)->attribute->subTaskName;
taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
taskInfo->averageExecutionTimeUs = getTask(taskId)->anticipatedExecutionTime >> TASK_EXEC_TIME_SHIFT;
@@ -235,11 +235,11 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
} else {
return;
}
- task->attr->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
+ task->attribute->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
// Catch the case where the gyro loop is adjusted
if (taskId == TASK_GYRO) {
- desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attr->desiredPeriodUs);
+ desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attribute->desiredPeriodUs);
}
}
@@ -247,7 +247,7 @@ void setTaskEnabled(taskId_e taskId, bool enabled)
{
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
task_t *task = taskId == TASK_SELF ? currentTask : getTask(taskId);
- if (enabled && task->attr->taskFunc) {
+ if (enabled && task->attribute->taskFunc) {
queueAdd(task);
} else {
queueRemove(task);
@@ -341,7 +341,7 @@ void schedulerInit(void)
taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP;
taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP;
- desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attr->desiredPeriodUs);
+ desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->attribute->desiredPeriodUs);
lastTargetCycles = getCycleCounter();
@@ -377,12 +377,12 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
taskNextStateTime = -1;
float period = currentTimeUs - selectedTask->lastExecutedAtUs;
selectedTask->lastExecutedAtUs = currentTimeUs;
- selectedTask->lastDesiredAt += selectedTask->attr->desiredPeriodUs;
+ selectedTask->lastDesiredAt += selectedTask->attribute->desiredPeriodUs;
selectedTask->dynamicPriority = 0;
// Execute task
const timeUs_t currentTimeBeforeTaskCallUs = micros();
- selectedTask->attr->taskFunc(currentTimeBeforeTaskCallUs);
+ selectedTask->attribute->taskFunc(currentTimeBeforeTaskCallUs);
taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
taskTotalExecutionTime += taskExecutionTimeUs;
if (!ignoreCurrentTaskExecRate) {
@@ -569,14 +569,14 @@ FAST_CODE void scheduler(void)
// Update task dynamic priorities
for (task_t *task = queueFirst(); task != NULL; task = queueNext()) {
- if (task->attr->staticPriority != TASK_PRIORITY_REALTIME) {
+ if (task->attribute->staticPriority != TASK_PRIORITY_REALTIME) {
// Task has checkFunc - event driven
- if (task->attr->checkFunc) {
+ if (task->attribute->checkFunc) {
// Increase priority for event driven tasks
if (task->dynamicPriority > 0) {
- task->taskAgePeriods = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attr->desiredPeriodUs);
- task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgePeriods;
- } else if (task->attr->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
+ task->taskAgePeriods = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->attribute->desiredPeriodUs);
+ task->dynamicPriority = 1 + task->attribute->staticPriority * task->taskAgePeriods;
+ } else if (task->attribute->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
#if !defined(UNIT_TEST)
DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
@@ -587,16 +587,16 @@ FAST_CODE void scheduler(void)
checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
task->lastSignaledAtUs = currentTimeUs;
task->taskAgePeriods = 1;
- task->dynamicPriority = 1 + task->attr->staticPriority;
+ task->dynamicPriority = 1 + task->attribute->staticPriority;
} else {
task->taskAgePeriods = 0;
}
} else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution
- task->taskAgePeriods = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attr->desiredPeriodUs);
+ task->taskAgePeriods = (cmpTimeUs(currentTimeUs, task->lastExecutedAtUs) / task->attribute->desiredPeriodUs);
if (task->taskAgePeriods > 0) {
- task->dynamicPriority = 1 + task->attr->staticPriority * task->taskAgePeriods;
+ task->dynamicPriority = 1 + task->attribute->staticPriority * task->taskAgePeriods;
}
}
@@ -690,5 +690,5 @@ uint16_t getAverageSystemLoadPercent(void)
float schedulerGetCycleTimeMultiplier(void)
{
- return (float)clockMicrosToCycles(getTask(TASK_GYRO)->attr->desiredPeriodUs) / desiredPeriodCycles;
+ return (float)clockMicrosToCycles(getTask(TASK_GYRO)->attribute->desiredPeriodUs) / desiredPeriodCycles;
}
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index ee679f6ec5..d70dde4a69 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -191,11 +191,11 @@ typedef struct {
void (*taskFunc)(timeUs_t currentTimeUs);
timeDelta_t desiredPeriodUs; // target period of execution
const int8_t staticPriority; // dynamicPriority grows in steps of this size
-} task_attr_t;
+} task_attribute_t;
typedef struct {
// Task static data
- task_attr_t *attr;
+ task_attribute_t *attribute;
// Scheduling
uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation
diff --git a/src/main/target/BETAFPVF4SX1280/target.h b/src/main/target/BETAFPVF4SX1280/target.h
index 5758ec5f05..87ff15fa56 100644
--- a/src/main/target/BETAFPVF4SX1280/target.h
+++ b/src/main/target/BETAFPVF4SX1280/target.h
@@ -134,7 +134,7 @@
#define USE_ESCSERIAL
#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
-#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_OFF
+#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_ON
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
diff --git a/src/main/target/CRAZYBEEF4SX1280/target.h b/src/main/target/CRAZYBEEF4SX1280/target.h
index c895b8c167..ee27fb83b5 100644
--- a/src/main/target/CRAZYBEEF4SX1280/target.h
+++ b/src/main/target/CRAZYBEEF4SX1280/target.h
@@ -125,7 +125,7 @@
#define USE_LED_STRIP
#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
-#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_OFF
+#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_ON
#define USE_PINIO
//#define PINIO1_PIN PB5 // VTX switcher
diff --git a/src/main/target/NEUTRONRCF411SX1280/target.c b/src/main/target/NEUTRONRCF411SX1280/target.c
new file mode 100644
index 0000000000..a9d5ad9b86
--- /dev/null
+++ b/src/main/target/NEUTRONRCF411SX1280/target.c
@@ -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 .
+ */
+
+#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
+};
diff --git a/src/main/target/NEUTRONRCF411SX1280/target.h b/src/main/target/NEUTRONRCF411SX1280/target.h
new file mode 100644
index 0000000000..96bee09d9a
--- /dev/null
+++ b/src/main/target/NEUTRONRCF411SX1280/target.h
@@ -0,0 +1,146 @@
+
+/*
+ * 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 "NERC"
+#define USBD_PRODUCT_STRING "NEUTRONRCF411SX1280"
+
+#define LED0_PIN PC14
+
+#define USE_BEEPER
+#define BEEPER_PIN PA14
+#define BEEPER_INVERTED
+
+#define USE_UART
+
+#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 3
+
+#define USE_GYRO
+#define USE_ACC
+#define USE_GYRO_SPI_MPU6000
+#define USE_ACC_SPI_MPU6000
+#define USE_ACC_SPI_ICM20689
+#define USE_GYRO_SPI_ICM20689
+#define USE_ACCGYRO_BMI270
+#define USE_EXTI
+#define USE_GYRO_EXTI
+#define GYRO_1_EXTI_PIN PB6
+#define USE_MPU_DATA_READY_SIGNAL
+#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 PB12
+
+#define USE_FLASHFS
+#define USE_FLASH_TOOLS
+#define USE_FLASH_M25P16
+#define USE_FLASH_W25N01G // 1Gb NAND flash support
+#define USE_FLASH_W25M // Stacked die support
+#define USE_FLASH_W25M512 // 512Kb (256Kb x 2 stacked) NOR flash support
+#define USE_FLASH_W25M02G // 2Gb (1Gb x 2 stacked) NAND flash support
+#define FLASH_CS_PIN PA8
+#define FLASH_SPI_INSTANCE SPI2
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#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 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 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)
+
+#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 5
+
+#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4))
diff --git a/src/main/target/NEUTRONRCF411SX1280/target.mk b/src/main/target/NEUTRONRCF411SX1280/target.mk
new file mode 100644
index 0000000000..b05b3b3726
--- /dev/null
+++ b/src/main/target/NEUTRONRCF411SX1280/target.mk
@@ -0,0 +1,19 @@
+F411_TARGETS += $(TARGET)
+HSE_VALUE = 8000000
+FEATURES += VCP ONBOARDFLASH
+
+TARGET_SRC = \
+ drivers/accgyro/accgyro_spi_icm20689.c\
+ $(addprefix drivers/accgyro/,$(notdir $(wildcard $(SRC_DIR)/drivers/accgyro/*.c))) \
+ $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.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/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h
index d66ac3b596..0b4f296594 100644
--- a/src/main/target/STM32_UNIFIED/target.h
+++ b/src/main/target/STM32_UNIFIED/target.h
@@ -239,6 +239,13 @@
#define USE_ACCGYRO_LSM6DSO
#define USE_ACCGYRO_BMI270
+#if !defined(STM32F411xE)
+#define USE_GYRO_SPI_ICM42605
+#define USE_GYRO_SPI_ICM42688P
+#define USE_ACC_SPI_ICM42605
+#define USE_ACC_SPI_ICM42688P
+#endif
+
#define USE_MAG
#define USE_MAG_DATA_READY_SIGNAL
#define USE_MAG_HMC5883
diff --git a/src/test/Makefile b/src/test/Makefile
index 75c0e79be7..4d770a704d 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -424,6 +424,7 @@ rx_spi_expresslrs_unittest_SRC := \
$(USER_DIR)/pg/rx_spi_expresslrs.c \
$(USER_DIR)/rx/expresslrs_common.c \
$(USER_DIR)/rx/expresslrs.c \
+ $(USER_DIR)/build/atomic.c \
rx_spi_expresslrs_unittest_DEFINES := \
USE_RX_SPI= \
diff --git a/src/test/unit/rx_spi_expresslrs_unittest.cc b/src/test/unit/rx_spi_expresslrs_unittest.cc
index bcfea96ac1..f37b594736 100644
--- a/src/test/unit/rx_spi_expresslrs_unittest.cc
+++ b/src/test/unit/rx_spi_expresslrs_unittest.cc
@@ -28,6 +28,8 @@
extern "C" {
#include "platform.h"
+ #include "build/atomic.h"
+
#include "drivers/io.h"
#include "common/filter.h"
@@ -372,6 +374,7 @@ TEST(RxSpiExpressLrsUnitTest, TestAnalogDecode)
extern "C" {
+ uint8_t systemState;
int16_t *debug;
uint8_t debugMode;
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index cbd955e575..818afeba65 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -99,7 +99,7 @@ extern "C" {
extern task_t *queueFirst(void);
extern task_t *queueNext(void);
- task_attr_t task_attrs[TASK_COUNT] = {
+ task_attribute_t task_attributes[TASK_COUNT] = {
[TASK_SYSTEM] = {
.taskName = "SYSTEM",
.taskFunc = taskSystemLoad,
@@ -181,18 +181,18 @@ extern "C" {
TEST(SchedulerUnittest, SetupTasks)
{
for (int i = 0; i < TASK_COUNT; ++i) {
- tasks[i].attr = &task_attrs[i];
+ tasks[i].attribute = &task_attributes[i];
}
}
TEST(SchedulerUnittest, TestPriorites)
{
- EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].attr->staticPriority);
- EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].attr->staticPriority);
- EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].attr->staticPriority);
- EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].attr->staticPriority);
- EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].attr->staticPriority);
+ EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, tasks[TASK_SYSTEM].attribute->staticPriority);
+ EXPECT_EQ(TASK_PRIORITY_REALTIME, tasks[TASK_GYRO].attribute->staticPriority);
+ EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_ACCEL].attribute->staticPriority);
+ EXPECT_EQ(TASK_PRIORITY_LOW, tasks[TASK_SERIAL].attribute->staticPriority);
+ EXPECT_EQ(TASK_PRIORITY_MEDIUM, tasks[TASK_BATTERY_VOLTAGE].attribute->staticPriority);
}
TEST(SchedulerUnittest, TestQueueInit)
@@ -294,7 +294,7 @@ TEST(SchedulerUnittest, TestQueueArray)
EXPECT_EQ(enqueuedTasks, taskQueueSize);
for (int taskId = 0; taskId < TASK_COUNT_UNITTEST - 1; ++taskId) {
- if (tasks[taskId].attr->taskFunc) {
+ if (tasks[taskId].attribute->taskFunc) {
setTaskEnabled(static_cast(taskId), true);
enqueuedTasks++;
EXPECT_EQ(enqueuedTasks, taskQueueSize);