mixer scheduler

This commit is contained in:
Raphael Coeffic
2020-12-09 07:24:27 +01:00
parent 17feb48d9d
commit 02660c763a
32 changed files with 644 additions and 314 deletions
+20
View File
@@ -184,6 +184,26 @@ void processCrossfireTelemetryFrame()
break;
}
case RADIO_ID:
if (telemetryRxBuffer[3] == 0xEA // radio address
&& telemetryRxBuffer[5] == 0x10 // timing correction frame
) {
uint32_t update_interval;
int32_t offset;
if (getCrossfireTelemetryValue<4>(6, (int32_t&)update_interval) && getCrossfireTelemetryValue<4>(10, offset)) {
// values are in 10th of micro-seconds
update_interval /= 10;
offset /= 10;
TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset);
getModuleSyncStatus(EXTERNAL_MODULE).update(update_interval, offset + SAFE_SYNC_LAG);
}
}
break;
#if defined(LUA)
default:
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {
+3 -3
View File
@@ -99,15 +99,15 @@ const uint32_t CROSSFIRE_BAUDRATES[] = {
115200,
};
const uint8_t CROSSFIRE_PERIODS[] = {
4,
4,
16,
};
#if SPORT_MAX_BAUDRATE < 400000
#define CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[g_eeGeneral.telemetryBaudrate]
#define CROSSFIRE_PERIOD CROSSFIRE_PERIODS[g_eeGeneral.telemetryBaudrate]
#define CROSSFIRE_PERIOD (CROSSFIRE_PERIODS[g_eeGeneral.telemetryBaudrate]*1000)
#else
#define CROSSFIRE_BAUDRATE 400000
#define CROSSFIRE_PERIOD 4 // 4ms
#define CROSSFIRE_PERIOD 6666 /* us; 150 Hz */
#endif
#define CROSSFIRE_TELEM_MIRROR_BAUDRATE 115200
+8 -125
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()) {
-22
View File
@@ -94,28 +94,6 @@ void processMultiTelemetryData(uint8_t data, uint8_t module);
#define MULTI_SCANNER_MAX_CHANNEL 249
// This should be put into the Module definition if other modules gain this functionality
struct MultiModuleSyncStatus {
uint32_t adjustedRefreshRate = 9000 * 1000; // in ps
tmr10ms_t lastUpdate;
uint16_t refreshRate;
uint16_t inputLag;
uint8_t interval;
uint8_t target;
inline bool isValid() const
{
return (get_tmr10ms() - lastUpdate < 100);
}
void getRefreshString(char * refreshText);
const uint16_t getAdjustedRefreshRate();
void calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag);
};
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module);
struct MultiModuleStatus {
uint8_t major;
+86
View File
@@ -21,6 +21,7 @@
#include "opentx.h"
#include "multi.h"
#include "pulses/afhds3.h"
#include "mixer_scheduler.h"
uint8_t telemetryStreaming = 0;
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
@@ -374,3 +375,88 @@ OutputTelemetryBuffer outputTelemetryBuffer __DMA;
#if defined(LUA)
Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
#endif
#if defined(HARDWARE_INTERNAL_MODULE)
static ModuleSyncStatus moduleSyncStatus[NUM_MODULES];
ModuleSyncStatus &getModuleSyncStatus(uint8_t moduleIdx)
{
return moduleSyncStatus[moduleIdx];
}
#else
static ModuleSyncStatus moduleSyncStatus;
ModuleSyncStatus &getModuleSyncStatus(uint8_t moduleIdx)
{
return moduleSyncStatus;
}
#endif
ModuleSyncStatus::ModuleSyncStatus()
{
memset(this, 0, sizeof(ModuleSyncStatus));
}
void ModuleSyncStatus::update(uint16_t newRefreshRate, uint16_t newInputLag)
{
if (!newRefreshRate)
return;
if (newRefreshRate < MIN_REFRESH_RATE)
newRefreshRate = newRefreshRate * (MIN_REFRESH_RATE / (newRefreshRate + 1));
else if (newRefreshRate > MAX_REFRESH_RATE)
newRefreshRate = MAX_REFRESH_RATE;
refreshRate = newRefreshRate;
inputLag = newInputLag;
currentLag = newInputLag;
lastUpdate = get_tmr10ms();
}
uint16_t ModuleSyncStatus::getAdjustedRefreshRate()
{
int16_t lag = currentLag - SAFE_SYNC_LAG;
int32_t newRefreshRate = refreshRate;
if (lag == 0) {
return refreshRate;
}
newRefreshRate += lag;
if (newRefreshRate < MIN_REFRESH_RATE) {
newRefreshRate = MIN_REFRESH_RATE;
}
else if (newRefreshRate > MAX_REFRESH_RATE) {
newRefreshRate = MAX_REFRESH_RATE;
}
TRACE("[SYNC] rate = %dus",newRefreshRate);
currentLag -= newRefreshRate - refreshRate;
return (uint16_t)newRefreshRate;
}
void ModuleSyncStatus::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) (refreshRate / 1000), 5);
tmp = strAppend(tmp, "us");
#else
tmp = strAppend(tmp, "Sync at ");
tmp = strAppendUnsigned(tmp, (uint32_t) (refreshRate / 1000000));
tmp = strAppend(tmp, " ms");
#endif
}
+31
View File
@@ -289,4 +289,35 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
void processPXX2Frame(uint8_t module, const uint8_t *frame);
// Module pulse synchronization
#define SAFE_SYNC_LAG 800 /* us */
struct ModuleSyncStatus
{
// feedback input: last received values
uint16_t refreshRate; // in us
int16_t inputLag; // in us
tmr10ms_t lastUpdate; // in 10ms
int16_t currentLag; // in us
inline bool isValid() {
// 2 seconds
return (get_tmr10ms() - lastUpdate < 200);
}
// Set feedback from RF module
void update(uint16_t newRefreshRate, uint16_t newInputLag);
// Get computed settings for scheduler
uint16_t getAdjustedRefreshRate();
// Status string for the UI
void getRefreshString(char* refreshText);
ModuleSyncStatus();
};
ModuleSyncStatus& getModuleSyncStatus(uint8_t moduleIdx);
#endif // _TELEMETRY_H_