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

mixer scheduler

Browse source
This commit is contained in:
Raphael Coeffic 2019-10-19 23:08:51 +02:00
parent 17feb48d9d
commit 02660c763a
32 changed files with 644 additions and 314 deletions
Download patch file Download diff file Expand all files Collapse all files

133
radio/src/telemetry/multi.cpp
View file

@ -63,7 +63,6 @@ enum MultiBufferState : uint8_t
#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];
@ -74,11 +73,6 @@ MultiModuleStatus &getMultiModuleStatus(uint8_t module)
return multiModuleStatus[module];
}
MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
{
return multiSyncStatus[module];
}
uint8_t getMultiBindStatus(uint8_t module)
{
return multiBindStatus[module];
@ -111,7 +105,6 @@ uint8_t intTelemetryRxBufferCount;
#else // !INTERNAL_MODULE_MULTI
static MultiModuleStatus multiModuleStatus;
static MultiModuleSyncStatus multiSyncStatus;
static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
static MultiBufferState multiTelemetryBufferState;
@ -122,11 +115,6 @@ MultiModuleStatus& getMultiModuleStatus(uint8_t)
return multiModuleStatus;
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
{
return multiSyncStatus;
}
uint8_t getMultiBindStatus(uint8_t)
{
return multiBindStatus;
@ -254,24 +242,17 @@ static void processMultiStatusPacket(const uint8_t * data, uint8_t module, uint8
static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
{
MultiModuleSyncStatus &status = getMultiSyncStatus(module);
ModuleSyncStatus &status = getModuleSyncStatus(module);
status.lastUpdate = get_tmr10ms();
status.interval = data[4];
status.target = data[5];
#if !defined(PPM_PIN_SERIAL)
auto oldlag = status.inputLag;
(void) oldlag;
#endif
uint16_t refreshRate = data[0] << 8 | data[1];
int16_t inputLag = data[2] << 8 | data[3];
status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
// if (inputLag > refreshRate/2)
// inputLag -= refreshRate;
#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.getAdjustedRefreshRate());
#endif
status.update(refreshRate, inputLag);
serialPrint("MP ADJ: R %d, L %04d", refreshRate, inputLag);
}
#if defined(PCBTARANIS) || defined(PCBHORUS)
@ -434,104 +415,6 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
}
}
#define MIN_REFRESH_RATE 5500
void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag)
{
// Check how far off we are from our target, positive means we are too slow, negative we are too fast
int lagDifference = newInputLag - inputLag;
// The refresh rate that we target
// Below is least common multiple of MIN_REFRESH_RATE and requested rate
uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
// Overflow, reverse sample
if (lagDifference < -targetRefreshRate / 2)
lagDifference = -lagDifference;
// Reset adjusted refresh if rate has changed
if (newRefreshRate != refreshRate) {
refreshRate = newRefreshRate;
adjustedRefreshRate = targetRefreshRate;
if (adjustedRefreshRate >= 30000)
adjustedRefreshRate /= 2;
// Our refresh rate in ps
adjustedRefreshRate *= 1000;
return;
}
// Caluclate how many samples went into the reported input Lag (*10)
int numsamples = interval * 10000 / targetRefreshRate;
// Convert lagDifference to ps
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;
// Caculate the time in ps each frame is to slow (positive), fast(negative)
int perframeps = lagDifference * 10 / numsamples;
if (perframeps > 20000)
perframeps = 20000;
if (perframeps < -20000)
perframeps = -20000;
adjustedRefreshRate = (adjustedRefreshRate + perframeps);
// Safeguards
if (adjustedRefreshRate < MIN_REFRESH_RATE * 1000)
adjustedRefreshRate = MIN_REFRESH_RATE * 1000;
if (adjustedRefreshRate > 30 * 1000 * 1000)
adjustedRefreshRate = 30 * 1000 * 1000;
inputLag = newInputLag;
}
static uint8_t counter;
const uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate()
{
if (!isValid() || refreshRate == 0)
return 18000;
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)
return (uint16_t) (rate + 1);
else
return rate;
}
void MultiModuleSyncStatus::getRefreshString(char * statusText)
{
if (!isValid()) {
return;
}
char * tmp = statusText;
#if defined(DEBUG)
*tmp++ = 'L';
tmp = strAppendUnsigned(tmp, inputLag, 5);
tmp = strAppend(tmp, "us R ");
tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000), 5);
tmp = strAppend(tmp, "us");
#else
tmp = strAppend(tmp, "Sync at ");
tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000000));
tmp = strAppend(tmp, " ms");
#endif
}
void MultiModuleStatus::getStatusString(char * statusText) const
{
if (!isValid()) {