Merge branch 'develop' into feature/PM-429-use-sw-files-from-esm-repo

Author: RutgerVanBeek

march_hardware/include/march_hardware/PDOmap.h

@@ -16,13 +16,15 @@ namespace march
 struct IMCObject
 {
   uint16_t address;           // in IMC memory (see IMC manual)
-  uint16_t length;            // bits (see IMC manual)
+  uint8_t sub_index;          // sub index corresponding to PDO register (see IMC manual)
+  uint8_t length;             // bits (see IMC manual)
   uint32_t combined_address;  // combine the address(hex), sub-index(hex) and length(hex)
 
-  explicit IMCObject(uint16_t _address, uint16_t _length) : address(_address), length(_length)
+  IMCObject(uint16_t _address, uint8_t _sub_index, uint8_t _length)
+    : address(_address), sub_index(_sub_index), length(_length)
   {
-    uint32_t MSword = ((address & 0xFFFF) << 16);  // Shift 16 bits left for most significant word
-    uint32_t LSword = (length & 0xFFFF);
+    uint32_t MSword = (address << 16);
+    uint32_t LSword = (sub_index << 8) | length;
 
     combined_address = (MSword | LSword);
   }

march_hardware/src/PDOmap.cpp

@@ -10,22 +10,22 @@
 namespace march
 {
 std::unordered_map<IMCObjectName, IMCObject> PDOmap::all_objects = {
-  { IMCObjectName::StatusWord, IMCObject(0x6041, 16) },
-  { IMCObjectName::ActualPosition, IMCObject(0x6064, 32) },
-  { IMCObjectName::ActualVelocity, IMCObject(0x6069, 32) },
-  { IMCObjectName::MotionErrorRegister, IMCObject(0x2000, 16) },
-  { IMCObjectName::DetailedErrorRegister, IMCObject(0x2002, 16) },
-  { IMCObjectName::DCLinkVoltage, IMCObject(0x2055, 16) },
-  { IMCObjectName::DriveTemperature, IMCObject(0x2058, 16) },
-  { IMCObjectName::ActualTorque, IMCObject(0x6077, 16) },
-  { IMCObjectName::CurrentLimit, IMCObject(0x207F, 16) },
-  { IMCObjectName::MotorPosition, IMCObject(0x2088, 32) },
-  { IMCObjectName::MotorVelocity, IMCObject(0x2087, 32) },
-  { IMCObjectName::ControlWord, IMCObject(0x6040, 16) },
-  { IMCObjectName::TargetPosition, IMCObject(0x607A, 32) },
-  { IMCObjectName::TargetTorque, IMCObject(0x6071, 16) },
-  { IMCObjectName::QuickStopDeceleration, IMCObject(0x6085, 32) },
-  { IMCObjectName::QuickStopOption, IMCObject(0x605A, 16) }
+  { IMCObjectName::StatusWord, IMCObject(0x6041, 0, 16) },
+  { IMCObjectName::ActualPosition, IMCObject(0x6064, 0, 32) },
+  { IMCObjectName::ActualVelocity, IMCObject(0x6069, 0, 32) },
+  { IMCObjectName::MotionErrorRegister, IMCObject(0x2000, 0, 16) },
+  { IMCObjectName::DetailedErrorRegister, IMCObject(0x2002, 0, 16) },
+  { IMCObjectName::DCLinkVoltage, IMCObject(0x2055, 0, 16) },
+  { IMCObjectName::DriveTemperature, IMCObject(0x2058, 0, 16) },
+  { IMCObjectName::ActualTorque, IMCObject(0x6077, 0, 16) },
+  { IMCObjectName::CurrentLimit, IMCObject(0x207F, 0, 16) },
+  { IMCObjectName::MotorPosition, IMCObject(0x2088, 0, 32) },
+  { IMCObjectName::MotorVelocity, IMCObject(0x2087, 0, 32) },
+  { IMCObjectName::ControlWord, IMCObject(0x6040, 0, 16) },
+  { IMCObjectName::TargetPosition, IMCObject(0x607A, 0, 32) },
+  { IMCObjectName::TargetTorque, IMCObject(0x6071, 0, 16) },
+  { IMCObjectName::QuickStopDeceleration, IMCObject(0x6085, 0, 32) },
+  { IMCObjectName::QuickStopOption, IMCObject(0x605A, 0, 16) }
 };
 
 void PDOmap::addObject(IMCObjectName object_name)

march_hardware/test/TestPDOmap.cpp

@@ -53,3 +53,27 @@ TEST_F(PDOTest, ObjectCounts)
   ASSERT_EQ(1u, misoByteOffsets.count(march::IMCObjectName::CurrentLimit));
   ASSERT_EQ(0u, misoByteOffsets.count(march::IMCObjectName::DCLinkVoltage));
 }
+
+TEST_F(PDOTest, CombinedAddressConstruct)
+{
+  march::PDOmap pdoMap;
+
+  auto status_word = pdoMap.all_objects.find(march::IMCObjectName::StatusWord);
+  uint32_t combined_address = status_word->second.combined_address;
+
+  ASSERT_EQ(16u, (combined_address & 0xFF));
+  ASSERT_EQ(0u, ((combined_address >> 8) & 0xFF));
+  ASSERT_EQ(0x6041u, ((combined_address >> 16) & 0xFFFF));
+}
+
+TEST_F(PDOTest, CombinedAdressConstructWithSubindexValue)
+{
+  march::PDOmap pdoMap;
+
+  auto test_object = march::IMCObject(0x6060, 2, 16);
+  uint32_t combined_address = test_object.combined_address;
+
+  ASSERT_EQ(16u, (combined_address & 0xFF));
+  ASSERT_EQ(2u, ((combined_address >> 8) & 0xFF));
+  ASSERT_EQ(0x6060u, ((combined_address >> 16) & 0xFFFF));
+}