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);
}
}
