Fix mode_of_operation_ not set to its default value when the default …

…value is put in the YAML config file only. Also document better the unit test that checks that last value is used for target position value when command is NaN.

ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

@@ -148,6 +148,17 @@ bool EcCiA402Drive::setup_from_config(YAML::Node drive_config)
   if (drive_config["auto_state_transitions"]) {
     auto_state_transitions_ = drive_config["auto_state_transitions"].as<bool>();
   }
+
+  // Find the default mode of operation if it was specified in the configuration file
+  for (auto & channel : pdo_channels_info_) {
+    if (channel.index == CiA402D_RPDO_MODE_OF_OPERATION) {
+      if (!std::isnan(channel.default_value) ) {
+        mode_of_operation_ = channel.default_value;
+      }
+      break;
+    }
+  }
+
   return true;
 }
 

ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp

@@ -275,44 +275,75 @@ TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition)
 {
   std::unordered_map<std::string, std::string> slave_paramters;
   std::vector<double> command_interface = {
-    std::numeric_limits<double>::quiet_NaN(),
-    std::numeric_limits<double>::quiet_NaN()};
+    std::numeric_limits<double>::quiet_NaN(),  // target position
+    std::numeric_limits<double>::quiet_NaN(),  // target velocity
+    std::numeric_limits<double>::quiet_NaN(),  // target torque
+    std::numeric_limits<double>::quiet_NaN(),  // max torque
+    std::numeric_limits<double>::quiet_NaN(),  // control word
+    std::numeric_limits<double>::quiet_NaN()  // mode of operation
+  };
   slave_paramters["command_interface/mode_of_operation"] = "1";
   plugin_->paramters_ = slave_paramters;
-  plugin_->command_interface_ptr_ = &command_interface;
+  plugin_->command_interface_ptr_ = &command_interface;  // initialize commands with NaN
   plugin_->setup_from_config(YAML::Load(test_drive_config));
   plugin_->setup_interface_mapping();
   plugin_->is_operational_ = true;
-  plugin_->mode_of_operation_display_ = 8;
+  plugin_->mode_of_operation_display_ = 18;  // 18 is not a valid mode
   uint8_t domain_address[4];
   uint8_t domain_address_moo[2];
 
-  plugin_->processData(5, domain_address_moo);
-  plugin_->processData(10, domain_address_moo);
-  ASSERT_EQ(plugin_->mode_of_operation_display_, 8);
+  /** Test that target position is set to actual value when command is NaN for
+   * mode of operation: position (8)
+  */
+  ASSERT_EQ(
+    plugin_->mode_of_operation_,
+    8) << "Default mode_of_operation from config is NOT correctly loaded";
+
+  plugin_->processData(5, domain_address_moo);  // write mode_of_operation
+  plugin_->processData(10, domain_address_moo);  // read mode_of_operation_display
+  ASSERT_EQ(
+    plugin_->mode_of_operation_display_,
+    8) << "Mode of operation is NOT correctly set or read";
 
-  EC_WRITE_S32(domain_address, 123456);
-  plugin_->processData(6, domain_address);
-  ASSERT_EQ(plugin_->last_position_, 123456);
+  EC_WRITE_S32(domain_address, 123456);  // initialize domain memory with 123456
+  plugin_->processData(6, domain_address);  // read position actual value from domain
+  ASSERT_EQ(plugin_->last_position_, 123456) << "Position actual value is NOT correctly set";
 
-  EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
-  ASSERT_EQ(EC_READ_S32(domain_address), 123456);
+  EC_WRITE_S32(domain_address, 0);  // initialize domain memory with 0
+  plugin_->processData(0, domain_address);  // write target position to domain
+  ASSERT_EQ(
+    EC_READ_S32(domain_address),
+    123456) <<
+    "Target position is NOT correctly set to actual value "
+    "when command is NaN in position mode of operation (8)";
 
+  /** Test that target position is set to actual value when command is NaN for
+   * mode of operation: velocity (9)
+  */
   command_interface[1] = 9;
-  plugin_->processData(5, domain_address_moo);
-  plugin_->processData(10, domain_address_moo);
-  ASSERT_EQ(plugin_->mode_of_operation_display_, 9);
+  plugin_->processData(5, domain_address_moo);  // write mode_of_operation
+  plugin_->processData(10, domain_address_moo);  // read mode_of_operation_display
+  ASSERT_EQ(
+    plugin_->mode_of_operation_display_,
+    9) << "Mode of operation is NOT correctly set or read";
 
-  EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
-  ASSERT_EQ(EC_READ_S32(domain_address), 123456);
+  /** Test with current actual position (123456) */
+  EC_WRITE_S32(domain_address, 0);  // initialize domain memory with 0
+  plugin_->processData(0, domain_address);  // write target position to domain
+  ASSERT_EQ(
+    EC_READ_S32(domain_address),
+    123456) <<
+    "Target position is NOT correctly set to actual value "
+    "when command is NaN in velocity mode of operation (9)";
 
-  EC_WRITE_S32(domain_address, 654321);
-  plugin_->processData(6, domain_address);
-  ASSERT_EQ(plugin_->last_position_, 654321);
+  /** Change actual position to 654321 */
+  EC_WRITE_S32(domain_address, 654321);  // initialize domain memory with 654321
+  plugin_->processData(6, domain_address);  // read position actual value from domain
+  ASSERT_EQ(plugin_->last_position_, 654321) << "Position actual value is NOT correctly set";
 
-  EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
-  ASSERT_EQ(EC_READ_S32(domain_address), 654321);
+  EC_WRITE_S32(domain_address, 0);  // initialize domain memory with 0
+  plugin_->processData(0, domain_address);  // write target position to domain
+  ASSERT_EQ(EC_READ_S32(domain_address), 654321) <<
+    "Target position is NOT correctly set to actual value "
+    "when command is NaN in velocity mode of operation (9)";
 }