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" }