/** \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 } }