void Sync_Main(void) { static uint16 frac = FracN_DEFAULT; static int16 prev_error = 0; static uint8 pll_resetting = 0; static uint16 pll_testing = PLL_VERIFY_TIME; static uint16 pll_diff_acc = 0, pll_error_count = 0; int16 cur_error, diff, p_term, d_term; uint16 pos; pos = CY_GET_REG8(SyncSOF_FRAME_POS_LO__STATUS_REG); if (pos & 0x01) { pos += (uint16)CY_GET_REG8(SyncSOF_FRAME_POS_HI__STATUS_REG) << 8; cur_error = pos - SYNC_SOF_CENTER; diff = cur_error - prev_error; p_term = cur_error * SYNC_P_GAIN; d_term = diff * SYNC_D_GAIN; prev_error = cur_error; frac += p_term + d_term; if (frac > SYNC_FRAC_MAX) frac = SYNC_FRAC_MAX; if (frac < SYNC_FRAC_MIN) frac = SYNC_FRAC_MIN; if (pll_testing) { if (pll_resetting) { frac = 0; // Wait for PLL reset if (!(--pll_resetting)) { Control_Write(Control_Read() & ~CONTROL_LED); frac = FracN_DEFAULT; FRAC_CLK_SetDividerValue(13); } } else { pll_testing--; if (diff < 0) pll_diff_acc -= diff; else pll_diff_acc += diff; pll_diff_acc /= 2; if (pll_diff_acc < 2) { if (pll_error_count) pll_error_count--; } else if (pll_diff_acc > 7) { pll_error_count++; } if (pll_error_count > 50) { pll_diff_acc = 0; pll_error_count = 0; pll_resetting = PLL_RESET_TIME; pll_testing = PLL_VERIFY_TIME; Control_Write(Control_Read() | CONTROL_LED); FRAC_CLK_SetDividerValue(14); } } } FracN_Set(frac); } }
void Sync_Main(void) { static uint16 frac = FracN_DEFAULT; static int16 prev_error = 0, int_error = 0; int16 cur_error, diff, p_term, i_term, d_term; uint16 pos; pos = CY_GET_REG8(SyncSOF_FRAME_POS_LO__STATUS_REG); if (pos & 0x01) { pos += (uint16)CY_GET_REG8(SyncSOF_FRAME_POS_HI__STATUS_REG) << 8; cur_error = pos - SYNC_SOF_CENTER; int_error += cur_error; if (int_error < -SYNC_I_GUARD) int_error = -SYNC_I_GUARD; else if (int_error > SYNC_I_GUARD) int_error = SYNC_I_GUARD; diff = cur_error - prev_error; p_term = cur_error * SYNC_P_GAIN; i_term = int_error * SYNC_I_GAIN; d_term = diff * SYNC_D_GAIN; prev_error = cur_error; int_error *= 0.9; frac += p_term + i_term + d_term; if (frac > SYNC_FRAC_MAX) frac = SYNC_FRAC_MAX; if (frac < SYNC_FRAC_MIN) frac = SYNC_FRAC_MIN; FracN_Set(frac); } }