piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Merge with latest 2.3

Browse Source
This commit is contained in:
Bertrand Songis
2019-11-07 15:04:45 +01:00
parent 579aa03cd1 50a07dc8d8
commit 071116afeb
286 changed files with 9956 additions and 10363 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/multi.cpp
+358 -123
View File
@@ -21,24 +21,29 @@
#include "telemetry.h"
#include "multi.h"
MultiModuleStatus multiModuleStatus;
MultiModuleSyncStatus multiSyncStatus;
uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
#define MULTI_CHAN_BITS 11
extern uint8_t g_moduleIdx;
enum MultiPacketTypes : uint8_t {
enum MultiPacketTypes : uint8_t
{
MultiStatus = 1,
FrSkySportTelemtry,
FrSkySportTelemetry,
FrSkyHubTelemetry,
SpektrumTelemetry,
DSMBindPacket,
FlyskyIBusTelemetry,
ConfigCommand,
InputSync,
FrskySportPolling
FrskySportPolling,
HitecTelemetry,
SpectrumScannerPacket,
FlyskyIBusTelemetryAC,
MultiRxChannels
};
enum MultiBufferState : uint8_t {
enum MultiBufferState : uint8_t
{
NoProtocolDetected,
MultiFirstByteReceived,
ReceivingMultiProtocol,
@@ -48,71 +53,248 @@ enum MultiBufferState : uint8_t {
FrskyTelemetryFallbackFirstByte,
FrskyTelemetryFallbackNextBytes,
FlyskyTelemetryFallback,
HitecTelemetryFallback,
MultiStatusOrFrskyData
};
MultiBufferState guessProtocol()
#if defined(INTERNAL_MODULE_MULTI)
static MultiModuleStatus multiModuleStatus[NUM_MODULES] = {MultiModuleStatus(), MultiModuleStatus()};
static MultiModuleSyncStatus multiSyncStatus[NUM_MODULES] = {MultiModuleSyncStatus(), MultiModuleSyncStatus()};
static uint8_t multiBindStatus[NUM_MODULES] = {MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION};
static MultiBufferState multiTelemetryBufferState[NUM_MODULES];
MultiModuleStatus &getMultiModuleStatus(uint8_t module)
{
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2)
return multiModuleStatus[module];
}
MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
{
return multiSyncStatus[module];
}
uint8_t getMultiBindStatus(uint8_t module)
{
return multiBindStatus[module];
}
void setMultiBindStatus(uint8_t module, uint8_t bindStatus)
{
multiBindStatus[module] = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t module)
{
return multiTelemetryBufferState[module];
}
void setMultiTelemetryBufferState(uint8_t module, MultiBufferState state)
{
multiTelemetryBufferState[module] = state;
}
// Use additional telemetry buffer
uint8_t intTelemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
uint8_t intTelemetryRxBufferCount;
#else // !INTERNAL_MODULE_MULTI
static MultiModuleStatus multiModuleStatus;
static MultiModuleSyncStatus multiSyncStatus;
static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
static MultiBufferState multiTelemetryBufferState;
MultiModuleStatus& getMultiModuleStatus(uint8_t)
{
return multiModuleStatus;
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
{
return multiSyncStatus;
}
uint8_t getMultiBindStatus(uint8_t)
{
return multiBindStatus;
}
void setMultiBindStatus(uint8_t, uint8_t bindStatus)
{
multiBindStatus = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t)
{
return multiTelemetryBufferState;
}
void setMultiTelemetryBufferState(uint8_t, MultiBufferState state)
{
multiTelemetryBufferState = state;
}
#endif // INTERNAL_MODULE_MULTI
static MultiBufferState guessProtocol(uint8_t module)
{
uint32_t moduleIdx = EXTERNAL_MODULE;
#if defined(INTERNAL_MODULE_MULTI)
if (isModuleMultimodule(INTERNAL_MODULE)) {
moduleIdx = INTERNAL_MODULE;
}
#endif
if (g_model.moduleData[moduleIdx].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2)
return SpektrumTelemetryFallback;
else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
else if (g_model.moduleData[module].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
return FlyskyTelemetryFallback;
else
return FrskyTelemetryFallback;
}
static void processMultiStatusPacket(const uint8_t *data)
static void processMultiScannerPacket(const uint8_t *data)
{
// At least two status packets without bind flag
bool wasBinding = (multiModuleStatus.isBinding());
uint8_t cur_channel = data[0];
if (moduleState[g_moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
for (uint8_t channel = 0; channel <5; channel++) {
uint8_t power = max<int>(0,(data[channel+1] - 34) >> 1); // remove everything below -120dB
multiModuleStatus.flags = data[0];
multiModuleStatus.major = data[1];
multiModuleStatus.minor = data[2];
multiModuleStatus.revision = data[3];
multiModuleStatus.patch = data[4];
multiModuleStatus.lastUpdate = get_tmr10ms();
if (wasBinding && !multiModuleStatus.isBinding() && multiBindStatus == MULTI_BIND_INITIATED)
multiBindStatus = MULTI_BIND_FINISHED;
#if LCD_W == 480
coord_t x = cur_channel*2;
if (x < LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
reusableBuffer.spectrumAnalyser.bars[x+1] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
reusableBuffer.spectrumAnalyser.max[x+1] = power;
}
#elif LCD_W == 212
coord_t x = cur_channel;
if (x <= LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
}
#else
coord_t x = cur_channel/2 + 1;
if (x <= LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
}
#endif
}
if (++cur_channel > MULTI_SCANNER_MAX_CHANNEL)
cur_channel = 0;
}
}
}
static void processMultiSyncPacket(const uint8_t *data)
static void processMultiStatusPacket(const uint8_t * data, uint8_t module)
{
multiSyncStatus.lastUpdate = get_tmr10ms();
multiSyncStatus.interval = data[4];
multiSyncStatus.target = data[5];
MultiModuleStatus &status = getMultiModuleStatus(module);
// At least two status packets without bind flag
bool wasBinding = status.isBinding();
status.flags = data[0];
status.major = data[1];
status.minor = data[2];
status.revision = data[3];
status.patch = data[4];
status.lastUpdate = get_tmr10ms();
if (getMultiModuleStatus(module).requiresFailsafeCheck) {
getMultiModuleStatus(module).requiresFailsafeCheck = false;
if (getMultiModuleStatus(module).supportsFailsafe() && g_model.moduleData[module].failsafeMode == FAILSAFE_NOT_SET)
POPUP_WARNING(STR_NO_FAILSAFE);
}
if (wasBinding && !status.isBinding() && getMultiBindStatus(module) == MULTI_BIND_INITIATED)
setMultiBindStatus(module, MULTI_BIND_FINISHED);
}
static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
{
MultiModuleSyncStatus &status = getMultiSyncStatus(module);
status.lastUpdate = get_tmr10ms();
status.interval = data[4];
status.target = data[5];
#if !defined(PPM_PIN_SERIAL)
auto oldlag = multiSyncStatus.inputLag;
auto oldlag = status.inputLag;
(void) oldlag;
#endif
multiSyncStatus.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
#if !defined(PPM_PIN_SERIAL)
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n", extmodulePulsesData.dsm2.rest,
multiSyncStatus.inputLag, oldlag-multiSyncStatus.inputLag, multiSyncStatus.target, multiSyncStatus.interval, multiSyncStatus.refreshRate, multiSyncStatus.adjustedRefreshRate/50,
multiSyncStatus.getAdjustedRefreshRate());
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
status.getAdjustedRefreshRate());
#endif
}
#if defined(PCBTARANIS) || defined(PCBHORUS)
static void processMultiRxChannels(const uint8_t * data, uint8_t len)
{
if (g_model.trainerData.mode != TRAINER_MODE_MULTI)
return;
static void processMultiTelemetryPaket(const uint8_t *packet)
//uint8_t pps = data[0];
//uint8_t rssi = data[1];
int ch = max(data[2], (uint8_t)0);
int maxCh = min(ch + data[3], MAX_TRAINER_CHANNELS);
uint32_t bits = 0;
uint8_t bitsavailable = 0;
uint8_t byteIdx = 4;
while (ch < maxCh) {
while (bitsavailable < MULTI_CHAN_BITS && byteIdx < len) {
bits |= (uint32_t)(data[byteIdx++]) << (uint32_t)bitsavailable;
bitsavailable += 8;
}
int value = bits & ((1 << MULTI_CHAN_BITS) - 1);
bitsavailable -= MULTI_CHAN_BITS;
bits >>= MULTI_CHAN_BITS;
ppmInput[ch] = (value - 1024) * 500 / 800;
ch++;
if (byteIdx >= len)
break;
}
if (ch == maxCh)
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
}
#endif
static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
{
uint8_t type = packet[0];
uint8_t len = packet[1];
const uint8_t *data = packet + 2;
const uint8_t * data = packet + 2;
// Switch type
switch (type) {
case MultiStatus:
if (len >= 5)
processMultiStatusPacket(data);
processMultiStatusPacket(data, module);
break;
case DSMBindPacket:
if (len >= 10)
processDSMBindPacket(data);
processDSMBindPacket(module, data);
break;
case SpektrumTelemetry:
@@ -131,6 +313,20 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
TRACE("[MP] Received IBUS telemetry len %d < 28", len);
break;
case FlyskyIBusTelemetryAC:
if (len >= 28)
processFlySkyPacketAC(data);
else
TRACE("[MP] Received IBUS telemetry AC len %d < 28", len);
break;
case HitecTelemetry:
if (len >= 8)
processHitecPacket(data);
else
TRACE("[MP] Received Hitec telemetry len %d < 8", len);
break;
case FrSkyHubTelemetry:
if (len >= 4)
frskyDProcessPacket(data);
@@ -138,7 +334,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
TRACE("[MP] Received Frsky HUB telemetry len %d < 4", len);
break;
case FrSkySportTelemtry:
case FrSkySportTelemetry:
if (len >= 4)
sportProcessTelemetryPacket(data);
else
@@ -147,7 +343,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
case InputSync:
if (len >= 6)
processMultiSyncPacket(data);
processMultiSyncPacket(data, module);
else
TRACE("[MP] Received input sync len %d < 6", len);
break;
@@ -164,6 +360,21 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
}
break;
#endif
case SpectrumScannerPacket:
if (len == 6)
processMultiScannerPacket(data);
else
TRACE("[MP] Received spectrum scanner len %d != 6", len);
break;
#if defined(PCBTARANIS) || defined(PCBHORUS)
case MultiRxChannels:
if (len >= 4)
processMultiRxChannels(data, len);
else
TRACE("[MP] Received RX channels len %d < 4", len);
break;
#endif
default:
TRACE("[MP] Unkown multi packet type 0x%02X, len %d", type, len);
@@ -173,7 +384,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
// sprintf does not work AVR ARM
// use a small helper function
static void appendInt(char *buf, uint32_t val)
static void appendInt(char * buf, uint32_t val)
{
while (*buf)
buf++;
@@ -193,8 +404,8 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
// Overflow, reverse sample
if (lagDifference < -targetRefreshRate/2)
lagDifference= -lagDifference;
if (lagDifference < -targetRefreshRate / 2)
lagDifference = -lagDifference;
// Reset adjusted refresh if rate has changed
@@ -205,7 +416,7 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
adjustedRefreshRate /= 2;
// Our refresh rate in ps
adjustedRefreshRate*=1000;
adjustedRefreshRate *= 1000;
return;
}
@@ -213,16 +424,16 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
int numsamples = interval * 10000 / targetRefreshRate;
// Convert lagDifference to ps
lagDifference=lagDifference*1000;
lagDifference = lagDifference * 1000;
// Calculate the time we intentionally were late/early
if (inputLag > target*10 +30)
lagDifference += numsamples*500;
else if (inputLag < target*10 - 30)
lagDifference -= numsamples*500;
if (inputLag > target * 10 + 30)
lagDifference += numsamples * 500;
else if (inputLag < target * 10 - 30)
lagDifference -= numsamples * 500;
// Caculate the time in ps each frame is to slow (positive), fast(negative)
int perframeps = lagDifference*10/ numsamples;
int perframeps = lagDifference * 10 / numsamples;
if (perframeps > 20000)
perframeps = 20000;
@@ -230,20 +441,21 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
if (perframeps < -20000)
perframeps = -20000;
adjustedRefreshRate =(adjustedRefreshRate + perframeps);
adjustedRefreshRate = (adjustedRefreshRate + perframeps);
// Safeguards
if (adjustedRefreshRate < 6*1000*1000)
adjustedRefreshRate = 6*1000*1000;
if (adjustedRefreshRate > 30*1000*1000)
adjustedRefreshRate = 30*1000*1000;
if (adjustedRefreshRate < 6 * 1000 * 1000)
adjustedRefreshRate = 6 * 1000 * 1000;
if (adjustedRefreshRate > 30 * 1000 * 1000)
adjustedRefreshRate = 30 * 1000 * 1000;
inputLag = newInputLag;
}
static uint8_t counter;
uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate() {
uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate()
{
if (!isValid() || refreshRate == 0)
return 18000;
@@ -251,9 +463,9 @@ uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate() {
counter = (uint8_t) (counter + 1 % 10);
uint16_t rate = (uint16_t) ((adjustedRefreshRate + counter * 50) / 500);
// Check how far off we are from our target, positive means we are too slow, negative we are too fast
if (inputLag > target*10 +30)
return (uint16_t) (rate - 1);
else if (inputLag < target*10 - 30)
if (inputLag > target * 10 + 30)
return (uint16_t) (rate - 1);
else if (inputLag < target * 10 - 30)
return (uint16_t) (rate + 1);
else
return rate;
@@ -265,17 +477,16 @@ static void prependSpaces(char * buf, int val)
while (*buf)
buf++;
int k=10000;
while(val/k==0 && k > 0)
{
*buf=' ';
int k = 10000;
while (val / k == 0 && k > 0) {
*buf = ' ';
buf++;
k/= 10;
k /= 10;
}
*buf='\0';
*buf = '\0';
}
void MultiModuleSyncStatus::getRefreshString(char *statusText)
void MultiModuleSyncStatus::getRefreshString(char * statusText)
{
if (!isValid()) {
return;
@@ -285,20 +496,22 @@ void MultiModuleSyncStatus::getRefreshString(char *statusText)
prependSpaces(statusText, inputLag);
appendInt(statusText, inputLag);
strcat(statusText, "ns R ");
prependSpaces(statusText, adjustedRefreshRate/1000);
prependSpaces(statusText, adjustedRefreshRate / 1000);
appendInt(statusText, (uint32_t) (adjustedRefreshRate / 1000));
strcat(statusText, "ns");
}
void MultiModuleStatus::getStatusString(char *statusText)
void MultiModuleStatus::getStatusString(char * statusText)
{
if (!isValid()) {
#if defined(PCBFRSKY)
if (IS_INTERNAL_MODULE_ENABLED())
#if !defined(INTERNAL_MODULE_MULTI)
if (isSportLineUsedByInternalModule())
strcpy(statusText, STR_DISABLE_INTERNAL);
else
#endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
#endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
return;
}
if (!protocolValid()) {
@@ -313,7 +526,7 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcpy(statusText, STR_MODULE_NO_INPUT);
return;
}
else if(isWaitingforBind()) {
else if (isWaitingforBind()) {
strcpy(statusText, STR_MODULE_WAITFORBIND);
return;
}
@@ -333,50 +546,71 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcat(statusText, STR_MODULE_BINDING);
}
static MultiBufferState multiTelemetryBufferState;
static void processMultiTelemetryByte(const uint8_t data)
static uint8_t * getRxBuffer(uint8_t moduleIdx)
{
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBuffer;
#endif
return telemetryRxBuffer;
}
static uint8_t &getRxBufferCount(uint8_t moduleIdx)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBufferCount;
#endif
return telemetryRxBufferCount;
}
static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
{
uint8_t * rxBuffer = getRxBuffer(module);
uint8_t &rxBufferCount = getRxBufferCount(module);
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[MP] array size %d error", telemetryRxBufferCount);
multiTelemetryBufferState = NoProtocolDetected;
TRACE("[MP] array size %d error", rxBufferCount);
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
// Length field does not count the header
if (telemetryRxBufferCount >= 2 && telemetryRxBuffer[1] == telemetryRxBufferCount - 2) {
if (rxBufferCount >= 2 && rxBuffer[1] == rxBufferCount - 2) {
// debug print the content of the packet
#if 0
debugPrintf("[MP] Packet type %02X len 0x%02X: ",
telemetryRxBuffer[0], telemetryRxBuffer[1]);
for (int i=0; i<(telemetryRxBufferCount+3)/4; i++) {
debugPrintf("[%02X%02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3],
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]);
rxBuffer[0], rxBuffer[1]);
for (int i=0; i<(rxBufferCount+3)/4; i++) {
debugPrintf("[%02X%02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
}
debugPrintf("\r\n");
#endif
// Packet is complete, process it
processMultiTelemetryPaket(telemetryRxBuffer);
multiTelemetryBufferState = NoProtocolDetected;
processMultiTelemetryPaket(rxBuffer, module);
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
}
void processMultiTelemetryData(const uint8_t data)
void processMultiTelemetryData(uint8_t data, uint8_t module)
{
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, telemetryRxBufferCount);
switch (multiTelemetryBufferState) {
uint8_t * rxBuffer = getRxBuffer(module);
uint8_t &rxBufferCount = getRxBufferCount(module);
//debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, rxBufferCount);
switch (getMultiTelemetryBufferState(module)) {
case NoProtocolDetected:
if (data == 'M') {
multiTelemetryBufferState = MultiFirstByteReceived;
setMultiTelemetryBufferState(module, MultiFirstByteReceived);
}
else if (data == 0xAA || data == 0x7e) {
multiTelemetryBufferState = guessProtocol();
setMultiTelemetryBufferState(module, guessProtocol(module));
// Process the first byte by the protocol
processMultiTelemetryData(data);
processMultiTelemetryData(data, module);
}
else {
TRACE("[MP] invalid start byte 0x%02X", data);
@@ -384,91 +618,92 @@ void processMultiTelemetryData(const uint8_t data)
break;
case FrskyTelemetryFallback:
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
processFrskyTelemetryData(data);
break;
case FrskyTelemetryFallbackFirstByte:
if (data == 'M') {
multiTelemetryBufferState = MultiStatusOrFrskyData;
setMultiTelemetryBufferState(module, MultiStatusOrFrskyData);
}
else {
processFrskyTelemetryData(data);
if (data != 0x7e)
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
}
break;
case FrskyTelemetryFallbackNextBytes:
processFrskyTelemetryData(data);
if (data == 0x7e)
if (data == 0x7e) {
// end of packet or start of new packet
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
}
break;
case FlyskyTelemetryFallback:
processFlySkyTelemetryData(data);
if (telemetryRxBufferCount == 0)
multiTelemetryBufferState = NoProtocolDetected;
processFlySkyTelemetryData(data, rxBuffer, rxBufferCount);
if (rxBufferCount == 0) {
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case SpektrumTelemetryFallback:
processSpektrumTelemetryData(data);
if (telemetryRxBufferCount == 0)
multiTelemetryBufferState = NoProtocolDetected;
processSpektrumTelemetryData(module, data, rxBuffer, rxBufferCount);
if (rxBufferCount == 0) {
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case MultiFirstByteReceived:
telemetryRxBufferCount = 0;
rxBufferCount = 0;
if (data == 'P') {
multiTelemetryBufferState = ReceivingMultiProtocol;
setMultiTelemetryBufferState(module, ReceivingMultiProtocol);
}
else if (data >= 5 && data <= 10) {
// Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
// a bit of validation
multiTelemetryBufferState = ReceivingMultiStatus;
processMultiTelemetryData(data);
setMultiTelemetryBufferState(module, ReceivingMultiStatus);
processMultiTelemetryData(data, module);
}
else {
TRACE("[MP] invalid second byte 0x%02X", data);
multiTelemetryBufferState = NoProtocolDetected;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case ReceivingMultiProtocol:
processMultiTelemetryByte(data);
processMultiTelemetryByte(data, module);
break;
case MultiStatusOrFrskyData:
// Check len byte if it makes sense for multi
if (data >= 5 && data <= 10) {
multiTelemetryBufferState = ReceivingMultiStatus;
telemetryRxBufferCount = 0;
setMultiTelemetryBufferState(module, ReceivingMultiStatus);
rxBufferCount = 0;
}
else {
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
processMultiTelemetryData('M');
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
processMultiTelemetryData('M', module);
}
processMultiTelemetryData(data);
processMultiTelemetryData(data, module);
break;
case ReceivingMultiStatus:
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (telemetryRxBufferCount > 5 && telemetryRxBuffer[0] == telemetryRxBufferCount-1) {
processMultiStatusPacket(telemetryRxBuffer+1);
telemetryRxBufferCount = 0;
multiTelemetryBufferState = NoProtocolDetected;
rxBuffer[rxBufferCount++] = data;
if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount - 1) {
processMultiStatusPacket(rxBuffer + 1, module);
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
if (telemetryRxBufferCount > 10) {
if (rxBufferCount > 10) {
// too long ignore
TRACE("Overlong multi status packet detected ignoring, wanted %d", telemetryRxBuffer[0]);
telemetryRxBufferCount =0;
multiTelemetryBufferState = NoProtocolDetected;
TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]);
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
}
}