From e881a134603ca176079d09c1188f9d44341b3d15 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Fri, 14 Oct 2016 14:54:07 +0200 Subject: [PATCH 01/13] add soem_master_types.hpp --- soem_master/soem_master_types.hpp | 79 +++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 soem_master/soem_master_types.hpp diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp new file mode 100644 index 0000000..f2a49a7 --- /dev/null +++ b/soem_master/soem_master_types.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + soem_master_types.hpp - description + ------------------- + begin : Wed October 12 2016 + copyright : (C) 2016 + email : luca@intermodalics.eu + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ +#include +#include +#include +#include + +/** The structure that contains the information to be sent for each CoE SDO */ +struct parameter +{ + /** slave's index starting from 1 and depending on position */ + int slavePosition; + /** Index of the CoE object */ + int index; + /** Subindex of the CoE object */ + int subIndex; + /** Falg to enable complete access */ + bool completeAccess; + /** Size of the CoE object to be written in bytes */ + int size; + /** The value of the parameter to be written (TODO change to a vector of chars to send parameters of any dimension)*/ + int param; + std::string name; + std::string description; + +}; + +//################################################################ + +namespace boost { +namespace serialization { +// The helper function which you write yourself: +template +void serialize( Archive & a, parameter & cd, unsigned int) { +using boost::serialization::make_nvp; +a & make_nvp("slavePosition", cd.slavePosition); +a & make_nvp("index", cd.index); +a & make_nvp("subIndex", cd.subIndex); +a & make_nvp("completeAccess", cd.completeAccess); +a & make_nvp("size", cd.size); +a & make_nvp("param", cd.param); +a & make_nvp("name", cd.name); +a & make_nvp("description", cd.description); + +} +} +} + +// The RTT helper class which uses the above function behind the scenes: +struct parameterTypeInfo +: public RTT::types::StructTypeInfo +{ +parameterTypeInfo() +: RTT::types::StructTypeInfo("parameter") +{} +}; + From 72ef26ea984a7cd5dc7a16ca1e54391a8d77a576 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Fri, 14 Oct 2016 15:14:18 +0200 Subject: [PATCH 02/13] updated files containing features --- soem_master/soem_driver.h | 24 +++++++++ soem_master/soem_master_component.cpp | 71 +++++++++++++++++++++++++++ soem_master/soem_master_component.h | 13 +++++ 3 files changed, 108 insertions(+) diff --git a/soem_master/soem_driver.h b/soem_master/soem_driver.h index c75da08..7988c04 100644 --- a/soem_master/soem_driver.h +++ b/soem_master/soem_driver.h @@ -105,6 +105,26 @@ class SoemDriver return (ec_state)(m_datap->state); }; + virtual int getInputByteLength(){ + + return (int)(m_datap->Ibytes); + }; + + virtual int getOutputByteLength(){ + + return (int)(m_datap->Obytes); + }; + + virtual int getInputBitLength(){ + + return (int)(m_datap->Ibits); + }; + + virtual int getOutputBitLength(){ + + return (int)(m_datap->Obits); + }; + protected: SoemDriver(ec_slavet* mem_loc) : m_datap(mem_loc), m_name("Slave_" + to_string(m_datap->configadr, @@ -114,6 +134,10 @@ class SoemDriver m_service->addOperation("checkState",&SoemDriver::checkState,this).doc("check the slaves state").arg("state","state value to check"); m_service->addOperation("getState",&SoemDriver::getState,this).doc("request slave state"); m_service->addOperation("configure",&SoemDriver::configure,this).doc("Configure slave"); + m_service->addOperation("getInputByteLength",&SoemDriver::getInputByteLength,this).doc("retrieve the length of the input cyclic data in bytes"); + m_service->addOperation("getOutputByteLength",&SoemDriver::getOutputByteLength,this).doc("retrieve the length of the output cyclic data in bytes"); + m_service->addOperation("getInputBitLength",&SoemDriver::getInputBitLength,this).doc("retrieve the length of the input cyclic data in bits"); + m_service->addOperation("getOutputBitLength",&SoemDriver::getOutputBitLength,this).doc("retrieve the length of the output cyclic data in bits"); } ; ec_slavet* m_datap; diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 1936f58..24e49e5 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -35,6 +35,7 @@ extern "C" #include "soem_master_component.h" #include "rtt/Component.hpp" +#include "rtt/types/EnumTypeInfo.hpp" ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) @@ -52,10 +53,22 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : "Second (redundant) interface to which the ethercat device is connected"); this->addProperty("redundant", prop_redundant=false).doc( "Whether to use a redundant nic"); + this->addProperty("slavesCoeParameters",parameters).doc( + "Vector of parameters to be sent to the slaves using CoE SDO"); + SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); this->addOperation("displayAvailableDrivers", &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( "display all available drivers for the soem master"); + this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( + "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); + this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( + "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); + + RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); + RTT::types::Types()->addType(new parameterTypeInfo()); + RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); + //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } @@ -94,6 +107,33 @@ bool SoemMasterComponent::configureHook() // wait for all slaves to reach PRE_OP state ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + } + + /* The parameter to be sent to the slaves are loaded */ + for (unsigned int i=0; i < parameters.size(); i++) + { + int wkc; + addressInfo tmp; + tmp.slavePosition = parameters[i].slavePosition; + tmp.index = parameters[i].index; + tmp.subIndex = parameters[i].subIndex; + + wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); + + if(wkc == 0) + { + log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index + << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size + << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + } + } + for (int i = 1; i <= ec_slavecount; i++) { SoemDriver @@ -142,6 +182,7 @@ bool SoemMasterComponent::configureHook() else { log(Error) << "Configuration of slaves failed!!!" << endlog(); + log(Error) << "The NIC currently used for EtherCAT is "<< prop_ifname1.c_str() << " . Another could be chosen by editing soem.cpf." << endlog(); return false; } return true; @@ -162,6 +203,14 @@ bool SoemMasterComponent::startHook() // wait for all slaves to reach SAFE_OP state ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + } + if (ec_slave[0].state == EC_STATE_SAFE_OP) { log(Info) << "Safe operational state reached for all slaves." @@ -205,6 +254,15 @@ bool SoemMasterComponent::startHook() // wait for all slaves to reach OP state ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + } + if (ec_slave[0].state == EC_STATE_OPERATIONAL) { log(Info) << "Operational state reached for all slaves." @@ -269,4 +327,17 @@ void SoemMasterComponent::cleanupHook() //stop SOEM, close socket ec_close(); } + + +int SoemMasterComponent::writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data) +{ + return ec_SDOwrite(address->slavePosition,address->index,address->subIndex,completeAccess,size, data,EC_TIMEOUTRXM); + +} + +int SoemMasterComponent::readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data) +{ + return ec_SDOread(address->slavePosition,address->index,address->subIndex,completeAccess, size, data,EC_TIMEOUTRXM); +} + }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index ce87a57..bb31ce2 100644 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -25,10 +25,19 @@ #include #include "soem_driver.h" +#include "soem_master_types.hpp" namespace soem_master { +/** CoE addressing info */ +struct addressInfo +{ + unsigned short slavePosition; + unsigned short index; + unsigned char subIndex; +}; + class SoemMasterComponent: public RTT::TaskContext { public: @@ -50,6 +59,10 @@ class SoemMasterComponent: public RTT::TaskContext bool prop_redundant; char m_IOmap[4096]; std::vector m_drivers; + std::vector parameters; + int writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data); + int readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data); + };//class }//namespace From e3a1f7ac3efb21f2ea6a86196bab2148c8a3d8e6 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 14 Oct 2016 15:21:29 +0200 Subject: [PATCH 03/13] minor space canges in soem_driver.h --- soem_master/soem_driver.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/soem_master/soem_driver.h b/soem_master/soem_driver.h index 7988c04..6b86c2f 100644 --- a/soem_master/soem_driver.h +++ b/soem_master/soem_driver.h @@ -106,22 +106,18 @@ class SoemDriver }; virtual int getInputByteLength(){ - return (int)(m_datap->Ibytes); }; virtual int getOutputByteLength(){ - return (int)(m_datap->Obytes); }; virtual int getInputBitLength(){ - return (int)(m_datap->Ibits); }; virtual int getOutputBitLength(){ - return (int)(m_datap->Obits); }; From 95b61fd98930a61724fa7d5231aba3470b2b7a9d Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 14 Oct 2016 15:28:01 +0200 Subject: [PATCH 04/13] tab alignment fix in soem_master_component --- soem_master/soem_master_component.cpp | 70 +++++++++++++-------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 24e49e5..647889a 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -54,20 +54,20 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : this->addProperty("redundant", prop_redundant=false).doc( "Whether to use a redundant nic"); this->addProperty("slavesCoeParameters",parameters).doc( - "Vector of parameters to be sent to the slaves using CoE SDO"); + "Vector of parameters to be sent to the slaves using CoE SDO"); SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); this->addOperation("displayAvailableDrivers", &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( "display all available drivers for the soem master"); - this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( - "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); - this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( - "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); + this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( + "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); + this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( + "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); - RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); - RTT::types::Types()->addType(new parameterTypeInfo()); - RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); + RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); + RTT::types::Types()->addType(new parameterTypeInfo()); + RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } @@ -110,29 +110,29 @@ bool SoemMasterComponent::configureHook() // The state should be verified for every slave because calling // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); - } - - /* The parameter to be sent to the slaves are loaded */ - for (unsigned int i=0; i < parameters.size(); i++) - { - int wkc; - addressInfo tmp; - tmp.slavePosition = parameters[i].slavePosition; - tmp.index = parameters[i].index; - tmp.subIndex = parameters[i].subIndex; - - wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); - - if(wkc == 0) - { - log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index - << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size - << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); - } - } + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + } + + /* The parameter to be sent to the slaves are loaded */ + for (unsigned int i=0; i < parameters.size(); i++) + { + int wkc; + addressInfo tmp; + tmp.slavePosition = parameters[i].slavePosition; + tmp.index = parameters[i].index; + tmp.subIndex = parameters[i].subIndex; + + wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); + + if(wkc == 0) + { + log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index + << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size + << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + } + } for (int i = 1; i <= ec_slavecount; i++) { @@ -258,10 +258,10 @@ bool SoemMasterComponent::startHook() // The state should be verified for every slave because calling // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - } + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + } if (ec_slave[0].state == EC_STATE_OPERATIONAL) { From c4b7c2f2f6e17be92487dccfebf744e80e1f68f0 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Fri, 14 Oct 2016 15:35:52 +0200 Subject: [PATCH 05/13] white spaces fixing --- soem_master/soem_master_component.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 647889a..8e5854f 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -110,7 +110,7 @@ bool SoemMasterComponent::configureHook() // The state should be verified for every slave because calling // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) + for(int i = 0; i <= ec_slavecount; i++) { ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); } @@ -258,7 +258,7 @@ bool SoemMasterComponent::startHook() // The state should be verified for every slave because calling // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) + for(int i = 0; i <= ec_slavecount; i++) { ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); } @@ -332,7 +332,6 @@ void SoemMasterComponent::cleanupHook() int SoemMasterComponent::writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data) { return ec_SDOwrite(address->slavePosition,address->index,address->subIndex,completeAccess,size, data,EC_TIMEOUTRXM); - } int SoemMasterComponent::readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data) From 59de91a9e85d5e06a7fd462432901bd5bea60a58 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 14 Oct 2016 15:42:39 +0200 Subject: [PATCH 06/13] Feature/coe parametrization (#1) * add soem_master_types.hpp * add the "parameter" type to soem_master * add the "ec_state" type to soem_master * minor bugfix: state of the slaves properly updated --- soem_master/soem_driver.h | 20 +++++++ soem_master/soem_master_component.cpp | 70 ++++++++++++++++++++++++ soem_master/soem_master_component.h | 13 +++++ soem_master/soem_master_types.hpp | 79 +++++++++++++++++++++++++++ 4 files changed, 182 insertions(+) create mode 100644 soem_master/soem_master_types.hpp diff --git a/soem_master/soem_driver.h b/soem_master/soem_driver.h index c75da08..6b86c2f 100644 --- a/soem_master/soem_driver.h +++ b/soem_master/soem_driver.h @@ -105,6 +105,22 @@ class SoemDriver return (ec_state)(m_datap->state); }; + virtual int getInputByteLength(){ + return (int)(m_datap->Ibytes); + }; + + virtual int getOutputByteLength(){ + return (int)(m_datap->Obytes); + }; + + virtual int getInputBitLength(){ + return (int)(m_datap->Ibits); + }; + + virtual int getOutputBitLength(){ + return (int)(m_datap->Obits); + }; + protected: SoemDriver(ec_slavet* mem_loc) : m_datap(mem_loc), m_name("Slave_" + to_string(m_datap->configadr, @@ -114,6 +130,10 @@ class SoemDriver m_service->addOperation("checkState",&SoemDriver::checkState,this).doc("check the slaves state").arg("state","state value to check"); m_service->addOperation("getState",&SoemDriver::getState,this).doc("request slave state"); m_service->addOperation("configure",&SoemDriver::configure,this).doc("Configure slave"); + m_service->addOperation("getInputByteLength",&SoemDriver::getInputByteLength,this).doc("retrieve the length of the input cyclic data in bytes"); + m_service->addOperation("getOutputByteLength",&SoemDriver::getOutputByteLength,this).doc("retrieve the length of the output cyclic data in bytes"); + m_service->addOperation("getInputBitLength",&SoemDriver::getInputBitLength,this).doc("retrieve the length of the input cyclic data in bits"); + m_service->addOperation("getOutputBitLength",&SoemDriver::getOutputBitLength,this).doc("retrieve the length of the output cyclic data in bits"); } ; ec_slavet* m_datap; diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 1936f58..8e5854f 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -35,6 +35,7 @@ extern "C" #include "soem_master_component.h" #include "rtt/Component.hpp" +#include "rtt/types/EnumTypeInfo.hpp" ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) @@ -52,10 +53,22 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : "Second (redundant) interface to which the ethercat device is connected"); this->addProperty("redundant", prop_redundant=false).doc( "Whether to use a redundant nic"); + this->addProperty("slavesCoeParameters",parameters).doc( + "Vector of parameters to be sent to the slaves using CoE SDO"); + SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); this->addOperation("displayAvailableDrivers", &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( "display all available drivers for the soem master"); + this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( + "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); + this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( + "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); + + RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); + RTT::types::Types()->addType(new parameterTypeInfo()); + RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); + //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } @@ -94,6 +107,33 @@ bool SoemMasterComponent::configureHook() // wait for all slaves to reach PRE_OP state ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + } + + /* The parameter to be sent to the slaves are loaded */ + for (unsigned int i=0; i < parameters.size(); i++) + { + int wkc; + addressInfo tmp; + tmp.slavePosition = parameters[i].slavePosition; + tmp.index = parameters[i].index; + tmp.subIndex = parameters[i].subIndex; + + wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); + + if(wkc == 0) + { + log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index + << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size + << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + } + } + for (int i = 1; i <= ec_slavecount; i++) { SoemDriver @@ -142,6 +182,7 @@ bool SoemMasterComponent::configureHook() else { log(Error) << "Configuration of slaves failed!!!" << endlog(); + log(Error) << "The NIC currently used for EtherCAT is "<< prop_ifname1.c_str() << " . Another could be chosen by editing soem.cpf." << endlog(); return false; } return true; @@ -162,6 +203,14 @@ bool SoemMasterComponent::startHook() // wait for all slaves to reach SAFE_OP state ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + } + if (ec_slave[0].state == EC_STATE_SAFE_OP) { log(Info) << "Safe operational state reached for all slaves." @@ -205,6 +254,15 @@ bool SoemMasterComponent::startHook() // wait for all slaves to reach OP state ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + + // The state should be verified for every slave because calling + // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + // otherwise the member state of every slave wouldn't be updated + for(int i = 0; i <= ec_slavecount; i++) + { + ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + } + if (ec_slave[0].state == EC_STATE_OPERATIONAL) { log(Info) << "Operational state reached for all slaves." @@ -269,4 +327,16 @@ void SoemMasterComponent::cleanupHook() //stop SOEM, close socket ec_close(); } + + +int SoemMasterComponent::writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data) +{ + return ec_SDOwrite(address->slavePosition,address->index,address->subIndex,completeAccess,size, data,EC_TIMEOUTRXM); +} + +int SoemMasterComponent::readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data) +{ + return ec_SDOread(address->slavePosition,address->index,address->subIndex,completeAccess, size, data,EC_TIMEOUTRXM); +} + }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index ce87a57..bb31ce2 100644 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -25,10 +25,19 @@ #include #include "soem_driver.h" +#include "soem_master_types.hpp" namespace soem_master { +/** CoE addressing info */ +struct addressInfo +{ + unsigned short slavePosition; + unsigned short index; + unsigned char subIndex; +}; + class SoemMasterComponent: public RTT::TaskContext { public: @@ -50,6 +59,10 @@ class SoemMasterComponent: public RTT::TaskContext bool prop_redundant; char m_IOmap[4096]; std::vector m_drivers; + std::vector parameters; + int writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data); + int readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data); + };//class }//namespace diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp new file mode 100644 index 0000000..f2a49a7 --- /dev/null +++ b/soem_master/soem_master_types.hpp @@ -0,0 +1,79 @@ +/*************************************************************************** + soem_master_types.hpp - description + ------------------- + begin : Wed October 12 2016 + copyright : (C) 2016 + email : luca@intermodalics.eu + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ +#include +#include +#include +#include + +/** The structure that contains the information to be sent for each CoE SDO */ +struct parameter +{ + /** slave's index starting from 1 and depending on position */ + int slavePosition; + /** Index of the CoE object */ + int index; + /** Subindex of the CoE object */ + int subIndex; + /** Falg to enable complete access */ + bool completeAccess; + /** Size of the CoE object to be written in bytes */ + int size; + /** The value of the parameter to be written (TODO change to a vector of chars to send parameters of any dimension)*/ + int param; + std::string name; + std::string description; + +}; + +//################################################################ + +namespace boost { +namespace serialization { +// The helper function which you write yourself: +template +void serialize( Archive & a, parameter & cd, unsigned int) { +using boost::serialization::make_nvp; +a & make_nvp("slavePosition", cd.slavePosition); +a & make_nvp("index", cd.index); +a & make_nvp("subIndex", cd.subIndex); +a & make_nvp("completeAccess", cd.completeAccess); +a & make_nvp("size", cd.size); +a & make_nvp("param", cd.param); +a & make_nvp("name", cd.name); +a & make_nvp("description", cd.description); + +} +} +} + +// The RTT helper class which uses the above function behind the scenes: +struct parameterTypeInfo +: public RTT::types::StructTypeInfo +{ +parameterTypeInfo() +: RTT::types::StructTypeInfo("parameter") +{} +}; + From 749a4d80caff1dc897b6dc0a2c970d97ee1b1416 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Tue, 18 Oct 2016 14:12:13 +0200 Subject: [PATCH 07/13] fixing marked issues for pr --- soem_master/soem_master_component.cpp | 27 +++++++++++++-------------- soem_master/soem_master_component.h | 12 ++++++------ soem_master/soem_master_types.hpp | 22 ++++++++++++---------- 3 files changed, 31 insertions(+), 30 deletions(-) diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 8e5854f..773bf9b 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -60,14 +60,14 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : this->addOperation("displayAvailableDrivers", &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( "display all available drivers for the soem master"); - this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( + this->addOperation("writeCoeSdo", &SoemMasterComponent::writeCoeSdo,this).doc( "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); - this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( + this->addOperation("readCoeSdo", &SoemMasterComponent::readCoeSdo,this).doc( "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); RTT::types::Types()->addType(new parameterTypeInfo()); - RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); + RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } @@ -119,17 +119,17 @@ bool SoemMasterComponent::configureHook() for (unsigned int i=0; i < parameters.size(); i++) { int wkc; - addressInfo tmp; - tmp.slavePosition = parameters[i].slavePosition; + AddressInfo tmp; + tmp.slave_position = parameters[i].slave_position; tmp.index = parameters[i].index; - tmp.subIndex = parameters[i].subIndex; + tmp.sub_index = parameters[i].sub_index; - wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); + wkc = writeCoeSdo(tmp,parameters[i].complete_access,parameters[i].size,¶meters[i].param); if(wkc == 0) { - log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index - << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size + log(Error) << "Slave_" << tmp.slave_position <<" SDOwrite{index["<< tmp.index + << "] subindex["<< (int)tmp.sub_index <<"] size "<< parameters[i].size << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); } } @@ -328,15 +328,14 @@ void SoemMasterComponent::cleanupHook() ec_close(); } - -int SoemMasterComponent::writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data) +int SoemMasterComponent::writeCoeSdo(const AddressInfo& address,bool completeAccess,int size,void* data) { - return ec_SDOwrite(address->slavePosition,address->index,address->subIndex,completeAccess,size, data,EC_TIMEOUTRXM); + return ec_SDOwrite(address.slave_position,address.index,address.sub_index,completeAccess,size,data,EC_TIMEOUTRXM); } -int SoemMasterComponent::readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data) +int SoemMasterComponent::readCoeSdo(const AddressInfo& address,bool completeAccess,int* size,void* data) { - return ec_SDOread(address->slavePosition,address->index,address->subIndex,completeAccess, size, data,EC_TIMEOUTRXM); + return ec_SDOread(address.slave_position,address.index,address.sub_index,completeAccess,size,data,EC_TIMEOUTRXM); } }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index bb31ce2..50adeb8 100644 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -31,11 +31,11 @@ namespace soem_master { /** CoE addressing info */ -struct addressInfo +struct AddressInfo { - unsigned short slavePosition; + unsigned short slave_position; unsigned short index; - unsigned char subIndex; + unsigned char sub_index; }; class SoemMasterComponent: public RTT::TaskContext @@ -59,9 +59,9 @@ class SoemMasterComponent: public RTT::TaskContext bool prop_redundant; char m_IOmap[4096]; std::vector m_drivers; - std::vector parameters; - int writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data); - int readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data); + std::vector parameters; + int writeCoeSdo(const AddressInfo& address,bool completeAccess,int size,void* data); + int readCoeSdo(const AddressInfo& address,bool completeAccess,int* size,void* data); };//class diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp index f2a49a7..be2bf47 100644 --- a/soem_master/soem_master_types.hpp +++ b/soem_master/soem_master_types.hpp @@ -27,17 +27,18 @@ #include #include +namespace rtt_soem { /** The structure that contains the information to be sent for each CoE SDO */ -struct parameter +struct Parameter { /** slave's index starting from 1 and depending on position */ - int slavePosition; + int slave_position; /** Index of the CoE object */ int index; /** Subindex of the CoE object */ - int subIndex; + int sub_index; /** Falg to enable complete access */ - bool completeAccess; + bool complete_access; /** Size of the CoE object to be written in bytes */ int size; /** The value of the parameter to be written (TODO change to a vector of chars to send parameters of any dimension)*/ @@ -46,6 +47,7 @@ struct parameter std::string description; }; +} //################################################################ @@ -53,12 +55,12 @@ namespace boost { namespace serialization { // The helper function which you write yourself: template -void serialize( Archive & a, parameter & cd, unsigned int) { +void serialize( Archive & a, rtt_soem::Parameter & cd, unsigned int) { using boost::serialization::make_nvp; -a & make_nvp("slavePosition", cd.slavePosition); +a & make_nvp("slavePosition", cd.slave_position); a & make_nvp("index", cd.index); -a & make_nvp("subIndex", cd.subIndex); -a & make_nvp("completeAccess", cd.completeAccess); +a & make_nvp("subIndex", cd.sub_index); +a & make_nvp("completeAccess", cd.complete_access); a & make_nvp("size", cd.size); a & make_nvp("param", cd.param); a & make_nvp("name", cd.name); @@ -70,10 +72,10 @@ a & make_nvp("description", cd.description); // The RTT helper class which uses the above function behind the scenes: struct parameterTypeInfo -: public RTT::types::StructTypeInfo +: public RTT::types::StructTypeInfo { parameterTypeInfo() -: RTT::types::StructTypeInfo("parameter") +: RTT::types::StructTypeInfo("Parameter") {} }; From 2170278b8e9fb82f36ff9c4d6cdaef1ef3bf0c08 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Tue, 18 Oct 2016 16:59:06 +0200 Subject: [PATCH 08/13] fix for pr --- soem_master/soem_master_component.cpp | 235 ++++++++++++++++---------- soem_master/soem_master_component.h | 6 +- 2 files changed, 147 insertions(+), 94 deletions(-) mode change 100644 => 100755 soem_master/soem_master_component.cpp mode change 100644 => 100755 soem_master/soem_master_component.h diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp old mode 100644 new mode 100755 index 773bf9b..4332cf5 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -104,18 +104,12 @@ bool SoemMasterComponent::configureHook() << endlog(); ec_slave[0].state = EC_STATE_PRE_OP; ec_writestate(0); - // wait for all slaves to reach PRE_OP state - ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + //Wait for all slaves to reach PRE_OP state + if(!checkNetworkState(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) + return false; - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); - } - - /* The parameter to be sent to the slaves are loaded */ + //The parameters to be sent to the slaves are loaded from the soem.cpf: + //parameters could be changed without modifying the code for (unsigned int i=0; i < parameters.size(); i++) { int wkc; @@ -123,15 +117,24 @@ bool SoemMasterComponent::configureHook() tmp.slave_position = parameters[i].slave_position; tmp.index = parameters[i].index; tmp.sub_index = parameters[i].sub_index; - - wkc = writeCoeSdo(tmp,parameters[i].complete_access,parameters[i].size,¶meters[i].param); - if(wkc == 0) + if(ec_slave[tmp.slave_position].mbx_proto & ECT_MBXPROT_COE) { - log(Error) << "Slave_" << tmp.slave_position <<" SDOwrite{index["<< tmp.index - << "] subindex["<< (int)tmp.sub_index <<"] size "<< parameters[i].size - << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + wkc = writeCoeSdo(tmp,parameters[i].complete_access,parameters[i].size,¶meters[i].param); + + if(wkc == 0) + { + log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" SDOwrite{index["<< tmp.index + << "] sub_index["<< (int)tmp.sub_index <<"] size "<< parameters[i].size + << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + } } + else + { + log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" does not support CoE" + << " but in soem.cpf there is a CoE parameter for this slave." << endlog(); + } + } for (int i = 1; i <= ec_slavecount; i++) @@ -197,48 +200,13 @@ bool SoemMasterComponent::configureHook() bool SoemMasterComponent::startHook() { + bool state_reached; + log(Info) << "Request safe-operational state for all slaves" << endlog(); ec_slave[0].state = EC_STATE_SAFE_OP; ec_writestate(0); // wait for all slaves to reach SAFE_OP state - ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - } - - if (ec_slave[0].state == EC_STATE_SAFE_OP) - { - log(Info) << "Safe operational state reached for all slaves." - << endlog(); - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } - } - else - { - log(Error) << "Not all slaves reached safe operational state." - << endlog(); - ec_readstate(); - //If not all slaves operational find out which one - for (int i = 0; i <= ec_slavecount; i++) - { - if (ec_slave[i].state != EC_STATE_SAFE_OP) - { - log(Error) << "Slave " << i << " State= " << to_string( - ec_slave[i].state, std::hex) << " StatusCode=" - << ec_slave[i].ALstatuscode << " : " - << ec_ALstatuscode2string( - ec_slave[i].ALstatuscode) << endlog(); - } - } - //return false; - } + checkNetworkState(EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); log(Info) << "Request operational state for all slaves" << endlog(); ec_slave[0].state = EC_STATE_OPERATIONAL; @@ -253,40 +221,9 @@ bool SoemMasterComponent::startHook() } // wait for all slaves to reach OP state - ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - } - - if (ec_slave[0].state == EC_STATE_OPERATIONAL) - { - log(Info) << "Operational state reached for all slaves." - << endlog(); - } - else - { - log(Error) << "Not all slaves reached operational state." - << endlog(); - //If not all slaves operational find out which one - for (int i = 1; i <= ec_slavecount; i++) - { - if (ec_slave[i].state != EC_STATE_OPERATIONAL) - { - log(Error) << "Slave " << i << " State= " << to_string( - ec_slave[i].state, std::hex) << " StatusCode=" - << ec_slave[i].ALstatuscode << " : " - << ec_ALstatuscode2string( - ec_slave[i].ALstatuscode) << endlog(); - } - } - return false; + if(!checkNetworkState(EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE)) + return false; - } return true; } @@ -328,14 +265,128 @@ void SoemMasterComponent::cleanupHook() ec_close(); } -int SoemMasterComponent::writeCoeSdo(const AddressInfo& address,bool completeAccess,int size,void* data) +int SoemMasterComponent::writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data) { - return ec_SDOwrite(address.slave_position,address.index,address.sub_index,completeAccess,size,data,EC_TIMEOUTRXM); + return ec_SDOwrite(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); } -int SoemMasterComponent::readCoeSdo(const AddressInfo& address,bool completeAccess,int* size,void* data) +int SoemMasterComponent::readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data) { - return ec_SDOread(address.slave_position,address.index,address.sub_index,completeAccess,size,data,EC_TIMEOUTRXM); + return ec_SDOread(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); +} + +bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout) +{ + bool state_is_reached = true; + bool error_detected = false; + + uint16 network_state = ec_statecheck(0, desired_state, timeout); + + if((network_state & 0xf0) == 0) + { + // No slave has toggled the error flag so the AlStatusCode (even if different from 0) should be ignored + for(int i = 0; i < ec_slavecount; i++) + { + ec_slave[i].ALstatuscode = 0x0000; + } + } + else + { + error_detected = true; + } + + switch(network_state) + { + case EC_STATE_INIT: + case EC_STATE_PRE_OP: + case EC_STATE_BOOT: + case EC_STATE_SAFE_OP: + case EC_STATE_OPERATIONAL: + if(!error_detected) + { + //All the slaves have reached the same state so we can update the state of every slave + for(int i = 0; i < ec_slavecount; i++) + { + ec_slave[i].state = network_state; + } + } + else + { + ec_readstate(); + } + + break; + + default: + //The state should be verified for every single slave + //since not all have the same state + ec_readstate(); + break; + } + + if (ec_slave[0].state == desired_state) + { + log(Info) << ecatStateToString(ec_slave[0].state) <<" state reached for all slaves." + << endlog(); + while (EcatError) + { + log(Error) << ec_elist2string() << endlog(); + } + } + else + { + log(Error) << "Not all slaves reached safe operational state." + << endlog(); + + //If not all slaves reached target state find out which one + for (int i = 0; i <= ec_slavecount; i++) + { + if (ec_slave[i].state != desired_state) + { + state_is_reached = false; + + log(Error) << "Slave " << i << " State= " << to_string( + ec_slave[i].state, std::hex) << " StatusCode=" + << ec_slave[i].ALstatuscode << " : " + << ec_ALstatuscode2string( + ec_slave[i].ALstatuscode) << endlog(); + } + } + } + + return state_is_reached; +} + +std::string SoemMasterComponent::ecatStateToString(uint16 ecat_state) +{ + std::string state_description; + + switch(ecat_state & 0x0f) + { + case EC_STATE_INIT: + state_description = "Init"; + break; + case EC_STATE_PRE_OP: + state_description = "PreOperational"; + break; + case EC_STATE_BOOT: + state_description = "PreOperational"; + break; + case EC_STATE_SAFE_OP: + state_description = "SafeOperational"; + break; + case EC_STATE_OPERATIONAL: + state_description = "SafeOperational"; + break; + default: + state_description = "Unknown"; + break; + } + + if (ecat_state & EC_STATE_ERROR) + state_description += "-Error"; + + return state_description; } }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h old mode 100644 new mode 100755 index 50adeb8..a16e2e0 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -60,8 +60,10 @@ class SoemMasterComponent: public RTT::TaskContext char m_IOmap[4096]; std::vector m_drivers; std::vector parameters; - int writeCoeSdo(const AddressInfo& address,bool completeAccess,int size,void* data); - int readCoeSdo(const AddressInfo& address,bool completeAccess,int* size,void* data); + int writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); + int readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); + bool checkNetworkState(ec_state desired_state, int timeout); + std::string ecatStateToString(uint16 ecat_state); };//class From ee637f73aea548b290fe6ce9081740c1976abb15 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 18 Oct 2016 17:21:04 +0200 Subject: [PATCH 09/13] Feature/coe parametrization (#2) * add soem_master_types.hpp * updated files containing features * minor space canges in soem_driver.h * tab alignment fix in soem_master_component * white spaces fixing * fixing marked issues for pr * fix for pr --- soem_master/soem_master_component.cpp | 246 ++++++++++++++++---------- soem_master/soem_master_component.h | 14 +- soem_master/soem_master_types.hpp | 22 +-- 3 files changed, 168 insertions(+), 114 deletions(-) mode change 100644 => 100755 soem_master/soem_master_component.cpp mode change 100644 => 100755 soem_master/soem_master_component.h diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp old mode 100644 new mode 100755 index 8e5854f..4332cf5 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -60,14 +60,14 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : this->addOperation("displayAvailableDrivers", &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( "display all available drivers for the soem master"); - this->addOperation("writeCoeSDO", &SoemMasterComponent::writeCoeSDO,this).doc( + this->addOperation("writeCoeSdo", &SoemMasterComponent::writeCoeSdo,this).doc( "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); - this->addOperation("readCoeSDO", &SoemMasterComponent::readCoeSDO,this).doc( + this->addOperation("readCoeSdo", &SoemMasterComponent::readCoeSdo,this).doc( "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); RTT::types::Types()->addType(new parameterTypeInfo()); - RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); + RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } @@ -104,34 +104,37 @@ bool SoemMasterComponent::configureHook() << endlog(); ec_slave[0].state = EC_STATE_PRE_OP; ec_writestate(0); - // wait for all slaves to reach PRE_OP state - ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + //Wait for all slaves to reach PRE_OP state + if(!checkNetworkState(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) + return false; - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); - } - - /* The parameter to be sent to the slaves are loaded */ + //The parameters to be sent to the slaves are loaded from the soem.cpf: + //parameters could be changed without modifying the code for (unsigned int i=0; i < parameters.size(); i++) { int wkc; - addressInfo tmp; - tmp.slavePosition = parameters[i].slavePosition; + AddressInfo tmp; + tmp.slave_position = parameters[i].slave_position; tmp.index = parameters[i].index; - tmp.subIndex = parameters[i].subIndex; - - wkc = writeCoeSDO(&tmp,parameters[i].completeAccess,parameters[i].size,¶meters[i].param); + tmp.sub_index = parameters[i].sub_index; - if(wkc == 0) + if(ec_slave[tmp.slave_position].mbx_proto & ECT_MBXPROT_COE) { - log(Error) << "Slave_" << tmp.slavePosition <<" SDOwrite{index["<< tmp.index - << "] subindex["<< (int)tmp.subIndex <<"] size "<< parameters[i].size - << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + wkc = writeCoeSdo(tmp,parameters[i].complete_access,parameters[i].size,¶meters[i].param); + + if(wkc == 0) + { + log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" SDOwrite{index["<< tmp.index + << "] sub_index["<< (int)tmp.sub_index <<"] size "<< parameters[i].size + << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); + } } + else + { + log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" does not support CoE" + << " but in soem.cpf there is a CoE parameter for this slave." << endlog(); + } + } for (int i = 1; i <= ec_slavecount; i++) @@ -197,48 +200,13 @@ bool SoemMasterComponent::configureHook() bool SoemMasterComponent::startHook() { + bool state_reached; + log(Info) << "Request safe-operational state for all slaves" << endlog(); ec_slave[0].state = EC_STATE_SAFE_OP; ec_writestate(0); // wait for all slaves to reach SAFE_OP state - ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - } - - if (ec_slave[0].state == EC_STATE_SAFE_OP) - { - log(Info) << "Safe operational state reached for all slaves." - << endlog(); - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } - } - else - { - log(Error) << "Not all slaves reached safe operational state." - << endlog(); - ec_readstate(); - //If not all slaves operational find out which one - for (int i = 0; i <= ec_slavecount; i++) - { - if (ec_slave[i].state != EC_STATE_SAFE_OP) - { - log(Error) << "Slave " << i << " State= " << to_string( - ec_slave[i].state, std::hex) << " StatusCode=" - << ec_slave[i].ALstatuscode << " : " - << ec_ALstatuscode2string( - ec_slave[i].ALstatuscode) << endlog(); - } - } - //return false; - } + checkNetworkState(EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); log(Info) << "Request operational state for all slaves" << endlog(); ec_slave[0].state = EC_STATE_OPERATIONAL; @@ -253,40 +221,9 @@ bool SoemMasterComponent::startHook() } // wait for all slaves to reach OP state - ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - - // The state should be verified for every slave because calling - // ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - // otherwise the member state of every slave wouldn't be updated - for(int i = 0; i <= ec_slavecount; i++) - { - ec_statecheck(i, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - } + if(!checkNetworkState(EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE)) + return false; - if (ec_slave[0].state == EC_STATE_OPERATIONAL) - { - log(Info) << "Operational state reached for all slaves." - << endlog(); - } - else - { - log(Error) << "Not all slaves reached operational state." - << endlog(); - //If not all slaves operational find out which one - for (int i = 1; i <= ec_slavecount; i++) - { - if (ec_slave[i].state != EC_STATE_OPERATIONAL) - { - log(Error) << "Slave " << i << " State= " << to_string( - ec_slave[i].state, std::hex) << " StatusCode=" - << ec_slave[i].ALstatuscode << " : " - << ec_ALstatuscode2string( - ec_slave[i].ALstatuscode) << endlog(); - } - } - return false; - - } return true; } @@ -328,15 +265,128 @@ void SoemMasterComponent::cleanupHook() ec_close(); } +int SoemMasterComponent::writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data) +{ + return ec_SDOwrite(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); +} + +int SoemMasterComponent::readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data) +{ + return ec_SDOread(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); +} -int SoemMasterComponent::writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data) +bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout) { - return ec_SDOwrite(address->slavePosition,address->index,address->subIndex,completeAccess,size, data,EC_TIMEOUTRXM); + bool state_is_reached = true; + bool error_detected = false; + + uint16 network_state = ec_statecheck(0, desired_state, timeout); + + if((network_state & 0xf0) == 0) + { + // No slave has toggled the error flag so the AlStatusCode (even if different from 0) should be ignored + for(int i = 0; i < ec_slavecount; i++) + { + ec_slave[i].ALstatuscode = 0x0000; + } + } + else + { + error_detected = true; + } + + switch(network_state) + { + case EC_STATE_INIT: + case EC_STATE_PRE_OP: + case EC_STATE_BOOT: + case EC_STATE_SAFE_OP: + case EC_STATE_OPERATIONAL: + if(!error_detected) + { + //All the slaves have reached the same state so we can update the state of every slave + for(int i = 0; i < ec_slavecount; i++) + { + ec_slave[i].state = network_state; + } + } + else + { + ec_readstate(); + } + + break; + + default: + //The state should be verified for every single slave + //since not all have the same state + ec_readstate(); + break; + } + + if (ec_slave[0].state == desired_state) + { + log(Info) << ecatStateToString(ec_slave[0].state) <<" state reached for all slaves." + << endlog(); + while (EcatError) + { + log(Error) << ec_elist2string() << endlog(); + } + } + else + { + log(Error) << "Not all slaves reached safe operational state." + << endlog(); + + //If not all slaves reached target state find out which one + for (int i = 0; i <= ec_slavecount; i++) + { + if (ec_slave[i].state != desired_state) + { + state_is_reached = false; + + log(Error) << "Slave " << i << " State= " << to_string( + ec_slave[i].state, std::hex) << " StatusCode=" + << ec_slave[i].ALstatuscode << " : " + << ec_ALstatuscode2string( + ec_slave[i].ALstatuscode) << endlog(); + } + } + } + + return state_is_reached; } -int SoemMasterComponent::readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data) +std::string SoemMasterComponent::ecatStateToString(uint16 ecat_state) { - return ec_SDOread(address->slavePosition,address->index,address->subIndex,completeAccess, size, data,EC_TIMEOUTRXM); + std::string state_description; + + switch(ecat_state & 0x0f) + { + case EC_STATE_INIT: + state_description = "Init"; + break; + case EC_STATE_PRE_OP: + state_description = "PreOperational"; + break; + case EC_STATE_BOOT: + state_description = "PreOperational"; + break; + case EC_STATE_SAFE_OP: + state_description = "SafeOperational"; + break; + case EC_STATE_OPERATIONAL: + state_description = "SafeOperational"; + break; + default: + state_description = "Unknown"; + break; + } + + if (ecat_state & EC_STATE_ERROR) + state_description += "-Error"; + + return state_description; } }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h old mode 100644 new mode 100755 index bb31ce2..a16e2e0 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -31,11 +31,11 @@ namespace soem_master { /** CoE addressing info */ -struct addressInfo +struct AddressInfo { - unsigned short slavePosition; + unsigned short slave_position; unsigned short index; - unsigned char subIndex; + unsigned char sub_index; }; class SoemMasterComponent: public RTT::TaskContext @@ -59,9 +59,11 @@ class SoemMasterComponent: public RTT::TaskContext bool prop_redundant; char m_IOmap[4096]; std::vector m_drivers; - std::vector parameters; - int writeCoeSDO(addressInfo* address,bool completeAccess,int size,void* data); - int readCoeSDO(addressInfo* address,bool completeAccess,int* size,void* data); + std::vector parameters; + int writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); + int readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); + bool checkNetworkState(ec_state desired_state, int timeout); + std::string ecatStateToString(uint16 ecat_state); };//class diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp index f2a49a7..be2bf47 100644 --- a/soem_master/soem_master_types.hpp +++ b/soem_master/soem_master_types.hpp @@ -27,17 +27,18 @@ #include #include +namespace rtt_soem { /** The structure that contains the information to be sent for each CoE SDO */ -struct parameter +struct Parameter { /** slave's index starting from 1 and depending on position */ - int slavePosition; + int slave_position; /** Index of the CoE object */ int index; /** Subindex of the CoE object */ - int subIndex; + int sub_index; /** Falg to enable complete access */ - bool completeAccess; + bool complete_access; /** Size of the CoE object to be written in bytes */ int size; /** The value of the parameter to be written (TODO change to a vector of chars to send parameters of any dimension)*/ @@ -46,6 +47,7 @@ struct parameter std::string description; }; +} //################################################################ @@ -53,12 +55,12 @@ namespace boost { namespace serialization { // The helper function which you write yourself: template -void serialize( Archive & a, parameter & cd, unsigned int) { +void serialize( Archive & a, rtt_soem::Parameter & cd, unsigned int) { using boost::serialization::make_nvp; -a & make_nvp("slavePosition", cd.slavePosition); +a & make_nvp("slavePosition", cd.slave_position); a & make_nvp("index", cd.index); -a & make_nvp("subIndex", cd.subIndex); -a & make_nvp("completeAccess", cd.completeAccess); +a & make_nvp("subIndex", cd.sub_index); +a & make_nvp("completeAccess", cd.complete_access); a & make_nvp("size", cd.size); a & make_nvp("param", cd.param); a & make_nvp("name", cd.name); @@ -70,10 +72,10 @@ a & make_nvp("description", cd.description); // The RTT helper class which uses the above function behind the scenes: struct parameterTypeInfo -: public RTT::types::StructTypeInfo +: public RTT::types::StructTypeInfo { parameterTypeInfo() -: RTT::types::StructTypeInfo("parameter") +: RTT::types::StructTypeInfo("Parameter") {} }; From 40615faab793e99ac71fabb6f77f8da8183b9f2e Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Thu, 20 Oct 2016 09:44:07 +0200 Subject: [PATCH 10/13] improved enum ec_state management --- soem_master/soem_driver.h | 2 + soem_master/soem_master_component.cpp | 63 +++++++++++---------------- soem_master/soem_master_component.h | 2 - soem_master/soem_master_types.hpp | 58 ++++++++++++++++++++++++ 4 files changed, 86 insertions(+), 39 deletions(-) diff --git a/soem_master/soem_driver.h b/soem_master/soem_driver.h index 6b86c2f..70e5048 100644 --- a/soem_master/soem_driver.h +++ b/soem_master/soem_driver.h @@ -45,6 +45,8 @@ extern "C" #include +#include "soem_master_types.hpp" + template inline std::string to_string(const T& t, std::ios_base & (*f)(std::ios_base&)) { diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 4332cf5..d25cf8c 100755 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -35,7 +35,6 @@ extern "C" #include "soem_master_component.h" #include "rtt/Component.hpp" -#include "rtt/types/EnumTypeInfo.hpp" ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) @@ -65,7 +64,29 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : this->addOperation("readCoeSdo", &SoemMasterComponent::readCoeSdo,this).doc( "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); - RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); + //RTT::types::Types()->addType(new types::EnumTypeInfo("ec_state")); + + RTT::types::GlobalsRepository::shared_ptr globals = RTT::types::GlobalsRepository::Instance(); + + globals->setValue( new Constant("EC_STATE_INIT",EC_STATE_INIT) ); + globals->setValue( new Constant("EC_STATE_PRE_OP",EC_STATE_PRE_OP) ); + globals->setValue( new Constant("EC_STATE_SAFE_OP",EC_STATE_SAFE_OP) ); + globals->setValue( new Constant("EC_STATE_OPERATIONAL",EC_STATE_OPERATIONAL) ); + globals->setValue( new Constant("EC_STATE_BOOT",EC_STATE_BOOT) ); + + //AssignableDataSource::shared_ptr init ; + //AssignableDataSource::shared_ptr preop ; + //AssignableDataSource::shared_ptr boot ; + //AssignableDataSource::shared_ptr safeop ; + //AssignableDataSource::shared_ptr op ; + + // init = new ValueDataSource( EC_STATE_INIT ); + // preop = new ValueDataSource( EC_STATE_PRE_OP ); + // boot = new ValueDataSource( EC_STATE_SAFE_OP ); + // safeop = new ValueDataSource( EC_STATE_OPERATIONAL ); + // op = new ValueDataSource( EC_STATE_BOOT ); + + RTT::types::Types()->addType(new ec_stateTypeInfo()); RTT::types::Types()->addType(new parameterTypeInfo()); RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); @@ -326,7 +347,7 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout if (ec_slave[0].state == desired_state) { - log(Info) << ecatStateToString(ec_slave[0].state) <<" state reached for all slaves." + log(Info) << (ec_state)ec_slave[0].state <<" state reached for all slaves." << endlog(); while (EcatError) { @@ -345,8 +366,8 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout { state_is_reached = false; - log(Error) << "Slave " << i << " State= " << to_string( - ec_slave[i].state, std::hex) << " StatusCode=" + log(Error) << "Slave " << i << " State= " << + (ec_state)ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string( ec_slave[i].ALstatuscode) << endlog(); @@ -357,36 +378,4 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout return state_is_reached; } -std::string SoemMasterComponent::ecatStateToString(uint16 ecat_state) -{ - std::string state_description; - - switch(ecat_state & 0x0f) - { - case EC_STATE_INIT: - state_description = "Init"; - break; - case EC_STATE_PRE_OP: - state_description = "PreOperational"; - break; - case EC_STATE_BOOT: - state_description = "PreOperational"; - break; - case EC_STATE_SAFE_OP: - state_description = "SafeOperational"; - break; - case EC_STATE_OPERATIONAL: - state_description = "SafeOperational"; - break; - default: - state_description = "Unknown"; - break; - } - - if (ecat_state & EC_STATE_ERROR) - state_description += "-Error"; - - return state_description; -} - }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index a16e2e0..c8350ed 100755 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -25,7 +25,6 @@ #include #include "soem_driver.h" -#include "soem_master_types.hpp" namespace soem_master { @@ -63,7 +62,6 @@ class SoemMasterComponent: public RTT::TaskContext int writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); int readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); bool checkNetworkState(ec_state desired_state, int timeout); - std::string ecatStateToString(uint16 ecat_state); };//class diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp index be2bf47..856115f 100644 --- a/soem_master/soem_master_types.hpp +++ b/soem_master/soem_master_types.hpp @@ -26,6 +26,15 @@ #include #include #include +#include +#include +#include + +extern "C" +{ +#include "ethercattype.h" +} + namespace rtt_soem { /** The structure that contains the information to be sent for each CoE SDO */ @@ -79,3 +88,52 @@ parameterTypeInfo() {} }; +//################################################################ +// To manage ec_state enum + +// Displaying: +std::ostream& operator<<(std::ostream& os, const ec_state& ecat_state) { + switch(ecat_state & 0x0f) + { + case EC_STATE_INIT: + os << "EC_STATE_INIT"; + break; + case EC_STATE_PRE_OP: + os << "EC_STATE_PRE_OP"; + break; + case EC_STATE_BOOT: + os << "EC_STATE_BOOT"; + break; + case EC_STATE_SAFE_OP: + os << "EC_STATE_SAFE_OP"; + break; + case EC_STATE_OPERATIONAL: + os << "EC_STATE_OPERATIONAL"; + break; + default: + os << "EC_STATE_UNKNOWN"; + break; + } + + if (ecat_state & EC_STATE_ERROR) + os << "-Error"; + + return os; +} +// Reading : +std::istream& operator>>(std::istream& is, ec_state& cd) { +return is; +} + +struct ec_stateTypeInfo: public +RTT::types::TemplateTypeInfo +{ +ec_stateTypeInfo() : +RTT::types::TemplateTypeInfo ("ec_state") +{ +} + +}; + + + From f7ccbfeaf48b0b8015efb7785534b80b5127f13a Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 20 Oct 2016 15:06:34 +0200 Subject: [PATCH 11/13] minor bug fix --- soem_master/soem_master_component.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 70425e9..0f670ce 100755 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -311,7 +311,7 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout if(!error_detected) { //All the slaves have reached the same state so we can update the state of every slave - for(int i = 0; i < ec_slavecount; i++) + for(int i = 1; i <= ec_slavecount; i++) { ec_slave[i].state = network_state; } @@ -320,7 +320,6 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout { ec_readstate(); } - break; default: @@ -345,7 +344,7 @@ bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout << endlog(); //If not all slaves reached target state find out which one - for (int i = 0; i <= ec_slavecount; i++) + for (int i = 1; i <= ec_slavecount; i++) { if (ec_slave[i].state != desired_state) { From 65bbbd553690dbc4bf767a6ae7ea73d081593642 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Fri, 21 Oct 2016 14:10:40 +0200 Subject: [PATCH 12/13] fix style and SDO functions management --- soem_master/soem_driver.h | 2 - soem_master/soem_master_component.cpp | 441 +++++++++++++++----------- soem_master/soem_master_component.h | 64 +++- soem_master/soem_master_types.hpp | 99 +++--- 4 files changed, 352 insertions(+), 254 deletions(-) diff --git a/soem_master/soem_driver.h b/soem_master/soem_driver.h index 70e5048..6b86c2f 100644 --- a/soem_master/soem_driver.h +++ b/soem_master/soem_driver.h @@ -45,8 +45,6 @@ extern "C" #include -#include "soem_master_types.hpp" - template inline std::string to_string(const T& t, std::ios_base & (*f)(std::ios_base&)) { diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 0f670ce..6c78e33 100755 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -44,38 +44,53 @@ namespace soem_master using namespace RTT; SoemMasterComponent::SoemMasterComponent(const std::string& name) : - TaskContext(name, PreOperational) -{ - this->addProperty("ifname", prop_ifname1="eth0").doc( - "interface to which the ethercat device is connected"); - this->addProperty("ifname2", prop_ifname2="eth1").doc( - "Second (redundant) interface to which the ethercat device is connected"); - this->addProperty("redundant", prop_redundant=false).doc( - "Whether to use a redundant nic"); - this->addProperty("slavesCoeParameters",parameters).doc( - "Vector of parameters to be sent to the slaves using CoE SDO"); - - SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); - this->addOperation("displayAvailableDrivers", - &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( - "display all available drivers for the soem master"); - this->addOperation("writeCoeSdo", &SoemMasterComponent::writeCoeSdo,this).doc( - "send a CoE SDO write (blocking: not to be done while slaves are in OP)"); - this->addOperation("readCoeSdo", &SoemMasterComponent::readCoeSdo,this).doc( - "send a CoE SDO read (blocking: not to be done while slaves are in OP)"); - - RTT::types::GlobalsRepository::shared_ptr globals = RTT::types::GlobalsRepository::Instance(); - globals->setValue( new Constant("EC_STATE_INIT",EC_STATE_INIT) ); - globals->setValue( new Constant("EC_STATE_PRE_OP",EC_STATE_PRE_OP) ); - globals->setValue( new Constant("EC_STATE_SAFE_OP",EC_STATE_SAFE_OP) ); - globals->setValue( new Constant("EC_STATE_OPERATIONAL",EC_STATE_OPERATIONAL) ); - globals->setValue( new Constant("EC_STATE_BOOT",EC_STATE_BOOT) ); - - RTT::types::Types()->addType(new ec_stateTypeInfo()); - RTT::types::Types()->addType(new parameterTypeInfo()); - RTT::types::Types()->addType(new types::SequenceTypeInfo< std::vector >("std.vector")); - - //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); + TaskContext(name, PreOperational) { + this->addProperty("ifname", prop_ifname1 = "eth0").doc( + "interface to which the ethercat device is connected"); + this->addProperty("ifname2", prop_ifname2 = "eth1").doc( + "Second (redundant) interface to which the ethercat device is connected"); + this->addProperty("redundant", prop_redundant = false).doc( + "Whether to use a redundant nic"); + this->addProperty("slavesCoeParameters", parameters).doc( + "Vector of parameters to be sent to the slaves using CoE SDO"); + + SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); + this->addOperation("displayAvailableDrivers", + &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( + "display all available drivers for the soem master"); + this->addOperation("writeCoeSdo", &SoemMasterComponent::writeCoeSdo, this).doc( + "send a CoE SDO write " + "(blocking: not to be done while slaves are in OP)"); + this->addOperation("readCoeSdo", &SoemMasterComponent::readCoeSdo, this).doc( + "send a CoE SDO read " + "(blocking: not to be done while slaves are in OP)"); + + // To teach RTT the ec_state constants + RTT::types::GlobalsRepository::shared_ptr globals = + RTT::types::GlobalsRepository::Instance(); + globals->setValue(new Constant("EC_STATE_INIT", EC_STATE_INIT)); + globals->setValue(new Constant("EC_STATE_PRE_OP", EC_STATE_PRE_OP)); + globals->setValue( + new Constant("EC_STATE_SAFE_OP", EC_STATE_SAFE_OP)); + globals->setValue( + new Constant("EC_STATE_OPERATIONAL", EC_STATE_OPERATIONAL)); + globals->setValue(new Constant("EC_STATE_BOOT", EC_STATE_BOOT)); + + RTT::types::Types()->addType(new ec_stateTypeInfo()); + + // To teach RTT SOEM fixed length types + RTT::types::Types()->addType(new types::StdTypeInfo("uint8")); + RTT::types::Types()->addType(new types::StdTypeInfo("uint16")); + RTT::types::Types()->addType(new types::StdTypeInfo("uint32")); + + // To teach RTT how to manage the CoE parameters + // that can be load from soem.cpf + RTT::types::Types()->addType(new parameterTypeInfo()); + RTT::types::Types()->addType( + new types::SequenceTypeInfo >( + "std.vector")); + + //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } SoemMasterComponent::~SoemMasterComponent() @@ -91,56 +106,46 @@ bool SoemMasterComponent::configureHook() ret = ec_init_redundant((char*)prop_ifname1.c_str(),(char*)prop_ifname2.c_str()); else ret = ec_init((char*)prop_ifname1.c_str()); - // initialise SOEM, bind socket to ifname - if ( ret > 0) + // Initialise SOEM, bind socket to ifname + if (ret > 0) { log(Info) << "ec_init on " << prop_ifname1 << (prop_redundant ? std::string("and ") + prop_ifname2 : "")<<" succeeded." << endlog(); - //Initialise default configuration, using the default config table (see ethercatconfiglist.h) + // Initialise default configuration, using the default config table (see ethercatconfiglist.h) if (ec_config_init(FALSE) > 0) { - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } + if (EcatError) + notifySoemErrors(); log(Info) << ec_slavecount << " slaves found and configured." << endlog(); - log(Info) << "Request pre-operational state for all slaves" - << endlog(); - ec_slave[0].state = EC_STATE_PRE_OP; - ec_writestate(0); - //Wait for all slaves to reach PRE_OP state - if(!checkNetworkState(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) + + setSlavesTargetState(EC_STATE_PRE_OP); + // Wait for all slaves to reach PRE_OP state + if(!checkSlavesStateReachedWaiting(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) return false; - //The parameters to be sent to the slaves are loaded from the soem.cpf: - //parameters could be changed without modifying the code + // The parameters to be sent to the slaves are loaded from the soem.cpf: + // parameters could be changed without modifying the code for (unsigned int i=0; i < parameters.size(); i++) { - int wkc; - AddressInfo tmp; - tmp.slave_position = parameters[i].slave_position; - tmp.index = parameters[i].index; - tmp.sub_index = parameters[i].sub_index; - - if(ec_slave[tmp.slave_position].mbx_proto & ECT_MBXPROT_COE) - { - wkc = writeCoeSdo(tmp,parameters[i].complete_access,parameters[i].size,¶meters[i].param); - - if(wkc == 0) - { - log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" SDOwrite{index["<< tmp.index - << "] sub_index["<< (int)tmp.sub_index <<"] size "<< parameters[i].size - << " value "<< parameters[i].param << "} wkc "<< wkc << endlog(); - } - } - else + bool sdoWriteDone; + AddressInfo tmpAddressInfo; + tmpAddressInfo.slave_position = (uint16)parameters[i].slave_position; + tmpAddressInfo.index = (uint16)parameters[i].index; + tmpAddressInfo.sub_index = (uint8)parameters[i].sub_index; + + sdoWriteDone = writeCoeSdo(tmpAddressInfo, + parameters[i].complete_access, + parameters[i].size, + ¶meters[i].param); + + if (!sdoWriteDone) { - log(Error) << "Slave_" << ec_slave[tmp.slave_position].configadr <<" does not support CoE" - << " but in soem.cpf there is a CoE parameter for this slave." << endlog(); + log(Error) << "SDO write requested from soem.cpf failed." + << endlog(); + return false; } - } for (int i = 1; i <= ec_slavecount; i++) @@ -154,7 +159,7 @@ bool SoemMasterComponent::configureHook() log(Info) << "Created driver for " << ec_slave[i].name << ", with address " << ec_slave[i].configadr << endlog(); - //Adding driver's services to master component + // Adding driver's services to master component this->provides()->addService(driver->provides()); log(Info) << "Put configured parameters in the slaves." << endlog(); @@ -169,10 +174,8 @@ bool SoemMasterComponent::configureHook() } ec_config_map(&m_IOmap); - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } + if (EcatError) + notifySoemErrors(); for (unsigned int i = 0; i < m_drivers.size(); i++) if (!m_drivers[i]->start()){ @@ -181,17 +184,20 @@ bool SoemMasterComponent::configureHook() } - //Configure distributed clock - //ec_configdc(); - //Read the state of all slaves + // Configure distributed clock + // ec_configdc(); + // Read the state of all slaves - //ec_readstate(); + // ec_readstate(); } else { - log(Error) << "Configuration of slaves failed!!!" << endlog(); - log(Error) << "The NIC currently used for EtherCAT is "<< prop_ifname1.c_str() << " . Another could be chosen by editing soem.cpf." << endlog(); + log(Error) << "Configuration of slaves failed!!! \n" + << "The NIC currently used for EtherCAT is " + << prop_ifname1.c_str() + << ". Another could be chosen by editing soem.cpf." + << endlog(); return false; } return true; @@ -204,60 +210,59 @@ bool SoemMasterComponent::configureHook() } -bool SoemMasterComponent::startHook() -{ - bool state_reached; +bool SoemMasterComponent::startHook() { + bool state_reached; - log(Info) << "Request safe-operational state for all slaves" << endlog(); - ec_slave[0].state = EC_STATE_SAFE_OP; - ec_writestate(0); - // wait for all slaves to reach SAFE_OP state - checkNetworkState(EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + setSlavesTargetState(EC_STATE_SAFE_OP); + // Wait for all slaves to reach SAFE_OP state + state_reached = checkSlavesStateReachedWaiting(EC_STATE_SAFE_OP, + EC_TIMEOUTSTATE); - log(Info) << "Request operational state for all slaves" << endlog(); - ec_slave[0].state = EC_STATE_OPERATIONAL; + if (EcatError) + notifySoemErrors(); - // send one valid process data to make outputs in slaves happy - ec_send_processdata(); + if (!state_reached) + return false; - ec_writestate(0); - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } + // Send one valid process data to make outputs in slaves happy + ec_send_processdata(); - // wait for all slaves to reach OP state - if(!checkNetworkState(EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE)) - return false; + setSlavesTargetState(EC_STATE_OPERATIONAL); - return true; -} + if (EcatError) + notifySoemErrors(); -void SoemMasterComponent::updateHook() -{ - bool success = true; - Logger::In in(this->getName()); - if (ec_receive_processdata(EC_TIMEOUTRET) == 0) - { - success = false; - log(Warning) << "receiving data failed" << endlog(); - } + // Wait for all slaves to reach OP state + state_reached = checkSlavesStateReachedWaiting(EC_STATE_OPERATIONAL, + EC_TIMEOUTSTATE); - if (success) - for (unsigned int i = 0; i < m_drivers.size(); i++) - m_drivers[i]->update(); + if (EcatError) + notifySoemErrors(); - if (ec_send_processdata() == 0) - { - success = false; - log(Warning) << "sending process data failed" << endlog(); - } - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } + if (!state_reached) + return false; + + return true; +} + +void SoemMasterComponent::updateHook() { + bool success = true; + Logger::In in(this->getName()); + if (ec_receive_processdata(EC_TIMEOUTRET) == 0) { + success = false; + log(Warning) << "receiving data failed" << endlog(); + } + if (success) + for (unsigned int i = 0; i < m_drivers.size(); i++) + m_drivers[i]->update(); + if (ec_send_processdata() == 0) { + success = false; + log(Warning) << "sending process data failed" << endlog(); + } + if (EcatError) + notifySoemErrors(); } void SoemMasterComponent::cleanupHook() @@ -267,99 +272,155 @@ void SoemMasterComponent::cleanupHook() delete m_drivers[i]; } - //stop SOEM, close socket + // stop SOEM, close socket ec_close(); } -int SoemMasterComponent::writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data) -{ - return ec_SDOwrite(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); +bool SoemMasterComponent::writeCoeSdo(const AddressInfo& address, + bool complete_access, int size, void* data) { + if (ec_slave[address.slave_position].mbx_proto & ECT_MBXPROT_COE) { + // Working counter of the EtherCAT datagram that ends the CoE + // download procedure. + // This value is written into the frame by the slave and is used + // to confirm that the command has been executed. + // In this case the expected working counter is 1. + int working_counter = ec_SDOwrite(address.slave_position, address.index, + address.sub_index, complete_access, size, data, EC_TIMEOUTRXM); + + if (working_counter == 1) + return true; + else { + log(Warning) << "Slave_" << ec_slave[address.slave_position].configadr + << " CoE SDO write failed for {index[" << address.index + << "] sub_index[" << (int) address.sub_index << "] size " << size + << "} wkc " << working_counter << endlog(); + + return false; + } + } else { + log(Error) << "Slave_" << ec_slave[address.slave_position].configadr + << " does not support CoE" << " but a CoE SDO write has been requested" + " for this slave." << endlog(); + + return false; + } } -int SoemMasterComponent::readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data) -{ - return ec_SDOread(address.slave_position,address.index,address.sub_index,complete_access,size,data,EC_TIMEOUTRXM); +bool SoemMasterComponent::readCoeSdo(const AddressInfo& address, + bool complete_access, int* size, void* data) { + if (ec_slave[address.slave_position].mbx_proto & ECT_MBXPROT_COE) { + // Working counter of the EtherCAT datagram that ends the CoE + // download procedure. + // This value is written into the frame by the slave and is used + // to confirm that the command has been executed. + // In this case the expected working counter is 1. + int working_counter = ec_SDOread(address.slave_position, address.index, + address.sub_index, complete_access, size, data, EC_TIMEOUTRXM); + + if (working_counter == 1) + return true; + else { + log(Warning) << "Slave_" << ec_slave[address.slave_position].configadr + << " CoE SDO read failed for {index[" << address.index + << "] sub_index[" << (int) address.sub_index << "] size " << size + << "} wkc " << working_counter << endlog(); + + return false; + } + } else { + log(Error) << "Slave_" << ec_slave[address.slave_position].configadr + << " does not support CoE" << " but a CoE SDO read has been requested" + " for this slave." << endlog(); + + return false; + } } -bool SoemMasterComponent::checkNetworkState(ec_state desired_state, int timeout) -{ +void SoemMasterComponent::setSlavesTargetState(ec_state target_state) { + log(Info) << "Request" << target_state << " state for all slaves" << endlog(); + ec_slave[0].state = (uint16) target_state; + ec_writestate(0); +} + +bool SoemMasterComponent::checkSlavesStateReachedWaiting(ec_state desired_state, + int timeout) { bool state_is_reached = true; bool error_detected = false; uint16 network_state = ec_statecheck(0, desired_state, timeout); - if((network_state & 0xf0) == 0) - { - // No slave has toggled the error flag so the AlStatusCode (even if different from 0) should be ignored - for(int i = 0; i < ec_slavecount; i++) - { + if ((network_state & EC_STATE_ERROR) == 0) { + // No slave has toggled the error flag so the AlStatusCode + // (even if different from 0) should be ignored + for (int i = 0; i < ec_slavecount; i++) { ec_slave[i].ALstatuscode = 0x0000; } - } - else - { + } else { error_detected = true; } - switch(network_state) - { - case EC_STATE_INIT: - case EC_STATE_PRE_OP: - case EC_STATE_BOOT: - case EC_STATE_SAFE_OP: - case EC_STATE_OPERATIONAL: - if(!error_detected) - { - //All the slaves have reached the same state so we can update the state of every slave - for(int i = 1; i <= ec_slavecount; i++) - { - ec_slave[i].state = network_state; - } - } - else - { - ec_readstate(); + switch (network_state) { + case EC_STATE_INIT: + case EC_STATE_PRE_OP: + case EC_STATE_BOOT: + case EC_STATE_SAFE_OP: + case EC_STATE_OPERATIONAL: + if (!error_detected) { + // All the slaves have reached the same state so we can update + // the state of every slave + for (int i = 1; i <= ec_slavecount; i++) { + ec_slave[i].state = network_state; } - break; - - default: - //The state should be verified for every single slave - //since not all have the same state + } else { + // The state should be verified for every single slave + // since at least one of them is in error ec_readstate(); - break; - } + } + break; - if (ec_slave[0].state == desired_state) - { - log(Info) << (ec_state)ec_slave[0].state <<" state reached for all slaves." - << endlog(); - while (EcatError) - { - log(Error) << ec_elist2string() << endlog(); - } + default: + // The state should be verified for every single slave + // since not all have the same state + ec_readstate(); + break; } - else - { - log(Error) << "Not all slaves reached safe operational state." - << endlog(); - - //If not all slaves reached target state find out which one - for (int i = 1; i <= ec_slavecount; i++) - { - if (ec_slave[i].state != desired_state) - { - state_is_reached = false; - - log(Error) << "Slave " << i << " State= " << - (ec_state)ec_slave[i].state << " StatusCode=" - << ec_slave[i].ALstatuscode << " : " - << ec_ALstatuscode2string( - ec_slave[i].ALstatuscode) << endlog(); - } + + if (ec_slave[0].state == desired_state) { + log(Info) << (ec_state) ec_slave[0].state + << " state reached for all slaves." << endlog(); + } else { + log(Error) << "Not all slaves reached" << desired_state << endlog(); + + // If not all slaves reached target state find out which one + // It may happens that since more time is passing all the slaves reach the + // target state before reading their state one by one. + // In that case the timeout should be increased + for (int i = 1; i <= ec_slavecount; i++) { + if (ec_slave[i].state != desired_state) { + state_is_reached = false; + + log(Error) << "Slave " << i << " State= " + << (ec_state) ec_slave[i].state << " StatusCode=" + << ec_slave[i].ALstatuscode << " : " + << ec_ALstatuscode2string(ec_slave[i].ALstatuscode) << endlog(); } + } } return state_is_reached; } +bool SoemMasterComponent::notifySoemErrors() { + // The EcatError flag is updated by SOEM library and set to 0 if + // in the SOEM error list there are no more errors + bool errorDetected = EcatError; + + // All the errors are retrieved from the list and notified + while (EcatError) { + log(Error) << ec_elist2string() << endlog(); + } + + return errorDetected; +} + }//namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index c8350ed..b1f8279 100755 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -25,6 +25,7 @@ #include #include "soem_driver.h" +#include "soem_master_types.hpp" namespace soem_master { @@ -32,9 +33,12 @@ namespace soem_master /** CoE addressing info */ struct AddressInfo { - unsigned short slave_position; - unsigned short index; - unsigned char sub_index; + /** Position of the slave in the EtherCAT network starting from 1 */ + uint16 slave_position; + /** Index of the CoE object */ + uint16 index; + /** Subindex of the CoE object*/ + uint8 sub_index; }; class SoemMasterComponent: public RTT::TaskContext @@ -58,10 +62,56 @@ class SoemMasterComponent: public RTT::TaskContext bool prop_redundant; char m_IOmap[4096]; std::vector m_drivers; - std::vector parameters; - int writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); - int readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); - bool checkNetworkState(ec_state desired_state, int timeout); + std::vector parameters; + + /** + * Perform a CoE SDO write (blocking) + * @param[in] address = structure that identifies the slave and the object + * @param[in] complete_access = FALSE = single subindex. + * TRUE = Complete Access, all subindexes written. + * @param[in] size = Size in bytes of the object to be written + * @param[in] data = Pointer to the data to be written + * @return true if it succeeds false if it fails + */ + bool writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); + + /** + * Perform a CoE SDO read (blocking) + * @param[in] address = structure that identifies the slave and the object + * @param[in] complete_access = FALSE = single subindex. + * TRUE = Complete Access, all subindexes written. + * @param[in,out] size = Size in bytes of the read object + * @param[out] data = Pointer to the read data + * @return true if it succeeds false if it fails + */ + bool readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); + + /** + * Set the EtherCAT target state to be reached by all the slaves + * @param[in] target_state = target EtherCAT state + */ + void setSlavesTargetState(ec_state target_state); + + /** + * Keep refreshing and checking the network state till all slaves reach the + * desired state or a certain amount of time has elapsed (blocking) + * @param[in] desired_state = target EtherCAT state + * @param[in] timeout = timeout in ms + * @return true if the state has been reached within the set timeout + */ + bool checkSlavesStateReachedWaiting(ec_state desired_state, int timeout); + + /** + * Notify if SOEM library detected an error. (non blocking) + * This function is not blocking because it simply retrieve if an error has + * already been detected. + * Possible errors are: packet errors, CoE Emergency messages, + * CoE SDO errors, FoE errors and SoE errors + * Note: Only packets error are possible in EC_STATE_OPERATIONAL if the User + * does not explicitly require a mail box service (CoE, FoE, SoE) + * @return true if at least one error has been detected + */ + bool notifySoemErrors(); };//class diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp index cf540fb..398bda1 100644 --- a/soem_master/soem_master_types.hpp +++ b/soem_master/soem_master_types.hpp @@ -35,8 +35,7 @@ extern "C" namespace rtt_soem { /** The structure that contains the information to be sent for each CoE SDO */ -struct Parameter -{ +struct Parameter { /** slave's index starting from 1 and depending on position */ int slave_position; /** Index of the CoE object */ @@ -47,7 +46,9 @@ struct Parameter bool complete_access; /** Size of the CoE object to be written in bytes */ int size; - /** The value of the parameter to be written (TODO change to a vector of chars to send parameters of any dimension)*/ + /** The value of the parameter to be written + * (TODO change to a vector of chars to send parameters of any dimension) + */ int param; std::string name; std::string description; @@ -55,82 +56,70 @@ struct Parameter }; } -//################################################################ - namespace boost { namespace serialization { // The helper function which you write yourself: template -void serialize( Archive & a, rtt_soem::Parameter & cd, unsigned int) { -using boost::serialization::make_nvp; -a & make_nvp("slavePosition", cd.slave_position); -a & make_nvp("index", cd.index); -a & make_nvp("subIndex", cd.sub_index); -a & make_nvp("completeAccess", cd.complete_access); -a & make_nvp("size", cd.size); -a & make_nvp("param", cd.param); -a & make_nvp("name", cd.name); -a & make_nvp("description", cd.description); +void serialize(Archive & a, rtt_soem::Parameter & cd, unsigned int) { + using boost::serialization::make_nvp; + a & make_nvp("slavePosition", cd.slave_position); + a & make_nvp("index", cd.index); + a & make_nvp("subIndex", cd.sub_index); + a & make_nvp("completeAccess", cd.complete_access); + a & make_nvp("size", cd.size); + a & make_nvp("param", cd.param); + a & make_nvp("name", cd.name); + a & make_nvp("description", cd.description); } } } // The RTT helper class which uses the above function behind the scenes: -struct parameterTypeInfo -: public RTT::types::StructTypeInfo -{ -parameterTypeInfo() -: RTT::types::StructTypeInfo("Parameter") -{} -}; +struct parameterTypeInfo: public RTT::types::StructTypeInfo { + parameterTypeInfo() : + RTT::types::StructTypeInfo("Parameter") { + } +}; -//################################################################ // To manage ec_state enum // Displaying: std::ostream& operator<<(std::ostream& os, const ec_state& ecat_state) { - switch(ecat_state & 0x0f) - { - case EC_STATE_INIT: - os << "EC_STATE_INIT"; - break; - case EC_STATE_PRE_OP: - os << "EC_STATE_PRE_OP"; - break; - case EC_STATE_BOOT: - os << "EC_STATE_BOOT"; - break; - case EC_STATE_SAFE_OP: - os << "EC_STATE_SAFE_OP"; - break; - case EC_STATE_OPERATIONAL: - os << "EC_STATE_OPERATIONAL"; - break; - default: - os << "EC_STATE_UNKNOWN"; - break; + switch (ecat_state & 0x0f) { + case EC_STATE_INIT: + os << "EC_STATE_INIT"; + break; + case EC_STATE_PRE_OP: + os << "EC_STATE_PRE_OP"; + break; + case EC_STATE_BOOT: + os << "EC_STATE_BOOT"; + break; + case EC_STATE_SAFE_OP: + os << "EC_STATE_SAFE_OP"; + break; + case EC_STATE_OPERATIONAL: + os << "EC_STATE_OPERATIONAL"; + break; + default: + os << "EC_STATE_UNKNOWN"; + break; } if (ecat_state & EC_STATE_ERROR) os << "-Error"; - return os; + return os; } // Reading : std::istream& operator>>(std::istream& is, ec_state& cd) { -return is; + return is; } -struct ec_stateTypeInfo: public -RTT::types::TemplateTypeInfo -{ -ec_stateTypeInfo() : -RTT::types::TemplateTypeInfo ("ec_state") -{ -} +struct ec_stateTypeInfo: public RTT::types::TemplateTypeInfo { + ec_stateTypeInfo() : + RTT::types::TemplateTypeInfo("ec_state") { + } }; - - - From 88f0907e730bd20a0c4103361a2fef666af3aa51 Mon Sep 17 00:00:00 2001 From: MagnaboscoL Date: Fri, 21 Oct 2016 14:33:26 +0200 Subject: [PATCH 13/13] style fixed --- soem_master/soem_master_component.cpp | 221 ++++++++++++-------------- soem_master/soem_master_component.h | 151 +++++++++--------- soem_master/soem_master_types.hpp | 3 +- 3 files changed, 176 insertions(+), 199 deletions(-) diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index 6c78e33..8960d30 100755 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -17,8 +17,7 @@ // License along with this library; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -extern "C" -{ +extern "C" { #include "ethercattype.h" #include "nicdrv.h" #include "ethercatbase.h" @@ -36,10 +35,9 @@ extern "C" #include "rtt/Component.hpp" -ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) +ORO_CREATE_COMPONENT(soem_master::SoemMasterComponent) -namespace soem_master -{ +namespace soem_master { using namespace RTT; @@ -93,120 +91,104 @@ SoemMasterComponent::SoemMasterComponent(const std::string& name) : //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); } -SoemMasterComponent::~SoemMasterComponent() -{ +SoemMasterComponent::~SoemMasterComponent() { } -bool SoemMasterComponent::configureHook() -{ - Logger::In in(this->getName()); - - int ret; - if(prop_redundant && !prop_ifname1.empty() && !prop_ifname2.empty()) - ret = ec_init_redundant((char*)prop_ifname1.c_str(),(char*)prop_ifname2.c_str()); - else - ret = ec_init((char*)prop_ifname1.c_str()); - // Initialise SOEM, bind socket to ifname - if (ret > 0) - { - log(Info) << "ec_init on " << prop_ifname1 << (prop_redundant ? std::string("and ") + prop_ifname2 : "")<<" succeeded." << endlog(); - - // Initialise default configuration, using the default config table (see ethercatconfiglist.h) - if (ec_config_init(FALSE) > 0) - { - if (EcatError) - notifySoemErrors(); - - log(Info) << ec_slavecount << " slaves found and configured." - << endlog(); - - setSlavesTargetState(EC_STATE_PRE_OP); - // Wait for all slaves to reach PRE_OP state - if(!checkSlavesStateReachedWaiting(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) - return false; - - // The parameters to be sent to the slaves are loaded from the soem.cpf: - // parameters could be changed without modifying the code - for (unsigned int i=0; i < parameters.size(); i++) - { - bool sdoWriteDone; - AddressInfo tmpAddressInfo; - tmpAddressInfo.slave_position = (uint16)parameters[i].slave_position; - tmpAddressInfo.index = (uint16)parameters[i].index; - tmpAddressInfo.sub_index = (uint8)parameters[i].sub_index; - - sdoWriteDone = writeCoeSdo(tmpAddressInfo, - parameters[i].complete_access, - parameters[i].size, - ¶meters[i].param); - - if (!sdoWriteDone) - { - log(Error) << "SDO write requested from soem.cpf failed." - << endlog(); - return false; - } - } - - for (int i = 1; i <= ec_slavecount; i++) - { - SoemDriver - * driver = SoemDriverFactory::Instance().createDriver( - &ec_slave[i]); - if (driver) - { - m_drivers.push_back(driver); - log(Info) << "Created driver for " << ec_slave[i].name - << ", with address " << ec_slave[i].configadr - << endlog(); - // Adding driver's services to master component - this->provides()->addService(driver->provides()); - log(Info) << "Put configured parameters in the slaves." - << endlog(); - if (!driver->configure()) - return false; - } - else - { - log(Warning) << "Could not create driver for " - << ec_slave[i].name << endlog(); - } - } - - ec_config_map(&m_IOmap); - if (EcatError) - notifySoemErrors(); - - for (unsigned int i = 0; i < m_drivers.size(); i++) - if (!m_drivers[i]->start()){ - log(Error)<<"Could not start driver for "<getName()); + + int ret; + if (prop_redundant && !prop_ifname1.empty() && !prop_ifname2.empty()) + ret = ec_init_redundant((char*) prop_ifname1.c_str(), + (char*) prop_ifname2.c_str()); + else + ret = ec_init((char*) prop_ifname1.c_str()); + // Initialise SOEM, bind socket to ifname + if (ret > 0) { + log(Info) << "ec_init on " << prop_ifname1 + << (prop_redundant ? std::string("and ") + prop_ifname2 : "") + << " succeeded." << endlog(); + + // Initialise default configuration, using the default config table + // (see ethercatconfiglist.h) + if (ec_config_init(FALSE) > 0) { + if (EcatError) + notifySoemErrors(); + + log(Info) << ec_slavecount << " slaves found and configured." << endlog(); + + setSlavesTargetState(EC_STATE_PRE_OP); + // Wait for all slaves to reach PRE_OP state + if (!checkSlavesStateReachedWaiting(EC_STATE_PRE_OP, EC_TIMEOUTSTATE)) + return false; + // The parameters to be sent to the slaves are loaded from the soem.cpf: + // parameters could be changed without modifying the code + for (unsigned int i = 0; i < parameters.size(); i++) { + bool sdoWriteDone; + AddressInfo tmpAddressInfo; + tmpAddressInfo.slave_position = (uint16) parameters[i].slave_position; + tmpAddressInfo.index = (uint16) parameters[i].index; + tmpAddressInfo.sub_index = (uint8) parameters[i].sub_index; + + sdoWriteDone = writeCoeSdo(tmpAddressInfo, + parameters[i].complete_access, parameters[i].size, + ¶meters[i].param); + + if (!sdoWriteDone) { + log(Error) << "SDO write requested from soem.cpf failed." << endlog(); + return false; } - else - { - log(Error) << "Configuration of slaves failed!!! \n" - << "The NIC currently used for EtherCAT is " - << prop_ifname1.c_str() - << ". Another could be chosen by editing soem.cpf." - << endlog(); + } + + for (int i = 1; i <= ec_slavecount; i++) { + SoemDriver * driver = SoemDriverFactory::Instance().createDriver( + &ec_slave[i]); + if (driver) { + m_drivers.push_back(driver); + log(Info) << "Created driver for " << ec_slave[i].name + << ", with address " << ec_slave[i].configadr << endlog(); + // Adding driver's services to master component + this->provides()->addService(driver->provides()); + log(Info) << "Put configured parameters in the slaves." << endlog(); + if (!driver->configure()) return false; + } else { + log(Warning) << "Could not create driver for " << ec_slave[i].name + << endlog(); } - return true; - } - else - { - log(Error) << "Could not initialize master on " << prop_ifname1 << (prop_redundant ? std::string("and ") + prop_ifname2 : "") << endlog(); - return false; + } + + ec_config_map(&m_IOmap); + if (EcatError) + notifySoemErrors(); + + for (unsigned int i = 0; i < m_drivers.size(); i++) + if (!m_drivers[i]->start()) { + log(Error) << "Could not start driver for " << m_drivers[i] + << getName() << endlog(); + return false; + } + + // Configure distributed clock + // ec_configdc(); + // Read the state of all slaves + + // ec_readstate(); + + } else { + log(Error) << "Configuration of slaves failed!!! \n" + << "The NIC currently used for EtherCAT is " << prop_ifname1.c_str() + << ". Another could be chosen by editing soem.cpf." << endlog(); + return false; } + return true; + } else { + log(Error) << "Could not initialize master on " << prop_ifname1 + << (prop_redundant ? std::string("and ") + prop_ifname2 : "") + << endlog(); + return false; + } } @@ -216,7 +198,7 @@ bool SoemMasterComponent::startHook() { setSlavesTargetState(EC_STATE_SAFE_OP); // Wait for all slaves to reach SAFE_OP state state_reached = checkSlavesStateReachedWaiting(EC_STATE_SAFE_OP, - EC_TIMEOUTSTATE); + EC_TIMEOUTSTATE); if (EcatError) notifySoemErrors(); @@ -234,7 +216,7 @@ bool SoemMasterComponent::startHook() { // Wait for all slaves to reach OP state state_reached = checkSlavesStateReachedWaiting(EC_STATE_OPERATIONAL, - EC_TIMEOUTSTATE); + EC_TIMEOUTSTATE); if (EcatError) notifySoemErrors(); @@ -265,15 +247,14 @@ void SoemMasterComponent::updateHook() { notifySoemErrors(); } -void SoemMasterComponent::cleanupHook() -{ - for (unsigned int i = 0; i < m_drivers.size(); i++){ +void SoemMasterComponent::cleanupHook() { + for (unsigned int i = 0; i < m_drivers.size(); i++) { this->provides()->removeService(m_drivers[i]->provides()->getName()); delete m_drivers[i]; } - // stop SOEM, close socket - ec_close(); + // stop SOEM, close socket + ec_close(); } bool SoemMasterComponent::writeCoeSdo(const AddressInfo& address, @@ -423,4 +404,4 @@ bool SoemMasterComponent::notifySoemErrors() { return errorDetected; } -}//namespace +} // namespace diff --git a/soem_master/soem_master_component.h b/soem_master/soem_master_component.h index b1f8279..7711933 100755 --- a/soem_master/soem_master_component.h +++ b/soem_master/soem_master_component.h @@ -27,94 +27,91 @@ #include "soem_driver.h" #include "soem_master_types.hpp" -namespace soem_master -{ +namespace soem_master { /** CoE addressing info */ -struct AddressInfo -{ +struct AddressInfo { /** Position of the slave in the EtherCAT network starting from 1 */ uint16 slave_position; - /** Index of the CoE object */ + /** Index of the CoE object. */ uint16 index; - /** Subindex of the CoE object*/ - uint8 sub_index; + /** Subindex of the CoE object. */ + uint8 sub_index; }; -class SoemMasterComponent: public RTT::TaskContext -{ +class SoemMasterComponent: public RTT::TaskContext { public: - SoemMasterComponent(const std::string& name); - ~SoemMasterComponent(); + SoemMasterComponent(const std::string& name); + ~SoemMasterComponent(); protected: - virtual bool configureHook(); - virtual bool startHook(); - virtual void updateHook(); - virtual void stopHook() - { - } - ; - virtual void cleanupHook(); + virtual bool configureHook(); + virtual bool startHook(); + virtual void updateHook(); + virtual void stopHook() { + } + virtual void cleanupHook(); private: - std::string prop_ifname1, prop_ifname2; - bool prop_redundant; - char m_IOmap[4096]; - std::vector m_drivers; - std::vector parameters; - - /** - * Perform a CoE SDO write (blocking) - * @param[in] address = structure that identifies the slave and the object - * @param[in] complete_access = FALSE = single subindex. - * TRUE = Complete Access, all subindexes written. - * @param[in] size = Size in bytes of the object to be written - * @param[in] data = Pointer to the data to be written - * @return true if it succeeds false if it fails - */ - bool writeCoeSdo(const AddressInfo& address, bool complete_access, int size, void* data); - - /** - * Perform a CoE SDO read (blocking) - * @param[in] address = structure that identifies the slave and the object - * @param[in] complete_access = FALSE = single subindex. - * TRUE = Complete Access, all subindexes written. - * @param[in,out] size = Size in bytes of the read object - * @param[out] data = Pointer to the read data - * @return true if it succeeds false if it fails - */ - bool readCoeSdo(const AddressInfo& address, bool complete_access, int* size, void* data); - - /** - * Set the EtherCAT target state to be reached by all the slaves - * @param[in] target_state = target EtherCAT state - */ - void setSlavesTargetState(ec_state target_state); - - /** - * Keep refreshing and checking the network state till all slaves reach the - * desired state or a certain amount of time has elapsed (blocking) - * @param[in] desired_state = target EtherCAT state - * @param[in] timeout = timeout in ms - * @return true if the state has been reached within the set timeout - */ - bool checkSlavesStateReachedWaiting(ec_state desired_state, int timeout); - - /** - * Notify if SOEM library detected an error. (non blocking) - * This function is not blocking because it simply retrieve if an error has - * already been detected. - * Possible errors are: packet errors, CoE Emergency messages, - * CoE SDO errors, FoE errors and SoE errors - * Note: Only packets error are possible in EC_STATE_OPERATIONAL if the User - * does not explicitly require a mail box service (CoE, FoE, SoE) - * @return true if at least one error has been detected - */ - bool notifySoemErrors(); - -};//class - -}//namespace + std::string prop_ifname1, prop_ifname2; + bool prop_redundant; + char m_IOmap[4096]; + std::vector m_drivers; + std::vector parameters; + + /** + * Perform a CoE SDO write (blocking) + * @param[in] address = structure that identifies the slave and the object + * @param[in] complete_access = FALSE = single subindex. + * TRUE = Complete Access, all subindexes written. + * @param[in] size = Size in bytes of the object to be written + * @param[in] data = Pointer to the data to be written + * @return true if it succeeds false if it fails + */ + bool writeCoeSdo(const AddressInfo& address, bool complete_access, int size, + void* data); + + /** + * Perform a CoE SDO read (blocking) + * @param[in] address = structure that identifies the slave and the object + * @param[in] complete_access = FALSE = single subindex. + * TRUE = Complete Access, all subindexes written. + * @param[in,out] size = Size in bytes of the read object + * @param[out] data = Pointer to the read data + * @return true if it succeeds false if it fails + */ + bool readCoeSdo(const AddressInfo& address, bool complete_access, int* size, + void* data); + + /** + * Set the EtherCAT target state to be reached by all the slaves + * @param[in] target_state = target EtherCAT state + */ + void setSlavesTargetState(ec_state target_state); + + /** + * Keep refreshing and checking the network state till all slaves reach the + * desired state or a certain amount of time has elapsed (blocking) + * @param[in] desired_state = target EtherCAT state + * @param[in] timeout = timeout in ms + * @return true if the state has been reached within the set timeout + */ + bool checkSlavesStateReachedWaiting(ec_state desired_state, int timeout); + + /** + * Notify if SOEM library detected an error. (non blocking) + * This function is not blocking because it simply retrieve if an error has + * already been detected. + * Possible errors are: packet errors, CoE Emergency messages, + * CoE SDO errors, FoE errors and SoE errors + * Note: Only packets error are possible in EC_STATE_OPERATIONAL if the User + * does not explicitly require a mail box service (CoE, FoE, SoE) + * @return true if at least one error has been detected + */ + bool notifySoemErrors(); + +}; + +} // namespace soem_master #endif diff --git a/soem_master/soem_master_types.hpp b/soem_master/soem_master_types.hpp index 398bda1..ffd821e 100644 --- a/soem_master/soem_master_types.hpp +++ b/soem_master/soem_master_types.hpp @@ -28,8 +28,7 @@ #include #include -extern "C" -{ +extern "C" { #include "ethercattype.h" }