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

WiP

Browse source
This commit is contained in:
Raphael Coeffic 2019-11-06 16:53:59 +01:00 • committed by 3djc
parent b98c6bba69
commit b5da1990db
12 changed files with 192 additions and 153 deletions
Download patch file Download diff file Expand all files Collapse all files

2
radio/src/gui/128x64/model_setup.cpp
View file

@ -1628,7 +1628,7 @@ void menuModelSetup(event_t event)
lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
char statusText[64];
getMultiSyncStatus(moduleIdx).getRefreshString(statusText);
getModuleSyncStatus(moduleIdx).getRefreshString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break;
}

1
radio/src/gui/gui_common.h
View file

@ -161,6 +161,7 @@ inline uint8_t MODULE_CHANNELS_ROWS(int moduleIdx)
#if defined(MULTIMODULE)
inline uint8_t MULTI_DISABLE_CHAN_MAP_ROW(uint8_t moduleIdx)
{
if (!isModuleMultimodule(moduleIdx))

5
radio/src/pulses/pulses.cpp
View file

@ -273,8 +273,13 @@ void setupPulsesExternalModule(uint8_t protocol)
#if defined(MULTIMODULE)
case PROTOCOL_CHANNELS_MULTIMODULE:
{
ModuleSyncStatus& status = getModuleSyncStatus(EXTERNAL_MODULE);
if (status.isValid())
mixerSchedulerSetPeriod(EXTERNAL_MODULE, status.getAdjustedRefreshRate());
setupPulsesMultiExternalModule();
break;
}
#endif
#if defined(PPM)

2
radio/src/rtos.h
View file

@ -241,7 +241,7 @@ template<int SIZE>
#define RTOS_CREATE_FLAG(flag) flag = CoCreateFlag(false, false)
#define RTOS_SET_FLAG(flag) (void)CoSetFlag(flag)
#define RTOS_CLEAR_FLAG(flag) (void)CoClearFlag(flag)
#define RTOS_WAIT_FLAG(flag,timeout) (void)CoWaitForSingleFlag(flag,timeout)
#define RTOS_WAIT_FLAG(flag,timeout) CoWaitForSingleFlag(flag,timeout)
static inline void RTOS_ISR_SET_FLAG(RTOS_FLAG_HANDLE flag)
{

4
radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp
View file

@ -91,10 +91,10 @@ void mixerSchedulerDisableTrigger()
MIXER_SCHEDULER_TIMER->DIER &= ~TIM_DIER_UIE; // disable interrupt
}
void mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
{
RTOS_CLEAR_FLAG(mixerFlag);
RTOS_WAIT_FLAG(mixerFlag, timeoutMs);
return RTOS_WAIT_FLAG(mixerFlag, timeoutMs) == E_TIMEOUT;
}
void mixerScheduleISRTrigger()

3
radio/src/targets/common/arm/stm32/mixer_scheduler_driver.h
View file

@ -34,7 +34,8 @@ void mixerSchedulerStop();
void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs);
// Wait for the scheduler timer to trigger
void mixerSchedulerWaitForTrigger(uint8_t timeoutMs);
// returns true if timeout, false otherwise
bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs);
// Enable the timer trigger
void mixerSchedulerEnableTrigger();

3
radio/src/tasks.cpp
View file

@ -143,7 +143,7 @@ TASK_FUNCTION(mixerTask)
// - add trigger based on heartbeat driver
// run mixer at least every 10ms
mixerSchedulerWaitForTrigger(10);
bool timeout = mixerSchedulerWaitForTrigger(10);
#if defined(DEBUG_MIXER_SCHEDULER)
GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
@ -200,6 +200,7 @@ TASK_FUNCTION(mixerTask)
if (t0 > maxMixerDuration)
maxMixerDuration = t0;
//if (!timeout)
sendSynchronousPulses();
}
}

11
radio/src/telemetry/crossfire.cpp
View file

