Fix/pm 229/rewrite pdo mapping (#192)

* Check in the write method of the SDO if an error is received, if so; a ros fatal message will be initiated.

* Fixed clang format.

* Fixed clang format. (now really)

* fixed indent errors.

* fixed clang format. (NOW REALLY)

* fixed clang format. ( NOW REALLY (really) )

* Removed the global variable.

It was a redundant decleration.

* removed new line.

* added the rwc cariable to .h file

* removed rwc

* added type to the rwc.

* PDO map is configured using the example given in the IMC manual. Also short docstrings have been added to improve readability.

* Added the reactivation of the sync manager, otherswise the slave would send an error.

This version of the PDO-mapping is a working but roughly written code. Has to be cleaned before merging.

* Updated layout to match the clang8 format.

* fixed byteOffset list.

This was necessary because the byteOffset of the next object was given to the currently handled object.

* Fixed clang8 format

* Removed print statement optimized code to a better readable format.

* Updated format to match the cpp style guide

* Removed the factor group values.

These were unused, the denominator and nominator were both 1.

* Rewritten the code to a more sustainable and readable structure.

Used the cpp style guide to set a certain standard for future refactor/rewriting of code. (this includes all olav his comments made on the PR)

* Added the constructor of the IMCObject because an error occurred when it was removed.

* Added lint ignores and utility.

* fixed format errors

* Removed the test for maximum amount.

Because it is not possible with the currently defined IMC PDO Objects.

* Set the ethercat communication error protocol using the 6007 register.

This is necessary to handle an possible ethercat connection loss.

* Updated the comments in the PDO map and edited the for loop condition of the unused PDO registers to match the if condition.

* Edit naming convention to match google cpp style guide.

* Changed the for loop and if condition for disabling the unused PDO registers.

* removed compare operator from the if statement and removed redundant variable from function.

* Fixed the if condition.

This if statement should be entered if one of the values returns 0. Previously it would enter if all the values are correct.

* Changed PDO_objects unordered map back to a normal map, otherwise it wont work!!!!

Also changed some last format styling

* Moved function of combining address to calculate when the object is created.

A separate function is redundant because every object needs this variable.

* Changed to const reference to avoid copying each pair in the for each loop.

Co-authored-by: martijnvandermarel <martijn.vander.marel@hetnet.nl>
Co-authored-by: Roelemans <47626418+Roelemans@users.noreply.github.com>
Co-authored-by: Olav de Haas <olavh@protonmail.com>

Author: 4 people

march_hardware/include/march_hardware/EtherCAT/EthercatSDO.h

@@ -2,7 +2,7 @@
 #ifndef MARCH_HARDWARE_ETHERCAT_ETHERCATSDO_H
 #define MARCH_HARDWARE_ETHERCAT_ETHERCATSDO_H
 
-#include <stdint.h>
+#include <cstdint>
 
 namespace march4cpp
 {
@@ -11,4 +11,5 @@ int sdo_bit16(int slave, uint32_t index, uint8_t sub, uint16_t value);
 int sdo_bit32(int slave, uint32_t index, uint8_t sub, uint32_t value);
 
 }  // namespace march4cpp
+
 #endif  // MARCH_HARDWARE_ETHERCAT_ETHERCATSDO_H

march_hardware/include/march_hardware/PDOmap.h

@@ -6,36 +6,40 @@
 #include <utility>
 #include <vector>
 #include <map>
+#include <unordered_map>
 
 #include <ros/ros.h>
 
 #include <march_hardware/EtherCAT/EthercatSDO.h>
 
 namespace march4cpp
 {
+/** Store IMC data as a struct to prevent data overlap.*/
 struct IMCObject
 {
-  int address;  // in IMC memory (see IMC manual)
-  int length;   // bits (see IMC manual)
+  uint16_t address;           // in IMC memory (see IMC manual)
+  uint16_t length;            // bits (see IMC manual)
+  uint32_t combined_address;  // combine the address(hex), sub-index(hex) and length(hex)
 
-  explicit IMCObject(int _address, int _length)
+  explicit IMCObject(uint16_t _address, uint16_t _length) : address(_address), length(_length)
   {
-    this->address = _address;
-    this->length = _length;
-  }
+    uint32_t MSword = ((address & 0xFFFF) << 16);  // Shift 16 bits left for most significant word
+    uint32_t LSword = (length & 0xFFFF);
 
-  IMCObject()
-  {
+    combined_address = (MSword | LSword);
   }
+
+  IMCObject(){};
 };
 
+/** The data direction to which the PDO is specified is restricted to master in slave out and slave out master in.*/
 enum class dataDirection
 {
   miso,
   mosi
 };
 
-// If a new object is added to this enum, make sure to also add it to PDOmap::initAllObjects()!
+/** All the available IMC object names divided over the PDO maps. make sure to also add it to PDOmap constructor.*/
 enum class IMCObjectName
 {
   StatusWord,
@@ -57,24 +61,27 @@ enum class IMCObjectName
 class PDOmap
 {
 public:
-  // Constructor
-  PDOmap();
+  /** Initiate all the entered IMC objects to prepare the PDO.*/
+  void addObject(IMCObjectName object_name);
+
+  std::map<IMCObjectName, int> map(int slave_index, dataDirection direction);
 
-  void addObject(IMCObjectName objectname);
-  std::map<IMCObjectName, int> map(int slaveIndex, dataDirection direction);
+  static std::unordered_map<IMCObjectName, IMCObject> all_objects;
 
 private:
-  void initAllObjects();
-  void sortPDOObjects();
-  uint32_t combineAddressLength(uint16_t address, uint16_t length);
-  std::map<IMCObjectName, IMCObject> PDOObjects;
-  std::map<IMCObjectName, IMCObject> allObjects;
-  std::vector<std::pair<IMCObjectName, IMCObject>> sortedPDOObjects;
-  std::map<IMCObjectName, int> byteOffsets;
-
-  const int bitsPerReg = 64;
-  const int nrofRegs = 4;
-  const int objectSizes[3] = { 8, 16, 32 };
+  /** Used to sort the objects in the all_objects according to data length.
+   * @return list of pairs <IMCObjectName, IMCObjects> according from object sizes */
+  std::vector<std::pair<IMCObjectName, IMCObject>> sortPDOObjects();
+
+  /** Configures the PDO in the IMC using the given base register address and sync manager address.
+   * @return map of the IMC PDO object name in combination with the byte-offset in the PDO register */
+  std::map<IMCObjectName, int> configurePDO(int slave_index, int base_register, int base_sync_manager);
+
+  std::map<IMCObjectName, IMCObject> PDO_objects;
+
+  const int bits_per_register = 64;           // Maximum amount of bits that can be constructed in one PDO message.
+  const int nr_of_regs = 4;                   // Amount of registers available.
+  const int object_sizes[3] = { 32, 16, 8 };  // Available sizes.
 };
 }  // namespace march4cpp
 

march_hardware/src/EtherCAT/EthercatSDO.cpp

@@ -18,19 +18,36 @@ namespace march4cpp
 int sdo_bit8(int slave, uint32_t index, uint8_t sub, uint8_t value)
 {
   ROS_DEBUG("sdo_bit8: slaveIndex %i, reg 0x%X, subindex %i, value 0x%X", slave, index, sub, value);
-  return ec_SDOwrite(slave, index, sub, FALSE, 1, &value, EC_TIMEOUTRXM);
+  int received_working_counter = ec_SDOwrite(slave, index, sub, FALSE, 1, &value, EC_TIMEOUTRXM);
+  if (received_working_counter == 0)
+  {
+    ROS_FATAL("(sdo_bit8) Error occurred when writing, reg 0x%X, sub-index %i, data: 0x%X to slave %i", index, sub,
+              value, slave);
+  }
+  return received_working_counter;
 }
 
 int sdo_bit16(int slave, uint32_t index, uint8_t sub, uint16_t value)
 {
   ROS_DEBUG("sdo_bit16: slaveIndex %i, reg 0x%X, subindex %i, value 0x%X", slave, index, sub, value);
-  return ec_SDOwrite(slave, index, sub, FALSE, 2, &value, EC_TIMEOUTRXM);
+  int received_working_counter = ec_SDOwrite(slave, index, sub, FALSE, 2, &value, EC_TIMEOUTRXM);
+  if (received_working_counter == 0)
+  {
+    ROS_FATAL("(sdo_bit16) Error occurred when writing, reg 0x%X, sub-index %i, data: 0x%X to slave %i", index, sub,
+              value, slave);
+  }
+  return received_working_counter;
 }
 
 int sdo_bit32(int slave, uint32_t index, uint8_t sub, uint32_t value)
 {
   ROS_DEBUG("sdo_bit32: slaveIndex %i, reg 0x%X, subindex %i, value 0x%X", slave, index, sub, value);
-  return ec_SDOwrite(slave, index, sub, FALSE, 4, &value, EC_TIMEOUTRXM);
+  int received_working_counter = ec_SDOwrite(slave, index, sub, FALSE, 4, &value, EC_TIMEOUTRXM);
+  if (received_working_counter == 0)
+  {
+    ROS_FATAL("(sdo_bit32) Error occurred when writing, reg 0x%X, sub-index %i, data: 0x%X to slave %i", index, sub,
+              value, slave);
+  }
+  return received_working_counter;
 }
-
 }  // namespace march4cpp

march_hardware/src/IMotionCube.cpp

@@ -76,41 +76,38 @@ void IMotionCube::validateMosiPDOs()
 void IMotionCube::writeInitialSettings(uint8 ecatCycleTime)
 {
   ROS_DEBUG("IMotionCube::writeInitialSettings");
-  bool success = true;
-  // sdo_bit32(slaveIndex, address, subindex, value);
 
   if (this->actuationMode == ActuationMode::unknown)
   {
     throw std::runtime_error("Cannot write initial settings to IMotionCube as it has actuation mode of unknown");
   }
 
   // mode of operation
-  success &= sdo_bit8(slaveIndex, 0x6060, 0, this->actuationMode.toModeNumber());
-
-  // position dimension index
-  success &= sdo_bit8(slaveIndex, 0x608A, 0, 1);
-
-  // position factor -- scaling factor numerator
-  success &= sdo_bit32(slaveIndex, 0x6093, 1, 1);
-  // position factor -- scaling factor denominator
-  success &= sdo_bit32(slaveIndex, 0x6093, 2, 1);
+  int mode_of_op = sdo_bit8(slaveIndex, 0x6060, 0, this->actuationMode.toModeNumber());
 
   // position limit -- min position
-  success &= sdo_bit32(slaveIndex, 0x607D, 1, this->encoder.getLowerSoftLimitIU());
+  int max_pos_lim = sdo_bit32(slaveIndex, 0x607D, 1, this->encoder.getLowerSoftLimitIU());
+
   // position limit -- max position
-  success &= sdo_bit32(slaveIndex, 0x607D, 2, this->encoder.getUpperSoftLimitIU());
+  int min_pos_lim = sdo_bit32(slaveIndex, 0x607D, 2, this->encoder.getUpperSoftLimitIU());
 
   // Quick stop option
-  success &= sdo_bit16(slaveIndex, 0x605A, 0, 6);
+  int stop_opt = sdo_bit16(slaveIndex, 0x605A, 0, 6);
 
   // Quick stop deceleration
-  success &= sdo_bit32(slaveIndex, 0x6085, 0, 0x7FFFFFFF);
+  int stop_decl = sdo_bit32(slaveIndex, 0x6085, 0, 0x7FFFFFFF);
+
+  // Abort connection option code
+  int abort_con = sdo_bit16(slaveIndex, 0x6007, 0, 1);
 
   // set the ethercat rate of encoder in form x*10^y
-  success &= sdo_bit8(slaveIndex, 0x60C2, 1, ecatCycleTime);
-  success &= sdo_bit8(slaveIndex, 0x60C2, 2, -3);
+  int rate_ec_x = sdo_bit8(slaveIndex, 0x60C2, 1, ecatCycleTime);
+  int rate_ec_y = sdo_bit8(slaveIndex, 0x60C2, 2, -3);
 
-  ROS_ASSERT_MSG(success, "Writing initial settings to IMC %d failed", this->slaveIndex);
+  if (!(mode_of_op && max_pos_lim && min_pos_lim && stop_opt && stop_decl && abort_con && rate_ec_x && rate_ec_y))
+  {
+    ROS_ERROR("Failed writing initial settings to IMC of slave %i", slaveIndex);
+  }
 }
 
 void IMotionCube::actuateRad(float targetRad)