Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 09:16:07 +03:00
Code Issues Wiki Activity

Fullres channels

Browse source
This commit is contained in:
phobos- 2022-08-01 14:17:52 +02:00
parent 931a0ffecb
commit 7ca3033279
6 changed files with 269 additions and 185 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -519,7 +519,7 @@ static const char* const lookupTableFreqDomain[] = {
};
static const char* const lookupTableSwitchMode[] = {
"HYBRID", "WIDE",
"WIDE", "HYBRID",
};
#endif

200
src/main/rx/expresslrs.c
View file

@ -192,12 +192,28 @@ static void setRssiChannelData(uint16_t *rcData)
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)
static void unpackAnalogChannelData(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr)
{
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));
const uint8_t numOfChannels = 4;
const uint8_t srcBits = 10;
const uint16_t inputChannelMask = (1 << srcBits) - 1;
uint8_t bitsMerged = 0;
uint32_t readValue = 0;
uint8_t readByteIndex = 0;
for (uint8_t n = 0; n < numOfChannels; n++) {
while (bitsMerged < srcBits) {
uint8_t readByte = otaPktPtr->rc.ch[readByteIndex++];
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8;
}
rcData[n] = 988 + (readValue & inputChannelMask);
readValue >>= srcBits;
bitsMerged -= srcBits;
}
// The low latency switch
rcData[4] = convertSwitch1b(otaPktPtr->rc.ch4);
}
/**
@ -208,47 +224,23 @@ static void unpackAnalogChannelData(uint16_t *rcData, const uint8_t *payload)
*
* sets telemetry status bit
*/
static void unpackChannelDataHybridSwitch8(uint16_t *rcData, const uint8_t *payload)
static void unpackChannelDataHybridSwitch8(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr)
{
unpackAnalogChannelData(rcData, payload);
unpackAnalogChannelData(rcData, otaPktPtr);
const uint8_t switchByte = payload[5];
// The low latency switch
rcData[4] = convertSwitch1b((switchByte & 0x40) >> 6);
const uint8_t switchByte = otaPktPtr->rc.switches;
// 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:
if (switchIndex >= 6) {
// Because AUX1 (index 0) is the low latency switch, the low bit
// of the switchIndex can be used as data, and arrives as index "6"
rcData[11] = convertSwitchNb(switchByte & 0x0F, 15); //4-bit
break;
default:
break;
} else {
rcData[5 + switchIndex] = convertSwitch3b(switchByte & 0x07);
}
setRssiChannelData(rcData);
@ -266,13 +258,10 @@ static void unpackChannelDataHybridSwitch8(uint16_t *rcData, const uint8_t *payl
* Output: crsf.PackedRCdataOut, crsf.LinkStatistics.uplink_TX_Power
* Returns: TelemetryStatus bit
*/
static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload)
static void unpackChannelDataHybridWide(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr)
{
unpackAnalogChannelData(rcData, payload);
const uint8_t switchByte = payload[5];
// The low latency switch
rcData[4] = convertSwitch1b((switchByte & 0x80) >> 7);
unpackAnalogChannelData(rcData, otaPktPtr);
const uint8_t switchByte = otaPktPtr->rc.switches;
// The round-robin switch, 6-7 bits with the switch index implied by the nonce. Some logic moved to processRFPacket
if (wideSwitchIndex >= 7) {
@ -288,8 +277,7 @@ static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload
switchValue = switchByte & 0x7F; // 7-bit
}
switchValue = convertSwitchNb(switchValue, bins);
rcData[5 + wideSwitchIndex] = switchValue;
rcData[5 + wideSwitchIndex] = convertSwitchNb(switchValue, bins);
}
setRssiChannelData(rcData);
@ -387,23 +375,23 @@ bool expressLrsTelemRespReq(void)
static void expressLrsSendTelemResp(void)
{
uint8_t *data;
uint8_t maxLength;
uint8_t packageIndex;
elrsOtaPacket_t otaPkt = {0};
receiver.alreadyTelemResp = true;
telemetryPacket[0] = ELRS_TLM_PACKET;
otaPkt.type = ELRS_TLM_PACKET;
if (nextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || !isTelemetrySenderActive()) {
telemetryPacket[1] = ELRS_TELEMETRY_TYPE_LINK;
telemetryPacket[2] = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; //diversity not supported
telemetryPacket[3] = connectionHasModelMatch << 7;
telemetryPacket[4] = receiver.snr;
telemetryPacket[5] = receiver.uplinkLQ;
otaPkt.tlm_dl.type = ELRS_TELEMETRY_TYPE_LINK;
otaPkt.tlm_dl.ul_link_stats.uplink_RSSI_1 = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered;
otaPkt.tlm_dl.ul_link_stats.uplink_RSSI_2 = 0; //diversity not supported
otaPkt.tlm_dl.ul_link_stats.antenna = 0;
otaPkt.tlm_dl.ul_link_stats.modelMatch = connectionHasModelMatch;
otaPkt.tlm_dl.ul_link_stats.lq = receiver.uplinkLQ;
otaPkt.tlm_dl.ul_link_stats.SNR = receiver.snr;
#ifdef USE_MSP_OVER_TELEMETRY
telemetryPacket[6] = getCurrentMspConfirm() ? 1 : 0;
otaPkt.tlm_dl.ul_link_stats.mspConfirm = getCurrentMspConfirm() ? 1 : 0;
#else
telemetryPacket[6] = 0;
otaPkt.tlm_dl.ul_link_stats.mspConfirm = 0;
#endif
nextTelemetryType = ELRS_TELEMETRY_TYPE_DATA;
// Start the count at 1 because the next will be DATA and doing +1 before checking
@ -416,18 +404,14 @@ static void expressLrsSendTelemResp(void)
nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
}
getCurrentTelemetryPayload(&packageIndex, &maxLength, &data);
telemetryPacket[1] = (packageIndex << ELRS_TELEMETRY_SHIFT) + ELRS_TELEMETRY_TYPE_DATA;
telemetryPacket[2] = maxLength > 0 ? *data : 0;
telemetryPacket[3] = maxLength >= 1 ? *(data + 1) : 0;
telemetryPacket[4] = maxLength >= 2 ? *(data + 2) : 0;
telemetryPacket[5] = maxLength >= 3 ? *(data + 3) : 0;
telemetryPacket[6] = maxLength >= 4 ? *(data + 4) : 0;
otaPkt.tlm_dl.type = ELRS_TELEMETRY_TYPE_DATA;
otaPkt.tlm_dl.packageIndex = getCurrentTelemetryPayload(otaPkt.tlm_dl.payload);
}
uint16_t crc = calcCrc14((uint8_t *)telemetryPacket, 7, crcInitializer);
telemetryPacket[0] |= (crc >> 6) & 0xFC;
telemetryPacket[7] = crc & 0xFF;
uint16_t crc = calcCrc14((uint8_t *) &otaPkt, 7, crcInitializer);
otaPkt.crcHigh = (crc >> 8);
otaPkt.crcLow = crc;
memcpy((uint8_t *) telemetryPacket, (uint8_t *) &otaPkt, ELRS_RX_TX_BUFF_SIZE);
}
static void updatePhaseLock(void)
@ -594,10 +578,10 @@ static void initializeReceiver(void)
static void unpackBindPacket(volatile uint8_t *packet)
{
rxExpressLrsSpiConfigMutable()->UID[2] = packet[3];
rxExpressLrsSpiConfigMutable()->UID[3] = packet[4];
rxExpressLrsSpiConfigMutable()->UID[4] = packet[5];
rxExpressLrsSpiConfigMutable()->UID[5] = packet[6];
rxExpressLrsSpiConfigMutable()->UID[2] = packet[0];
rxExpressLrsSpiConfigMutable()->UID[3] = packet[1];
rxExpressLrsSpiConfigMutable()->UID[4] = packet[2];
rxExpressLrsSpiConfigMutable()->UID[5] = packet[3];
receiver.UID = rxExpressLrsSpiConfigMutable()->UID;
crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5];
@ -608,12 +592,12 @@ static void unpackBindPacket(volatile uint8_t *packet)
/**
* Process the assembled MSP packet in mspBuffer[]
**/
static void processRFMspPacket(volatile uint8_t *packet)
static void processRFMspPacket(volatile elrsOtaPacket_t const * const otaPktPtr)
{
// 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
if (receiver.inBindingMode && otaPktPtr->msp_ul.packageIndex == 1 && otaPktPtr->msp_ul.payload[0] == ELRS_MSP_BIND) {
unpackBindPacket((uint8_t *)&otaPktPtr->msp_ul.payload[1]); //onELRSBindMSP
return;
}
@ -625,7 +609,7 @@ static void processRFMspPacket(volatile uint8_t *packet)
}
bool currentMspConfirmValue = getCurrentMspConfirm();
receiveMspData(packet[1], packet + 2);
receiveMspData(otaPktPtr->msp_ul.packageIndex, otaPktPtr->msp_ul.payload);
if (currentMspConfirmValue != getCurrentMspConfirm()) {
nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
}
@ -645,25 +629,25 @@ static void processRFMspPacket(volatile uint8_t *packet)
#endif
}
static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeStampMs)
static bool processRFSyncPacket(volatile elrsOtaPacket_t const * const otaPktPtr, 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]) {
if (otaPktPtr->sync.UID3 != receiver.UID[3] || otaPktPtr->sync.UID4 != 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)) {
if ((otaPktPtr->sync.UID5 & ~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] & 0xE0) >> 5) - 3;
uint8_t switchEncMode = ((packet[3] & 0x03) >> 0) - 1; //spi implementation uses 0 based index for hybrid
receiver.nextRateIndex = (rxExpressLrsSpiConfig()->domain == ISM2400) ? airRateIndexToIndex24(otaPktPtr->sync.rateIndex, receiver.rateIndex) : airRateIndexToIndex900(otaPktPtr->sync.rateIndex, receiver.rateIndex);
uint8_t switchEncMode = otaPktPtr->sync.switchEncMode;
// Update switch mode encoding immediately
if (switchEncMode != rxExpressLrsSpiConfig()->switchMode) {
@ -672,7 +656,7 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta
}
// Update TLM ratio
uint8_t tlmRateIn = ((packet[3] & 0x1C) >> 2) + TLM_RATIO_NO_TLM;
uint8_t tlmRateIn = otaPktPtr->sync.newTlmRatio + TLM_RATIO_NO_TLM;
uint8_t tlmDenom = tlmRatioEnumToValue(tlmRateIn);
if (currTlmDenom != tlmDenom) {
currTlmDenom = tlmDenom;
@ -681,11 +665,11 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta
// 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);
bool modelMatched = otaPktPtr->sync.UID5 == (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];
if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != otaPktPtr->sync.nonce || fhssGetCurrIndex() != otaPktPtr->sync.fhssIndex || connectionHasModelMatch != modelMatched) {
fhssSetCurrIndex(otaPktPtr->sync.fhssIndex);
receiver.nonceRX = otaPktPtr->sync.nonce;
tentativeConnection(timeStampMs);
connectionHasModelMatch = modelMatched;
@ -698,22 +682,25 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta
return false;
}
static bool validatePacketCrcStd(volatile elrsOtaPacket_t * const otaPktPtr)
{
uint16_t const inCRC = ((uint16_t)otaPktPtr->crcHigh << 8) + otaPktPtr->crcLow;
// For smHybrid the CRC only has the packet type in byte 0
// For smWide the FHSS slot is added to the CRC in byte 0 on PACKET_TYPE_RCDATAs
if (otaPktPtr->type == ELRS_RC_DATA_PACKET && rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) {
otaPktPtr->crcHigh = (receiver.nonceRX % receiver.modParams->fhssHopInterval) + 1;
} else {
otaPktPtr->crcHigh = 0;
}
uint16_t const calculatedCRC = calcCrc14((uint8_t *) otaPktPtr, 7, crcInitializer);
return inCRC == calculatedCRC;
}
rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampUs)
{
elrsPacketType_e type = dmaBuffer[0] & 0x03;
uint16_t inCRC = (((uint16_t)(dmaBuffer[0] & 0xFC)) << 6 ) | dmaBuffer[7];
volatile elrsOtaPacket_t * const otaPktPtr = (elrsOtaPacket_t * const) dmaBuffer;
// 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) {
dmaBuffer[0] = type;
} else {
uint8_t nonceFHSSresult = receiver.nonceRX % receiver.modParams->fhssHopInterval;
dmaBuffer[0] = type | (nonceFHSSresult << 2);
}
uint16_t calculatedCRC = calcCrc14((uint8_t *)dmaBuffer, 7, crcInitializer);
if (inCRC != calculatedCRC) {
if (!validatePacketCrcStd(otaPktPtr)) {
return RX_SPI_RECEIVED_NONE;
}
@ -724,7 +711,7 @@ rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampU
receiver.lastValidPacketMs = timeStampMs;
switch(type) {
switch(otaPktPtr->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
@ -732,22 +719,22 @@ rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampU
if (rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) {
wideSwitchIndex = hybridWideNonceToSwitchIndex(receiver.nonceRX);
if ((currTlmDenom < 8) || wideSwitchIndex == 7) {
confirmCurrentTelemetryPayload((dmaBuffer[6] & 0x40) >> 6);
confirmCurrentTelemetryPayload((otaPktPtr->rc.switches & 0x40) >> 6);
}
} else {
confirmCurrentTelemetryPayload(dmaBuffer[6] & (1 << 7));
confirmCurrentTelemetryPayload(otaPktPtr->rc.switches & (1 << 6));
}
memcpy((uint8_t *)payload, (uint8_t *)&dmaBuffer[1], 6); // stick data handling is done in expressLrsSetRcDataFromPayload
memcpy((uint8_t *)payload, (uint8_t *)dmaBuffer, ELRS_RX_TX_BUFF_SIZE); // stick data handling is done in expressLrsSetRcDataFromPayload
}
break;
case ELRS_MSP_DATA_PACKET:
processRFMspPacket(dmaBuffer);
processRFMspPacket(otaPktPtr);
break;
case ELRS_TLM_PACKET:
//not implemented
break;
case ELRS_SYNC_PACKET:
shouldStartTimer = processRFSyncPacket(dmaBuffer, timeStampMs) && !receiver.inBindingMode;
shouldStartTimer = processRFSyncPacket(otaPktPtr, timeStampMs) && !receiver.inBindingMode;
break;
default:
return RX_SPI_RECEIVED_NONE;
@ -926,7 +913,7 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta
initTelemetry();
#ifdef USE_MSP_OVER_TELEMETRY
setMspDataToReceive(ELRS_MSP_BUFFER_SIZE, mspBuffer, ELRS_MSP_BYTES_PER_CALL);
setMspDataToReceive(ELRS_MSP_BUFFER_SIZE, mspBuffer);
#endif
// Timer IRQs must only be enabled after the receiver is configured otherwise race conditions occur.
@ -1039,7 +1026,7 @@ void expressLrsHandleTelemetryUpdate(void)
uint8_t *nextPayload = 0;
uint8_t nextPlayloadSize = 0;
if (!isTelemetrySenderActive() && getNextTelemetryPayload(&nextPlayloadSize, &nextPayload)) {
setTelemetryDataToTransmit(nextPlayloadSize, nextPayload, ELRS_TELEMETRY_BYTES_PER_CALL);
setTelemetryDataToTransmit(nextPlayloadSize, nextPayload);
}
updateTelemetryBurst();
}
@ -1047,7 +1034,8 @@ void expressLrsHandleTelemetryUpdate(void)
void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
{
if (rcData && payload) {
rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE ? unpackChannelDataHybridWide(rcData, payload) : unpackChannelDataHybridSwitch8(rcData, payload);
volatile elrsOtaPacket_t * const otaPktPtr = (elrsOtaPacket_t * const) payload;
rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE ? unpackChannelDataHybridWide(rcData, otaPktPtr) : unpackChannelDataHybridSwitch8(rcData, otaPktPtr);
}
}

53
src/main/rx/expresslrs_common.c
View file

@ -190,7 +190,7 @@ void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom)
seed = ((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5];
fhssConfig = &fhssConfigs[dom];
seqCount = (256 / MAX(fhssConfig->freqCount, 1)) * fhssConfig->freqCount;
syncChannel = fhssConfig->freqCount / 2;
syncChannel = (fhssConfig->freqCount / 2) + 1;
freqSpread = (fhssConfig->freqStop - fhssConfig->freqStart) * ELRS_FREQ_SPREAD_SCALE / MAX((fhssConfig->freqCount - 1), 1);
// initialize the sequence array
@ -348,11 +348,6 @@ 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):
@ -365,4 +360,50 @@ uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce)
return ((nonce & 0x07) + ((nonce >> 3) & 0x01)) % 8;
}
uint8_t airRateIndexToIndex900(uint8_t airRate, uint8_t currentIndex)
{
switch (airRate) {
case 0:
return 0;
case 1:
return currentIndex;
case 2:
return 1;
case 3:
return 2;
case 4:
return 3;
default:
return currentIndex;
}
}
uint8_t airRateIndexToIndex24(uint8_t airRate, uint8_t currentIndex)
{
switch (airRate) {
case 0:
return currentIndex;
case 1:
return currentIndex;
case 2:
return currentIndex;
case 3:
return currentIndex;
case 4:
return 0;
case 5:
return currentIndex;
case 6:
return 1;
case 7:
return 2;
case 8:
return currentIndex;
case 9:
return 3;
default:
return currentIndex;
}
}
#endif /* USE_RX_EXPRESSLRS */

