# HG changeset patch # User Florian Pose # Date 1269878657 -7200 # Node ID 131f655c03d382ab71ba473d4ba8b5c226773994 # Parent 02aedba35b94aebf250dd928c0b57bc339004f40 Improved DC tome offset calculation: - Only write time offset, if absolute difference to application time is more than 100 ms. - Separate handling for 32 bit and 64 bit time registers. diff -r 02aedba35b94 -r 131f655c03d3 TODO --- a/TODO Mon Mar 29 15:45:44 2010 +0200 +++ b/TODO Mon Mar 29 18:04:17 2010 +0200 @@ -52,7 +52,6 @@ * Override sync manager size? * Show Record / Array / List type of SDOs. * Distributed clocks: - - Check 32/64 bit operations. - Use vendor correction factors when calculating transmission delays. - Skip setting system time offset when application detached. - How to use the SYNC1 shift time? diff -r 02aedba35b94 -r 131f655c03d3 master/fsm_slave_config.c --- a/master/fsm_slave_config.c Mon Mar 29 15:45:44 2010 +0200 +++ b/master/fsm_slave_config.c Mon Mar 29 18:04:17 2010 +0200 @@ -44,6 +44,12 @@ /*****************************************************************************/ +/** Time difference [ns] to tolerate without setting a new system time offset. + */ +#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000 + +/*****************************************************************************/ + void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); @@ -400,7 +406,9 @@ ec_datagram_print_wc_error(datagram); } - // read DC system time and system time offset + // read DC system time (0x0910, 64 bit) + // gap (64 bit) + // and time offset (0x0920, 64 bit) ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_dc_read_offset; @@ -408,6 +416,93 @@ /*****************************************************************************/ +#define ABS(X) ((X) >= 0 ? (X) : -(X)) + +/** Configure 32 bit time offset. + */ +u64 ec_fsm_slave_config_dc_offset32( + ec_fsm_slave_config_t *fsm, /**< slave state machine */ + u64 system_time, /**< System time register. */ + u64 old_offset, /**< Time offset register. */ + unsigned long jiffies_since_read /**< Jiffies for correction. */ + ) +{ + ec_slave_t *slave = fsm->slave; + u32 correction, system_time32, old_offset32, new_offset; + s32 time_diff; + + system_time32 = (u32) system_time; + old_offset32 = (u32) old_offset; + + // correct read system time by elapsed time since read operation + correction = jiffies_since_read * 1000 / HZ * 1000000; + system_time32 += correction; + time_diff = (u32) slave->master->app_time - system_time32; + + if (slave->master->debug_level) + EC_DBG("Slave %u: system_time=%u (corrected with %u)," + " app_time=%u, diff=%i\n", + slave->ring_position, system_time32, correction, + (u32) slave->master->app_time, time_diff); + + if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { + new_offset = time_diff + old_offset32; + if (slave->master->debug_level) + EC_DBG("Slave %u: Setting time offset to %u (was %u)\n", + slave->ring_position, new_offset, old_offset32); + return (u64) new_offset; + } else { + if (slave->master->debug_level) + EC_DBG("Slave %u: Not touching time offset.\n", + slave->ring_position); + return old_offset; + } +} + +/*****************************************************************************/ + +/** Configure 64 bit time offset. + */ +u64 ec_fsm_slave_config_dc_offset64( + ec_fsm_slave_config_t *fsm, /**< slave state machine */ + u64 system_time, /**< System time register. */ + u64 old_offset, /**< Time offset register. */ + unsigned long jiffies_since_read /**< Jiffies for correction. */ + ) +{ + ec_slave_t *slave = fsm->slave; + u64 new_offset, correction; + s64 time_diff; + + // correct read system time by elapsed time since read operation + correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; + system_time += correction; + time_diff = fsm->slave->master->app_time - system_time; + + if (slave->master->debug_level) + EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," + " app_time=%llu, diff=%lli\n", + slave->ring_position, system_time, correction, + slave->master->app_time, time_diff); + + + if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { + new_offset = time_diff + old_offset; + if (slave->master->debug_level) + EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n", + slave->ring_position, new_offset, old_offset); + } else { + new_offset = old_offset; + if (slave->master->debug_level) + EC_DBG("Slave %u: Not touching time offset.\n", + slave->ring_position); + } + + return new_offset; +} + +/*****************************************************************************/ + /** Slave configuration state: DC READ OFFSET. */ void ec_fsm_slave_config_state_dc_read_offset( @@ -417,6 +512,7 @@ ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; u64 system_time, old_offset, new_offset; + unsigned long jiffies_since_read; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -438,15 +534,17 @@ return; } - system_time = EC_READ_U64(datagram->data); - old_offset = EC_READ_U64(datagram->data + 16); - new_offset = slave->master->app_time - system_time + old_offset; - - if (slave->master->debug_level) - EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, " - "app_time=%llu, new_offset=%llu\n", - slave->ring_position, system_time, old_offset, - slave->master->app_time, new_offset); + system_time = EC_READ_U64(datagram->data); // 0x0910 + old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 + jiffies_since_read = jiffies - datagram->jiffies_sent; + + if (slave->base_dc_range == EC_DC_32) { + new_offset = ec_fsm_slave_config_dc_offset32(fsm, + system_time, old_offset, jiffies_since_read); + } else { + new_offset = ec_fsm_slave_config_dc_offset64(fsm, + system_time, old_offset, jiffies_since_read); + } // set DC system time offset and transmission delay ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);