# HG changeset patch # User Florian Pose # Date 1239301278 0 # Node ID 9d04cc08f40f2638388a16ce51e5c4209e371f30 # Parent e9fe40c458ccca2adcdd43afd861fc3e615ad626 DC sync reference clock to application time. TBC... diff -r e9fe40c458cc -r 9d04cc08f40f include/ecrt.h --- a/include/ecrt.h Thu Apr 09 14:56:23 2009 +0000 +++ b/include/ecrt.h Thu Apr 09 18:21:18 2009 +0000 @@ -67,9 +67,11 @@ #ifdef __KERNEL__ #include #include +#include #else #include // for size_t #include +#include // for struct timeval #endif /****************************************************************************** @@ -505,9 +507,13 @@ ); /** Queues the DC drift compensation datagram for sending. + * + * The reference clock will by synchronized to the \a app_time, while the + * other slaves will by synchronized to the reference clock. */ void ecrt_master_sync( - ec_master_t *master /**< EtherCAT master. */ + ec_master_t *master, /**< EtherCAT master. */ + const struct timeval *app_time /**< Application time. */ ); /****************************************************************************** @@ -685,14 +691,14 @@ * * The AssignActivate word is vendor-specific and can be taken from the XML * device description file (Device -> Dc -> AssignActivate). Set this to zero, - * if the slave shall be not operated without distributed clocks (default). + * if the slave shall be operated without distributed clocks (default). */ void ecrt_slave_config_dc_assign_activate( ec_slave_config_t *sc, /**< Slave configuration. */ uint16_t assign_activate /**< AssignActivate word. */ ); -/** Sets the cylce times for the SYNC0 and SYNC1 signals. +/** Sets the cycle times for the SYNC0 and SYNC1 signals. */ void ecrt_slave_config_dc_sync_cycle_times( ec_slave_config_t *sc, /**< Slave configuration. */ diff -r e9fe40c458cc -r 9d04cc08f40f master/fsm_slave_config.c --- a/master/fsm_slave_config.c Thu Apr 09 14:56:23 2009 +0000 +++ b/master/fsm_slave_config.c Thu Apr 09 18:21:18 2009 +0000 @@ -52,6 +52,8 @@ void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_dc_read(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_state_dc_offset(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); @@ -66,8 +68,7 @@ void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_enter_dc_cycle(ec_fsm_slave_config_t *); -void ec_fsm_slave_config_enter_dc_assign(ec_fsm_slave_config_t *); +void ec_fsm_slave_config_enter_dc_read(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); @@ -811,7 +812,7 @@ } if (!slave->base_fmmu_count) { // skip FMMU configuration - ec_fsm_slave_config_enter_dc_cycle(fsm); + ec_fsm_slave_config_enter_dc_read(fsm); return; } @@ -867,28 +868,111 @@ return; } - ec_fsm_slave_config_enter_dc_cycle(fsm); + ec_fsm_slave_config_enter_dc_read(fsm); } /*****************************************************************************/ /** Check for DCs to be configured. */ -void ec_fsm_slave_config_enter_dc_cycle( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - ec_slave_config_t *config = slave->config; +void ec_fsm_slave_config_enter_dc_read( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; + + if (slave->base_dc_supported) { + // read DC system time and system time offset + ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0910, 24); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_dc_read; + } else { + ec_fsm_slave_config_enter_safeop(fsm); + } +} + +/*****************************************************************************/ + +/** Slave configuration state: DC READ. + */ +void ec_fsm_slave_config_state_dc_read( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + u64 system_time, old_offset, new_offset; + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to receive DC times datagram for slave %u" + " (datagram state %u).\n", + slave->ring_position, datagram->state); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to get DC times of slave %u: ", + slave->ring_position); + ec_datagram_print_wc_error(datagram); + 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); + + // set DC system time offset + ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 8); + EC_WRITE_U64(datagram->data, new_offset); + fsm->retries = EC_FSM_RETRIES; + fsm->state = ec_fsm_slave_config_state_dc_offset; +} + +/*****************************************************************************/ + +/** Slave configuration state: DC OFFSET. + */ +void ec_fsm_slave_config_state_dc_offset( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_datagram_t *datagram = fsm->datagram; + ec_slave_t *slave = fsm->slave; + ec_slave_config_t *config = slave->config; // FIXME + + if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) + return; + + if (datagram->state != EC_DATAGRAM_RECEIVED) { + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to receive DC system time offset datagram for slave %u" + " (datagram state %u).\n", + slave->ring_position, datagram->state); + return; + } + + if (datagram->working_counter != 1) { + slave->error_flag = 1; + fsm->state = ec_fsm_slave_config_state_error; + EC_ERR("Failed to set DC system time offset of slave %u: ", + slave->ring_position); + ec_datagram_print_wc_error(datagram); + return; + } if (config->dc_assign_activate) { - if (!slave->base_dc_supported) { - EC_WARN("Attempt to enable synchronized mode for slave %u," - " that seems not to support distributed clocks!\n", - slave->ring_position); - } - // set DC cycle times ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); EC_WRITE_U32(datagram->data, config->dc_sync_cycle_times[0]); @@ -896,7 +980,7 @@ fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_dc_cycle; } else { - ec_fsm_slave_config_enter_dc_assign(fsm); + ec_fsm_slave_config_enter_safeop(fsm); } } @@ -910,6 +994,7 @@ { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; + u64 start_time; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -932,8 +1017,13 @@ } // set DC start time + start_time = slave->master->app_time + 1000000000; // now plus 1 s + if (slave->master->debug_level) + EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", + slave->ring_position, start_time); + ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); - EC_WRITE_U64(datagram->data, 0x37E11D600ULL); // 15 s, FIXME + EC_WRITE_U64(datagram->data, start_time); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_dc_start; } @@ -948,6 +1038,7 @@ { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; + ec_slave_config_t *config = slave->config; // FIXME if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -969,21 +1060,6 @@ return; } - ec_fsm_slave_config_enter_dc_assign(fsm); -} - -/*****************************************************************************/ - -/** Set the DC AssignActivate word. - */ -void ec_fsm_slave_config_enter_dc_assign( - ec_fsm_slave_config_t *fsm /**< slave state machine */ - ) -{ - ec_datagram_t *datagram = fsm->datagram; - ec_slave_t *slave = fsm->slave; - ec_slave_config_t *config = slave->config; - // assign sync unit to EtherCAT or PDI ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); EC_WRITE_U16(datagram->data, config->dc_assign_activate); diff -r e9fe40c458cc -r 9d04cc08f40f master/globals.h --- a/master/globals.h Thu Apr 09 14:56:23 2009 +0000 +++ b/master/globals.h Thu Apr 09 18:21:18 2009 +0000 @@ -251,6 +251,13 @@ .name = EC_STR(NAME), .owner = THIS_MODULE, .mode = S_IRUGO | S_IWUSR \ } +/** Timeval to nanoseconds conversion. + * + * \param TV Pointer to struct timeval. + */ +#define EC_TIMEVAL2NANO(TV) \ + (((TV)->tv_sec - 946684800ULL) * 1000000000ULL + (TV)->tv_usec * 1000ULL) + /*****************************************************************************/ extern char *ec_master_version_str; diff -r e9fe40c458cc -r 9d04cc08f40f master/master.c --- a/master/master.c Thu Apr 09 14:56:23 2009 +0000 +++ b/master/master.c Thu Apr 09 18:21:18 2009 +0000 @@ -198,6 +198,16 @@ // create state machine object ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); + // init reference sync datagram + ec_datagram_init(&master->ref_sync_datagram); + snprintf(master->ref_sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "refsync"); + ret = ec_datagram_apwr(&master->ref_sync_datagram, 0, 0x0910, 8); + if (ret < 0) { + ec_datagram_clear(&master->ref_sync_datagram); + EC_ERR("Failed to allocate reference synchronisation datagram.\n"); + goto out_clear_fsm; + } + // init sync datagram ec_datagram_init(&master->sync_datagram); snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync"); @@ -205,7 +215,7 @@ if (ret < 0) { ec_datagram_clear(&master->sync_datagram); EC_ERR("Failed to allocate synchronisation datagram.\n"); - goto out_clear_fsm; + goto out_clear_ref_sync; } // init character device @@ -242,6 +252,8 @@ ec_cdev_clear(&master->cdev); out_clear_sync: ec_datagram_clear(&master->sync_datagram); +out_clear_ref_sync: + ec_datagram_clear(&master->ref_sync_datagram); out_clear_fsm: ec_fsm_master_clear(&master->fsm); ec_datagram_clear(&master->fsm_datagram); @@ -277,6 +289,7 @@ ec_master_clear_slaves(master); ec_datagram_clear(&master->sync_datagram); + ec_datagram_clear(&master->ref_sync_datagram); ec_fsm_master_clear(&master->fsm); ec_datagram_clear(&master->fsm_datagram); ec_device_clear(&master->backup_device); @@ -1607,10 +1620,19 @@ /*****************************************************************************/ -void ecrt_master_sync(ec_master_t *master) -{ +void ecrt_master_sync(ec_master_t *master, const struct timeval *app_time) +{ + master->app_time = EC_TIMEVAL2NANO(app_time); + +#if 1 + EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); + ec_master_queue_datagram(master, &master->ref_sync_datagram); +#endif + +#if 1 ec_datagram_zero(&master->sync_datagram); ec_master_queue_datagram(master, &master->sync_datagram); +#endif } /*****************************************************************************/ diff -r e9fe40c458cc -r 9d04cc08f40f master/master.h --- a/master/master.h Thu Apr 09 14:56:23 2009 +0000 +++ b/master/master.h Thu Apr 09 18:21:18 2009 +0000 @@ -117,6 +117,9 @@ struct list_head configs; /**< List of slave configurations. */ + u64 app_time; /**< Time of the last ecrt_master_sync() call. */ + ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the + reference clock to the master clock. */ ec_datagram_t sync_datagram; /**< Datagram used for DC drift compensation. */