72
src/main/rx/expresslrs_common.h
View file

@ -31,6 +31,8 @@
#include "drivers/io_types.h"
#include "drivers/time.h"
#include "rx/expresslrs_telemetry.h"
#define ELRS_CRC_LEN 256
#define ELRS_CRC14_POLY 0x2E57
@ -82,8 +84,8 @@ typedef enum {
} elrsFreqDomain_e;
typedef enum {
SM_HYBRID = 0,
SM_HYBRID_WIDE = 1
SM_HYBRID_WIDE = 0,
SM_HYBRID = 1
} elrsSwitchMode_e;
typedef enum {
@ -125,12 +127,12 @@ typedef struct elrsModSettings_s {
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
int16_t sensitivity; // expected RF sensitivity based on
uint16_t toa; // time on air in microseconds
uint16_t disconnectTimeoutMs; // Time without a packet before receiver goes to disconnected (ms)
uint16_t rxLockTimeoutMs; // Max time to go from tentative -> connected state on receiver (ms)
uint16_t syncPktIntervalDisconnected; // how often to send the SYNC_PACKET packet (ms) when there is no response from RX
uint16_t syncPktIntervalConnected; // how often to send the SYNC_PACKET packet (ms) when there we have a connection
} elrsRfPerfParams_t;
typedef struct elrsFhssConfig_s {
@ -140,6 +142,56 @@ typedef struct elrsFhssConfig_s {
uint8_t freqCount;
} elrsFhssConfig_t;
typedef struct elrsOtaPacket_s {
// The packet type must always be the low two bits of the first byte of the
// packet to match the same placement in OTA_Packet8_s
uint8_t type: 2,
crcHigh: 6;
union {
/** PACKET_TYPE_RCDATA **/
struct {
uint8_t ch[5];
uint8_t switches : 7,
ch4 : 1;
} rc;
/** PACKET_TYPE_MSP **/
struct {
uint8_t packageIndex;
uint8_t payload[ELRS_MSP_BYTES_PER_CALL];
} msp_ul;
/** PACKET_TYPE_SYNC **/
struct {
uint8_t fhssIndex;
uint8_t nonce;
uint8_t switchEncMode : 1,
newTlmRatio : 3,
rateIndex : 4;
uint8_t UID3;
uint8_t UID4;
uint8_t UID5;
} sync;
/** PACKET_TYPE_TLM **/
struct {
uint8_t type : ELRS_TELEMETRY_SHIFT,
packageIndex : (8 - ELRS_TELEMETRY_SHIFT);
union {
struct {
uint8_t uplink_RSSI_1 : 7,
antenna : 1;
uint8_t uplink_RSSI_2 : 7,
modelMatch : 1;
uint8_t lq : 7,
mspConfirm : 1;
int8_t SNR;
uint8_t free;
} ul_link_stats;
uint8_t payload[ELRS_TELEMETRY_BYTES_PER_CALL];
};
} tlm_dl;
};
uint8_t crcLow;
} __attribute__ ((__packed__)) elrsOtaPacket_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);
@ -180,5 +232,7 @@ 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);
uint8_t airRateIndexToIndex24(uint8_t airRate, uint8_t currentIndex);
uint8_t airRateIndexToIndex900(uint8_t airRate, uint8_t currentIndex);

