mixer scheduler
This commit is contained in:
@@ -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) ) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()) {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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_
|
||||
|
||||
Reference in New Issue
Block a user