@ -178,6 +178,17 @@ void processCrossfireTelemetryFrame()
break;
}
// case RADIO_ID:
// {
// uint32_t update_interval, offset;
// if (getCrossfireTelemetryValue<4>(2, update_interval) && getCrossfireTelemetryValue<4>(2, offset)) {
// // values are in 10th of micro-seconds
// update_interval /= 10;
// offset /= 10;
// getModuleSyncStatus(EXTERNAL_MODULE).update(update_interval, offset);
// }
// }
#if defined(LUA)
default:
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {

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

@ -62,7 +62,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];
@ -73,11 +72,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];
@ -110,7 +104,6 @@ uint8_t intTelemetryRxBufferCount;
#else // !INTERNAL_MODULE_MULTI
static MultiModuleStatus multiModuleStatus;
static MultiModuleSyncStatus multiSyncStatus;
static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
static MultiBufferState multiTelemetryBufferState;
@ -121,11 +114,6 @@ MultiModuleStatus& getMultiModuleStatus(uint8_t)
return multiModuleStatus;
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
{
return multiSyncStatus;
}
uint8_t getMultiBindStatus(uint8_t)
{
return multiBindStatus;
@ -253,24 +241,41 @@ 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
// status.lastUpdate = get_tmr10ms();
// status.interval = data[4];
// status.target = data[5];
// #if !defined(PPM_PIN_SERIAL)
// auto oldlag = status.inputLag;
// (void) oldlag;
// #endif
status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
// uint16_t refreshRate = data[0] << 8 | data[1];
// status.calcAdjustedRefreshRate(refreshRate, 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",
module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
status.getAdjustedRefreshRate());
#endif
// serialPrint("MP ADJ: R %d, L %04d, T %03d, calc %04d",
// refreshRate,
// data[2] << 8 | data[3],
// status.target,
// status.getAdjustedRefreshRate()/2);
uint16_t refreshRate = data[0] << 8 | data[1];
int16_t inputLag = data[2] << 8 | data[3];
// if (inputLag > refreshRate/2)
// inputLag -= refreshRate;
status.update(refreshRate, inputLag);
serialPrint("MP ADJ: R %d, L %04d",
refreshRate, inputLag);
// #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
}
#if defined(PCBTARANIS) || defined(PCBHORUS)
@ -420,104 +425,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
radio/src/telemetry/multi.h
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;

107
radio/src/telemetry/telemetry.cpp
View file

@ -337,3 +337,110 @@ OutputTelemetryBuffer outputTelemetryBuffer __DMA;
#if defined(LUA)
Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
#endif
#define MIN_REFRESH_RATE 4000 /* us */
#define MAX_REFRESH_RATE 25000 /* us */
#define SAFE_SYNC_LAG 800 /* us */
#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;
newRefreshRate += lag/2;
if (newRefreshRate < MIN_REFRESH_RATE) {
newRefreshRate = MIN_REFRESH_RATE;
}
else if (newRefreshRate > MAX_REFRESH_RATE) {
newRefreshRate = MAX_REFRESH_RATE;
}
currentLag -= newRefreshRate - refreshRate;
return (uint16_t)newRefreshRate;
}
// sprintf does not work AVR ARM
// use a small helper function
static void appendInt(char * buf, uint32_t val)
{
while (*buf)
buf++;
strAppendUnsigned(buf, val);
}
static void prependSpaces(char * buf, int val)
{
while (*buf)
buf++;
int k = 10000;
while (val / k == 0 && k > 0) {
*buf = ' ';
buf++;
k /= 10;
}
*buf = '\0';
}
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) (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
}

28
radio/src/telemetry/telemetry.h
View file

@ -257,4 +257,32 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
void processPXX2Frame(uint8_t module, const uint8_t *frame);
// Module pulse synchronization
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() {
return (get_tmr10ms() - lastUpdate < 100);
}
// 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_