/**
\brief update compensationTimeout at the beginning of each slot and adjust current slot length when the elapsed slots rearch to compensation interval.
Once compensationTimeout == 0, extend or shorten current slot length for one tick.
*/
void adaptive_sync_countCompensationTimeout() {
uint16_t newSlotDuration;
newSlotDuration = TsSlotDuration;
// if clockState is not set yet, don't compensate.
if(adaptive_sync_vars.clockState == S_NONE) {
return;
}
if(adaptive_sync_vars.compensationTimeout == 0) {
return; // should not happen
}
adaptive_sync_vars.compensationTimeout--;
// when compensationTimeout, adjust current slot length
if(adaptive_sync_vars.compensationTimeout == 0) {
if(adaptive_sync_vars.clockState == S_SLOWER) {
newSlotDuration -= SYNC_ACCURACY;
adaptive_sync_vars.compensateTicks += SYNC_ACCURACY;
} else { // clock is fast
newSlotDuration += SYNC_ACCURACY;
adaptive_sync_vars.compensateTicks += SYNC_ACCURACY;
}
// update current slot duration and reload compensationTimeout
radio_setTimerPeriod(newSlotDuration);
adaptive_sync_vars.compensationTimeout = adaptive_sync_vars.compensationInfo_vars.compensationSlots;
#ifdef OPENSIM
debugpins_debug_set();
debugpins_debug_clr();
#endif
}
}
/**
\brief update compensationTimeout when compound slots are scheduled and adjust the slot when the elapsed slots rearch to compensation interval(e.g. SERIALRX slots)
\param[in] compoundSlots how many slots will be elapsed before wakeup next time.
*/
void adaptive_sync_countCompensationTimeout_compoundSlots(uint16_t compoundSlots) {
uint16_t counter;
uint8_t compensateTicks;
uint16_t newSlotDuration;
newSlotDuration = TsSlotDuration*(compoundSlots+1);
// if clockState is not set yet, don't compensate.
if(adaptive_sync_vars.clockState == S_NONE) {
return;
}
if(adaptive_sync_vars.compensationTimeout == 0) {
return; // should not happen
}
if(compoundSlots < 1) {
// return, if this is not a compoundSlot
return;
}
counter = compoundSlots;
compensateTicks = 0;
while(counter > 0) {
adaptive_sync_vars.compensationTimeout--;
if (adaptive_sync_vars.compensationTimeout == 0) {
compensateTicks += 1;
adaptive_sync_vars.compensationTimeout = adaptive_sync_vars.compensationInfo_vars.compensationSlots;
}
counter--;
}
// when compensateTicks > 0, I need to do compensation by adjusting current slot length
if(compensateTicks > 0) {
if(adaptive_sync_vars.clockState == S_SLOWER) {
newSlotDuration -= compensateTicks*SYNC_ACCURACY;
adaptive_sync_vars.compensateTicks += compensateTicks*SYNC_ACCURACY;
} else { // clock is fast
newSlotDuration += compensateTicks*SYNC_ACCURACY;
adaptive_sync_vars.compensateTicks += compensateTicks * SYNC_ACCURACY;
}
radio_setTimerPeriod(newSlotDuration);
#ifdef OPENSIM
debugpins_debug_set();
debugpins_debug_clr();
#endif
}
}
