From f46b4ba654f8cf81fccad8f337689c429afcf560 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Nov 2023 14:27:01 +0100 Subject: [PATCH 1/2] Fixed wrong state assignment for IDN config. --- master/ioctl.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/master/ioctl.c b/master/ioctl.c index ac87de51..8c63bc6a 100644 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -1,8 +1,6 @@ /****************************************************************************** * - * $Id$ - * - * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH + * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH * * This file is part of the IgH EtherCAT Master. * @@ -1482,7 +1480,7 @@ static ATTRIBUTES int ec_ioctl_config_idn( ioctl->drive_no = req->drive_no; ioctl->idn = req->idn; - ioctl->state = req->state; + ioctl->state = req->al_state; ioctl->size = req->data_size; memcpy(ioctl->data, req->data, min((u32) ioctl->size, (u32) EC_MAX_IDN_DATA_SIZE)); From 97ee76462b641f2c47e991266adbdbf513400967 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Thu, 23 Nov 2023 14:38:29 +0100 Subject: [PATCH 2/2] Introduced feature flag WaitBeforeSAFEOPms. --- include/ecrt.h | 3 ++ master/fsm_slave_config.c | 65 ++++++++++++++++++++++++++++++++++++--- master/fsm_slave_config.h | 5 ++- 3 files changed, 65 insertions(+), 8 deletions(-) diff --git a/include/ecrt.h b/include/ecrt.h index 3f6b4b19..bae36673 100644 --- a/include/ecrt.h +++ b/include/ecrt.h @@ -1656,6 +1656,9 @@ int ecrt_slave_config_idn( * assigned to EtherCAT (except during transition to PREOP). Non-zero * assigns the SII to the slave controller side before going to PREOP and * leaves it there until a write command happens. + * - WaitBeforeSAFEOPms: Number of milliseconds to wait before commanding the + * transition from PREOP to SAFEOP. This can be used as a workaround for + * slaves that need a little time to initialize. * * This method has to be called in non-realtime context before * ecrt_master_activate(). diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 81a1ecc6..52f5a2d3 100644 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -1,8 +1,6 @@ /****************************************************************************** * - * $Id$ - * - * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH * * This file is part of the IgH EtherCAT Master. * @@ -85,6 +83,7 @@ void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_dc_sync_check(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 *); +void ec_fsm_slave_config_state_wait_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_soe_conf_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); @@ -105,6 +104,7 @@ void ec_fsm_slave_config_enter_watchdog(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_wait_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_soe_conf_safeop(ec_fsm_slave_config_t *); void ec_fsm_slave_config_enter_op(ec_fsm_slave_config_t *); @@ -135,6 +135,8 @@ void ec_fsm_slave_config_init( fsm->fsm_coe = fsm_coe; fsm->fsm_soe = fsm_soe; fsm->fsm_pdo = fsm_pdo; + + fsm->wait_ms = 0; } /*****************************************************************************/ @@ -1224,7 +1226,7 @@ void ec_fsm_slave_config_enter_fmmu( const ec_sync_t *sync; if (!slave->config) { - ec_fsm_slave_config_enter_safeop(fsm); + ec_fsm_slave_config_enter_wait_safeop(fsm); return; } @@ -1329,7 +1331,7 @@ void ec_fsm_slave_config_enter_dc_cycle( fsm->state = ec_fsm_slave_config_state_dc_cycle; } else { // DC are unused - ec_fsm_slave_config_enter_safeop(fsm); + ec_fsm_slave_config_enter_wait_safeop(fsm); } } @@ -1551,6 +1553,59 @@ void ec_fsm_slave_config_state_dc_assign( return; } + ec_fsm_slave_config_enter_wait_safeop(fsm); +} + +/*****************************************************************************/ + +/** Wait before SAFEOP transition. + * + * The feature flag WaitBeforeSAFEOPms can be used to add a wait time before + * going to SAFEOP. This can be used as a workaround for slaves that need some + * extra time for initialisation. + */ +void ec_fsm_slave_config_enter_wait_safeop( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + ec_slave_config_t *config = fsm->slave->config; + fsm->wait_ms = 0UL; + if (config) { + ec_flag_t *flag = ec_slave_config_find_flag(config, + "WaitBeforeSAFEOPms"); + if (flag && flag->value > 0) { + fsm->wait_ms = (unsigned long) flag->value; + } + } + + if (fsm->wait_ms > 0) { + fsm->state = ec_fsm_slave_config_state_wait_safeop; + + /* dummy read */ + ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, + 0x0600, 1); + + fsm->jiffies_start = jiffies; + } + else { + ec_fsm_slave_config_enter_safeop(fsm); + } +} + +/*****************************************************************************/ + +/** Slave configuration state: WAIT SAFEOP. + */ +void ec_fsm_slave_config_state_wait_safeop( + ec_fsm_slave_config_t *fsm /**< slave state machine */ + ) +{ + unsigned long diff = jiffies - fsm->jiffies_start; + + if (diff * 1000 / HZ < fsm->wait_ms) { + return; + } + ec_fsm_slave_config_enter_safeop(fsm); } diff --git a/master/fsm_slave_config.h b/master/fsm_slave_config.h index 0111be7b..a89037bc 100644 --- a/master/fsm_slave_config.h +++ b/master/fsm_slave_config.h @@ -1,8 +1,6 @@ /****************************************************************************** * - * $Id$ - * - * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * Copyright (C) 2006-2023 Florian Pose, Ingenieurgemeinschaft IgH * * This file is part of the IgH EtherCAT Master. * @@ -68,6 +66,7 @@ struct ec_fsm_slave_config ec_soe_request_t soe_request_copy; /**< Copied SDO request. */ unsigned long jiffies_start; /**< For timeout calculations. */ unsigned int take_time; /**< Store jiffies after datagram reception. */ + unsigned long wait_ms; /**< Wait time (used to wait before SAFEOP). */ }; /*****************************************************************************/