1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-23 16:25:16 +03:00

Multi telemetry : add Hitec and improve FlySky (#6835)

* Add Hitec and update Flysky telemetry

* Travis (and sky9x) doesn't like  cheating !

* Cosmetics
This commit is contained in:
3djc 2019-09-28 13:31:02 +02:00 committed by GitHub
parent 97743ed2f6
commit b8fa8438ba
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
21 changed files with 1001 additions and 152 deletions

View file

@ -21,7 +21,8 @@
#include "telemetry.h"
#include "multi.h"
enum MultiPacketTypes : uint8_t {
enum MultiPacketTypes : uint8_t
{
MultiStatus = 1,
FrSkySportTelemtry,
FrSkyHubTelemetry,
@ -30,10 +31,14 @@ enum MultiPacketTypes : uint8_t {
FlyskyIBusTelemetry,
ConfigCommand,
InputSync,
FrskySportPolling
FrskySportPolling,
HitecTelemetry,
SpectrumScannerPacket,
FlyskyIBusTelemetryAC
};
enum MultiBufferState : uint8_t {
enum MultiBufferState : uint8_t
{
NoProtocolDetected,
MultiFirstByteReceived,
ReceivingMultiProtocol,
@ -43,24 +48,25 @@ enum MultiBufferState : uint8_t {
FrskyTelemetryFallbackFirstByte,
FrskyTelemetryFallbackNextBytes,
FlyskyTelemetryFallback,
HitecTelemetryFallback,
MultiStatusOrFrskyData
};
#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 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)
MultiModuleStatus &getMultiModuleStatus(uint8_t module)
{
return multiModuleStatus[module];
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module)
MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
{
return multiSyncStatus[module];
}
@ -140,9 +146,9 @@ static MultiBufferState guessProtocol(uint8_t module)
return FrskyTelemetryFallback;
}
static void processMultiStatusPacket(const uint8_t *data, uint8_t module)
static void processMultiStatusPacket(const uint8_t * data, uint8_t module)
{
MultiModuleStatus& status = getMultiModuleStatus(module);
MultiModuleStatus &status = getMultiModuleStatus(module);
// At least two status packets without bind flag
bool wasBinding = status.isBinding();
@ -158,9 +164,9 @@ static void processMultiStatusPacket(const uint8_t *data, uint8_t module)
setMultiBindStatus(module, MULTI_BIND_FINISHED);
}
static void processMultiSyncPacket(const uint8_t *data, uint8_t module)
static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
{
MultiModuleSyncStatus& status = getMultiSyncStatus(module);
MultiModuleSyncStatus &status = getMultiSyncStatus(module);
status.lastUpdate = get_tmr10ms();
status.interval = data[4];
@ -175,17 +181,17 @@ static void processMultiSyncPacket(const uint8_t *data, uint8_t module)
#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",
module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
status.inputLag, oldlag-status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate/50,
status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
status.getAdjustedRefreshRate());
#endif
}
static void processMultiTelemetryPaket(const uint8_t *packet, uint8_t module)
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) {
@ -215,6 +221,20 @@ static void processMultiTelemetryPaket(const uint8_t *packet, uint8_t module)
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);
@ -257,7 +277,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet, uint8_t module)
// 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++;
@ -277,8 +297,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
@ -289,7 +309,7 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
adjustedRefreshRate /= 2;
// Our refresh rate in ps
adjustedRefreshRate*=1000;
adjustedRefreshRate *= 1000;
return;
}
@ -297,16 +317,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;
@ -314,20 +334,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;
@ -335,9 +356,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;
@ -349,17 +370,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;
@ -369,12 +389,12 @@ 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(PCBTARANIS) || defined(PCBHORUS)
@ -384,7 +404,7 @@ void MultiModuleStatus::getStatusString(char *statusText)
else
#endif
#endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
return;
}
if (!protocolValid()) {
@ -399,7 +419,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;
}
@ -419,7 +439,7 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcat(statusText, STR_MODULE_BINDING);
}
static uint8_t* getRxBuffer(uint8_t moduleIdx)
static uint8_t * getRxBuffer(uint8_t moduleIdx)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
@ -428,7 +448,7 @@ static uint8_t* getRxBuffer(uint8_t moduleIdx)
return telemetryRxBuffer;
}
static uint8_t& getRxBufferCount(uint8_t moduleIdx)
static uint8_t &getRxBufferCount(uint8_t moduleIdx)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
@ -439,8 +459,8 @@ static uint8_t& getRxBufferCount(uint8_t moduleIdx)
static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
{
uint8_t* rxBuffer = getRxBuffer(module);
uint8_t& rxBufferCount = getRxBufferCount(module);
uint8_t * rxBuffer = getRxBuffer(module);
uint8_t &rxBufferCount = getRxBufferCount(module);
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
@ -470,8 +490,8 @@ static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
void processMultiTelemetryData(uint8_t data, uint8_t module)
{
uint8_t* rxBuffer = getRxBuffer(module);
uint8_t& rxBufferCount = getRxBufferCount(module);
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)) {
@ -523,7 +543,7 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
break;
case SpektrumTelemetryFallback:
processSpektrumTelemetryData(module, data,rxBuffer, rxBufferCount);
processSpektrumTelemetryData(module, data, rxBuffer, rxBufferCount);
if (rxBufferCount == 0) {
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
@ -565,15 +585,15 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
case ReceivingMultiStatus:
rxBuffer[rxBufferCount++] = data;
if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount-1) {
processMultiStatusPacket(rxBuffer+1, module);
if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount - 1) {
processMultiStatusPacket(rxBuffer + 1, module);
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
if (rxBufferCount > 10) {
// too long ignore
TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]);
rxBufferCount =0;
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;