117
src/main/rx/expresslrs_telemetry.c
View file

@ -31,6 +31,7 @@
#ifdef USE_RX_EXPRESSLRS
#include "common/maths.h"
#include "config/feature.h"
#include "fc/runtime_config.h"
@ -65,10 +66,10 @@ static crsfFrameType_e payloadTypes[] = {
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 *data = NULL;
static uint8_t length = 0;
static uint8_t currentOffset;
static uint8_t bytesLastPayload;
static uint8_t currentPackage;
static bool waitUntilTelemetryConfirm;
static uint16_t waitCount;
@ -77,33 +78,26 @@ static volatile stubbornSenderState_e senderState;
static void telemetrySenderResetState(void)
{
data = 0;
bytesLastPayload = 0;
currentOffset = 0;
currentPackage = 0;
length = 0;
currentPackage = 1;
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)
void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit)
{
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;
}
@ -112,33 +106,39 @@ bool isTelemetrySenderActive(void)
return senderState != ELRS_SENDER_IDLE;
}
void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData)
/***
* Copy up to maxLen bytes from the current package to outData
* packageIndex
***/
uint8_t getCurrentTelemetryPayload(uint8_t *outData)
{
uint8_t packageIndex;
bytesLastPayload = 0;
switch (senderState) {
case ELRS_RESYNC:
case ELRS_RESYNC_THEN_SEND:
*packageIndex = ELRS_TELEMETRY_MAX_PACKAGES;
*count = 0;
*currentData = 0;
packageIndex = ELRS_TELEMETRY_MAX_PACKAGES;
break;
case ELRS_SENDING:
*currentData = data + currentOffset;
*packageIndex = currentPackage;
if (bytesPerCall > 1) {
if (currentOffset + bytesPerCall <= length) {
*count = bytesPerCall;
} else {
*count = length - currentOffset;
bytesLastPayload = MIN((uint8_t)(length - currentOffset), ELRS_TELEMETRY_BYTES_PER_CALL);
for (unsigned n = 0; n < bytesLastPayload; ++n)
{
outData[n] = data[currentOffset + n];
}
// If this is the last data chunk, and there has been at least one other packet
// skip the blank packet needed for WAIT_UNTIL_NEXT_CONFIRM
if (currentPackage > 1 && (currentOffset + bytesLastPayload) >= length) {
packageIndex = 0;
} else {
*count = 1;
packageIndex = currentPackage;
}
break;
default:
*count = 0;
*currentData = 0;
*packageIndex = 0;
packageIndex = 0;
}
return packageIndex;
}
void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
@ -156,15 +156,20 @@ void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
break;
}
currentOffset += bytesPerCall;
currentOffset += bytesLastPayload;
if (currentOffset >= length) {
// A 0th packet is always requred so the reciver can
// differentiate a new send from a resend, if this is
// the first packet acked, send another, else IDLE
if (currentPackage == 1)
nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM;
else
nextSenderState = ELRS_SENDER_IDLE;
}
currentPackage++;
waitUntilTelemetryConfirm = !waitUntilTelemetryConfirm;
waitCount = 0;
if (currentOffset >= length) {
nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM;
}
break;
case ELRS_RESYNC:
@ -191,10 +196,9 @@ void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
}
#ifdef USE_MSP_OVER_TELEMETRY
static uint8_t *mspData;
static uint8_t *mspData = NULL;
static volatile bool finishedData;
static volatile uint8_t mspLength;
static volatile uint8_t mspBytesPerCall;
static volatile uint8_t mspLength = 0;
static volatile uint8_t mspCurrentOffset;
static volatile uint8_t mspCurrentPackage;
static volatile bool mspConfirm;
@ -203,11 +207,8 @@ 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;
mspCurrentPackage = 1;
mspConfirm = false;
mspReplyPending = false;
deviceInfoReplyPending = false;
@ -218,24 +219,18 @@ bool getCurrentMspConfirm(void)
return mspConfirm;
}
void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc)
void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive)
{
mspLength = maxLength;
mspData = dataToReceive;
mspCurrentPackage = 1;
mspCurrentOffset = 0;
finishedData = false;
mspBytesPerCall = bpc;
}
void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData)
void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* const receiveData)
{
if (packageIndex == 0 && mspCurrentPackage > 1) {
finishedData = true;
mspConfirm = !mspConfirm;
return;
}
// Resync
if (packageIndex == ELRS_MSP_MAX_PACKAGES) {
mspConfirm = !mspConfirm;
mspCurrentPackage = 1;
@ -248,17 +243,23 @@ void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveD
return;
}
if (packageIndex == mspCurrentPackage) {
for (uint8_t i = 0; i < mspBytesPerCall; i++) {
mspData[mspCurrentOffset++] = *(receiveData + i);
}
bool acceptData = false;
if (packageIndex == 0 && mspCurrentPackage > 1) {
// PackageIndex 0 (the final packet) can also contain data
acceptData = true;
finishedData = true;
} else if (packageIndex == mspCurrentPackage) {
acceptData = true;
mspCurrentPackage++;
mspConfirm = !mspConfirm;
return;
}
return;
if (acceptData) {
uint8_t len = MIN((uint8_t)(mspLength - mspCurrentOffset), ELRS_MSP_BYTES_PER_CALL);
for (unsigned i = 0; i < len; i++) {
mspData[mspCurrentOffset++] = receiveData[i];
}
mspConfirm = !mspConfirm;
}
}
bool hasFinishedMspData(void)

8
src/main/rx/expresslrs_telemetry.h
View file

@ -45,16 +45,16 @@ typedef enum {
void initTelemetry(void);
bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData);
void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc);
void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit);
bool isTelemetrySenderActive(void);
void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData);
uint8_t getCurrentTelemetryPayload(uint8_t *outData);
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);
void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive);
void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* const receiveData);
bool hasFinishedMspData(void);
void mspReceiverUnlock(void);
void processMspPacket(uint8_t *packet);