Commit 5f17003f authored by Tomasz Wlostowski's avatar Tomasz Wlostowski Committed by Grzegorz Daniluk

wr_servo.c: don't reset on negative HW offset close to 0, some fixes in timestamps arithmetics

parent ef04daa5
......@@ -109,13 +109,27 @@ static wr_timestamp_t ts_sub(wr_timestamp_t a, wr_timestamp_t b)
c.nsec--;
}
while(c.phase > 1000)
{
c.phase-=1000;
c.nsec++;
}
c.nsec += a.nsec - b.nsec;
while(c.nsec < 0)
{
c.nsec += 1000000000L;
c.sec--;
}
while(c.nsec > 1000000000L)
{
c.nsec -= 1000000000L;
c.sec++;
}
c.sec += a.sec - b.sec;
return c;
......@@ -287,17 +301,6 @@ int wr_servo_update(PtpPortDS *clock)
cur_servo_state.update_count++;
got_sync = 0;
if (0) { /* enable for debugging */
dump_timestamp("servo:t1", s->t1);
dump_timestamp("servo:t2", s->t2);
dump_timestamp("servo:t3", s->t3);
dump_timestamp("servo:t4", s->t4);
dump_timestamp("->mdelay", s->mu);
}
s->mu = ts_sub(ts_sub(s->t4, s->t1), ts_sub(s->t3, s->t2));
big_delta_fix = s->delta_tx_m + s->delta_tx_s
......@@ -311,6 +314,22 @@ int wr_servo_update(PtpPortDS *clock)
ts_offset = ts_add(ts_sub(s->t1, s->t2), picos_to_ts(delay_ms_fix));
ts_offset_hw = ts_hardwarize(ts_offset, s->clock_period_ps);
if (0) { /* enable for debugging */
printf("State %s\n", cur_servo_state.slave_servo_state); //, "SYNC_PHASE");
dump_timestamp("servo:t1", s->t1);
dump_timestamp("servo:t2", s->t2);
dump_timestamp("servo:t3", s->t3);
dump_timestamp("servo:t4", s->t4);
dump_timestamp("->mdelay", s->mu);
dump_timestamp("->o", ts_offset);
dump_timestamp("->hwo", ts_offset_hw);
}
cur_servo_state.mu = (uint64_t)ts_to_picos(s->mu);
cur_servo_state.cur_offset = ts_to_picos(ts_offset);
......@@ -396,8 +415,8 @@ int wr_servo_update(PtpPortDS *clock)
{
int64_t remaining_offset = abs(ts_to_picos(ts_offset_hw));
if(ts_offset_hw.sec !=0 || ts_offset_hw.nsec != 0)
s->state = WR_SYNC_TAI;
if(remaining_offset > REF_CLOCK_PERIOD_PS)
s->state = WR_SYNC_TAI;
else if(remaining_offset < WR_SERVO_OFFSET_STABILITY_THRESHOLD)
{
ptpd_netif_enable_timing_output(1);
......@@ -406,33 +425,32 @@ int wr_servo_update(PtpPortDS *clock)
if(s->missed_iters >= 10)
s->state = WR_SYNC_TAI;
break;
}
case WR_TRACK_PHASE:
strcpy(cur_servo_state.slave_servo_state, "TRACK_PHASE");
cur_servo_state.cur_setpoint = s->cur_setpoint;
cur_servo_state.cur_skew = s->delta_ms - s->delta_ms_prev;
{
int64_t remaining_offset = abs(ts_to_picos(ts_offset_hw));
strcpy(cur_servo_state.slave_servo_state, "TRACK_PHASE");
cur_servo_state.cur_setpoint = s->cur_setpoint;
cur_servo_state.cur_skew = s->delta_ms - s->delta_ms_prev;
if(ts_offset_hw.sec !=0 || ts_offset_hw.nsec != 0)
if(remaining_offset > REF_CLOCK_PERIOD_PS)
s->state = WR_SYNC_TAI;
if(tracking_enabled)
{
// shw_pps_gen_enable_output(1);
// just follow the changes of deltaMS
s->cur_setpoint += (s->delta_ms - s->delta_ms_prev);
if(tracking_enabled)
{
// just follow the changes of deltaMS
s->cur_setpoint += (s->delta_ms - s->delta_ms_prev);
ptpd_netif_adjust_phase(s->cur_setpoint);
ptpd_netif_adjust_phase(s->cur_setpoint);
s->delta_ms_prev = s->delta_ms;
s->next_state = WR_TRACK_PHASE;
s->state = WR_WAIT_SYNC_IDLE;
s->last_tics = tics;
s->delta_ms_prev = s->delta_ms;
s->next_state = WR_TRACK_PHASE;
s->state = WR_WAIT_SYNC_IDLE;
s->last_tics = tics;
}
break;
}
break;
}
return 0;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment