void updateClock(TimeStructType *watch, uint8_t mode){
uint16_t col;
uint8_t tmode=0;
/* Deleting the hands related to the old time */
drawTime(oldwatch.hours, oldwatch.minutes, oldwatch.seconds, oldwatch.tenths, Black, 1);
/* tickSeconds(watch); */
/* Draw the new hands */
switch (mode) {
case 1:
col = White;
break;
case 2:
col = Red;
break;
case 3:
col = Green;
break;
case 4:
col = Blue;
tmode=1;
}
drawTime(watch->hours, watch->minutes, watch->seconds, watch->tenths, col, tmode);
drawCenter();
copyTime(&oldwatch, watch);
}
void updatePathDelay(one_way_delay_filter *owd_filt, // one way delay filter
RunTimeOpts *rtOpts, // run time options
PtpClock *ptpClock // PTP main data structure
)
{
Integer16 s;
TimeInternal remote_time;
DBGV("updatePathDelay:\n");
DBGV(" t1 PDelay Req Tx time %10.10ds.%9.9dns\n",
ptpClock->t1_pdelay_req_tx_time.seconds,
ptpClock->t1_pdelay_req_tx_time.nanoseconds
);
DBGV(" t2 PDelay Req Rx time %10.10ds.%9.9dns\n",
ptpClock->t2_pdelay_req_rx_time.seconds,
ptpClock->t2_pdelay_req_rx_time.nanoseconds
);
DBGV(" t3 PDelay Resp Tx time %10.10ds.%9.9dns\n",
ptpClock->t3_pdelay_resp_tx_time.seconds,
ptpClock->t3_pdelay_resp_tx_time.nanoseconds
);
DBGV(" t4 PDelay Resp Rx time %10.10ds.%9.9dns\n",
ptpClock->t4_pdelay_resp_rx_time.seconds,
ptpClock->t4_pdelay_resp_rx_time.nanoseconds
);
DBGV(" PDelay Resp correction %10.10ds.%9.9dns\n",
ptpClock->pdelay_resp_correction.seconds,
ptpClock->pdelay_resp_correction.nanoseconds
);
DBGV(" PDelay Resp follow up %10.10ds.%9.9dns\n",
ptpClock->pdelay_followup_correction.seconds,
ptpClock->pdelay_followup_correction.nanoseconds
);
/* calc 'slave_to_master_delay' */
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->t4_pdelay_resp_rx_time, // PDelay Response Receive time
&ptpClock->t1_pdelay_req_tx_time // minus PDelay Request Transmit time
);
DBGV(" (t4-t1) %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&remote_time, // Result
&ptpClock->t3_pdelay_resp_tx_time, // PDelay Resp Transmit time (from responder)
&ptpClock->t2_pdelay_req_rx_time // minus PDelay Request Receive time (from responder)
);
DBGV(" (t3-t2) %10.10ds.%9.9dns\n",
remote_time.seconds,
remote_time.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // (T4-T1)
&remote_time // minus (T3-T2)
);
DBGV(" (t4-t1)-(t3-t2) %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // Current Calculation
&ptpClock->pdelay_resp_correction // minus PDelay Resp Correction
);
DBGV(" minus 1st correction %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // Current Calculation
&ptpClock->pdelay_followup_correction // minus PDelay Resp Correction
);
DBGV(" minus 2nd correction %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
halveTime(&ptpClock->one_way_delay);
DBGV(" divided by 2 %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
copyTime( &ptpClock->slave_to_master_delay, // Destination
&ptpClock->one_way_delay // Source
);
clearTime(&ptpClock->t1_pdelay_req_tx_time);
clearTime(&ptpClock->t2_pdelay_req_rx_time);
clearTime(&ptpClock->t3_pdelay_resp_tx_time);
clearTime(&ptpClock->t4_pdelay_resp_rx_time);
clearTime(&ptpClock->pdelay_resp_correction);
clearTime(&ptpClock->pdelay_followup_correction);
clearTime(&ptpClock->t1_sync_delta_time);
clearTime(&ptpClock->t2_sync_delta_time);
if(ptpClock->one_way_delay.seconds) // Check if delay is larger than one second
{
/* Delay is larger than one second, clear s_exp and timestamp
* of previously received sent time of Sync message (usually from
* preciseOriginTimestamp of follow up message) and return
*/
DBG("updatePathDelay: One way delay seconds != 0\n");
DBG("updatePathDelay: Clearing one way delay filter s_exp, nsec_prev\n");
owd_filt->s_exp = 0;
owd_filt->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = rtOpts->s;
while(abs(owd_filt->y)>>(31-s))
--s;
DBGV("updatePathDelay: rtOpts->s: %d, s:%d\n",
rtOpts->s,
s
);
DBGV("updatePathDelay: current owd_filt->y: %d, s_exp: %d\n",
owd_filt->y,
owd_filt->s_exp
);
/* crank down filter cutoff by increasing 's_exp' */
if(owd_filt->s_exp < 1)
owd_filt->s_exp = 1;
else if(owd_filt->s_exp < 1<<s)
++owd_filt->s_exp;
else if(owd_filt->s_exp > 1<<s)
owd_filt->s_exp = 1<<s;
/* filter 'one_way_delay' */
owd_filt->y = (owd_filt->s_exp-1)
*owd_filt->y/owd_filt->s_exp
+ (ptpClock->one_way_delay.nanoseconds/2
+ owd_filt->nsec_prev/2
)
/owd_filt->s_exp;
/* Record previous one way delay nanosecond value
* and update it with value calculated above
*/
owd_filt->nsec_prev = ptpClock->one_way_delay.nanoseconds;
ptpClock->one_way_delay.nanoseconds = owd_filt->y;
DBGV("updatePathDelay: delay filter y:%d, s_exp:%d\n",
owd_filt->y,
owd_filt->s_exp
);
}
void updateClock(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
Integer32 adj=0;
TimeInternal timeTmpA; // AKB: Added values for adjusting calc based on time to get time
TimeInternal timeTmpB;
TimeInternal timeTmpC;
TimeInternal timeTmpD;
TimeInternal timeTmpE;
TimeInternal timeTmpF;
Integer64 delta_time_calc;
DBGV("updateClock:\n");
if(ptpClock->offset_from_master.seconds)
{
/* if offset from master seconds is non-zero, then this is a "big jump:
* in time. Check Run Time options to see if we will reset the clock or
* set frequency adjustment to max to adjust the time
*/
if(!rtOpts->noAdjust)
{
if(!rtOpts->noResetClock)
{
if (!isNonZeroTime(&ptpClock->t1_sync_delta_time))
{
// Delta time is zero, so this is the first sync to capture and we'll do the major
// adjustment on the next sync instead of this one
//
// Store t1 and t2 times as current delta, next time we'll subtract
//
copyTime(&ptpClock->t1_sync_delta_time,
&ptpClock->t1_sync_tx_time
);
copyTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_rx_time
);
NOTIFY("updateClock: Storing current T1 and T2 values for later calc\n");
DBG("updateClock: Storing T1: %10ds %11dns\n",
ptpClock->t1_sync_delta_time.seconds,
ptpClock->t1_sync_delta_time.nanoseconds
);
DBG("updateClock: Storing T2: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
return;
}
// If we are here then t1 and t2 sync delta were set to previous t1 and t2
// values. Now we calculate the deltas
DBG("updateClock: Current T1: %10ds %11dns\n",
ptpClock->t1_sync_tx_time.seconds,
ptpClock->t1_sync_tx_time.nanoseconds
);
DBG("updateClock: Current T2: %10ds %11dns\n",
ptpClock->t2_sync_rx_time.seconds,
ptpClock->t2_sync_rx_time.nanoseconds
);
subTime(&ptpClock->t1_sync_delta_time,
&ptpClock->t1_sync_tx_time,
&ptpClock->t1_sync_delta_time
);
subTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_rx_time,
&ptpClock->t2_sync_delta_time
);
DBG("updateClock: Delta T1: %10ds %11dns\n",
ptpClock->t1_sync_delta_time.seconds,
ptpClock->t1_sync_delta_time.nanoseconds
);
DBG("updateClock: Delta T2: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
// Now we get the difference between the two time bases and store in the T2 time delta
// as we will use the T1 time as the divisor (so master clock drives the time)
subTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_delta_time,
&ptpClock->t1_sync_delta_time
);
DBG("updateClock: Delta T2 - Delta T1: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
delta_time_calc = getNanoseconds(&ptpClock->t2_sync_delta_time)
* 1000000000;
delta_time_calc /= getNanoseconds(&ptpClock->t1_sync_delta_time);
DBG("updateClock: Calculated Parts/billion: %d\n",
(int)delta_time_calc
);
/* clamp the accumulator to ADJ_FREQ_MAX for sanity */
if( delta_time_calc > ADJ_FREQ_MAX)
adj = ADJ_FREQ_MAX;
else if(delta_time_calc < -ADJ_FREQ_MAX)
adj = -ADJ_FREQ_MAX;
else
adj = (UInteger32)delta_time_calc;
NOTIFY("updateClock: Initial clock adjust: %d, base: %d\n",
adj,
ptpClock->baseAdjustValue
);
NOTIFY("updateClock: Offset from Master %ds.%9.9d seconds\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( "updateClock: offset_from_master seconds != 0\n");
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBGV(" one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBG( " offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( " observed drift: %10d\n",
ptpClock->observed_drift
);
getTime(&timeTmpA, ptpClock->current_utc_offset); // Get current time #1
getTime(&timeTmpB, ptpClock->current_utc_offset); // Get current time #2
subTime(&timeTmpC, // Calculate time #3, time elapsed between calls
&timeTmpB,
&timeTmpA
);
getTime(&timeTmpD, ptpClock->current_utc_offset); // Get current time #4
subTime(&timeTmpE, // Subtract calculated offset from master
&timeTmpD,
&ptpClock->offset_from_master
);
addTime(&timeTmpF, // Add calculated time to get timer value
&timeTmpE,
&timeTmpC
);
setTime(&timeTmpF, ptpClock->current_utc_offset); // Set new PTP time
DBGV(" get Time A : %10ds %11dns\n",
timeTmpA.seconds,
timeTmpA.nanoseconds
);
DBGV(" get Time B : %10ds %11dns\n",
timeTmpB.seconds,
timeTmpB.nanoseconds
);
DBGV(" calc Time C (B-A) : %10ds %11dns\n",
timeTmpC.seconds,
timeTmpC.nanoseconds
);
DBGV(" get Time D : %10ds %11dns\n",
timeTmpD.seconds,
timeTmpD.nanoseconds
);
DBGV(" offset from master : %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBGV(" calc Time E (D+offset): %10ds %11dns\n",
timeTmpE.seconds,
timeTmpE.nanoseconds
);
DBGV(" calc Time F (E+C) : %10ds %11dns\n",
timeTmpF.seconds,
timeTmpF.nanoseconds
);
DBGV("updateClock: set time to Time F\n");
// Initialize clock variables based on run time options (rtOpts)
initClockVars(rtOpts, ptpClock);
// Adjust clock based on calculation from Delta T1, T2 times
adjFreq(ptpClock->baseAdjustValue - adj);
// Set initial observed drift to this calculated value
ptpClock->observed_drift = adj;
DBG( "updateClock: after initClock:\n");
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBG( " one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBG( " offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( " observed drift: %10d\n",
ptpClock->observed_drift
);
}
else
{
/* Run time options indicate we can't reset the clock, so we slow
* it down or speed it up based on ADJ_FREQ_MAX adjustment rather
* than actually setting the time.
*/
adj = ptpClock->offset_from_master.nanoseconds > 0 ? ADJ_FREQ_MAX : -ADJ_FREQ_MAX;
adjFreq(ptpClock->baseAdjustValue - adj);
}
}
}
else
{
/* Offset from master is less than one second. Use the the PI controller
* to adjust the time
*/
DBGV("updateClock: using PI controller to update clock\n");
/* no negative or zero attenuation */
if(rtOpts->ap < 1)
rtOpts->ap = 1;
if(rtOpts->ai < 1)
rtOpts->ai = 1;
DBGV(" previous observed drift: %10d\n",
ptpClock->observed_drift
);
DBGV(" run time opts P: %10d\n",
rtOpts->ap
);
DBGV(" run time opts I: %10d\n",
rtOpts->ai
);
DBGV(" current observed drift: %d\n",
ptpClock->observed_drift
);
DBGV(" current offset %dns\n",
rtOpts->ai
);
/* the accumulator for the I component */
ptpClock->observed_drift += ptpClock->offset_from_master.nanoseconds/rtOpts->ai;
DBGV(" new observed drift (I): %d\n",
ptpClock->observed_drift
);
/* clamp the accumulator to ADJ_FREQ_MAX for sanity */
if( ptpClock->observed_drift > ADJ_FREQ_MAX)
ptpClock->observed_drift = ADJ_FREQ_MAX;
else if(ptpClock->observed_drift < -ADJ_FREQ_MAX)
ptpClock->observed_drift = -ADJ_FREQ_MAX;
DBGV(" clamped drift: %d\n",
ptpClock->observed_drift
);
adj = ptpClock->offset_from_master.nanoseconds/rtOpts->ap + ptpClock->observed_drift;
DBGV(" calculated adjust: %d\n",
adj
);
DBGV(" base adjust: %d\n",
ptpClock->baseAdjustValue
);
/* apply controller output as a clock tick rate adjustment */
if(!rtOpts->noAdjust)
{
DBGV(" calling adjFreq with: %d\n",
(ptpClock->baseAdjustValue-adj)
);
adjFreq(ptpClock->baseAdjustValue - adj);
if (rtOpts->rememberAdjustValue == TRUE)
{
if ( ptpClock->offset_from_master.nanoseconds <= 100
&& ptpClock->offset_from_master.nanoseconds >= -100
)
{
ptpClock->lastAdjustValue = -adj; // Store value if it gave a good clock
// result.
}
}
}
}
/* Display statistics (save to a file if -f specified) if run time option enabled */
if(rtOpts->displayStats)
displayStats(rtOpts, ptpClock);
DBGV(" offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBGV(" one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBGV( " current observed drift: %10d\n",
ptpClock->observed_drift
);
DBGV(" clock adjust value: %10d\n",
(ptpClock->baseAdjustValue - adj)
);
}
