feat: add EtherCAT DC monitoring and four-motor sync test

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

@@ -10,6 +10,8 @@ motor {
     ethercat {
       master_index: 0
       cycle_us: 1000
+      slave_op_timeout_ms: 12000
+      slave_state_poll_period_ms: 10
 
       cia402 {
         profile_position_trigger_delay_ms: 2
@@ -19,6 +21,16 @@ motor {
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      dc {
+        enable: true
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
       slaves { motor_id: 1 alias: 0 position: 0 }
       slaves { motor_id: 2 alias: 0 position: 1 }
       slaves { motor_id: 3 alias: 0 position: 2 }

cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt

@@ -10,6 +10,8 @@ motor {
     ethercat {
       master_index: 0
       cycle_us: 1000
+      slave_op_timeout_ms: 15000
+      slave_state_poll_period_ms: 10
 
       cia402 {
         profile_position_trigger_delay_ms: 2
@@ -19,6 +21,16 @@ motor {
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      dc {
+        enable: false
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
       slaves { motor_id: 1 alias: 0 position: 0 }
       slaves { motor_id: 2 alias: 0 position: 1 }
       slaves { motor_id: 3 alias: 0 position: 2 }

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
 
 #include <atomic>
+#include <chrono>
 #include <cstdint>
 #include <cstring>
 #include <mutex>
@@ -87,6 +88,8 @@ private:
     };
 
     bool configureSlave_(SlaveRuntime& slave);
+    bool configureDc_();
+    bool waitSlavesOperational_();
     void cyclicLoop_();
     void readFeedbackLocked_();
     void writeCommandsLocked_();
@@ -139,6 +142,10 @@ private:
                                          const PdoEntryRuntime& entry);
     static void writeEntryValue_(std::uint8_t* domain_data,
                                  const PdoEntryRuntime& entry);
+    static std::uint64_t steadyTimeNs_();
+    static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point);
+    static std::uint32_t usToNs_(std::uint32_t value_us);
+    static std::int32_t usToNs_(std::int32_t value_us);
 
     std::string id_;
     config::EtherCATConfig config_;

cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp

@@ -6,6 +6,7 @@
 #include <cstddef>
 #include <map>
 #include <sstream>
+#include <thread>
 #include <utility>
 
 #include "common/base/logging/logger.h"
@@ -35,6 +36,25 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
         CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_;
         return false;
     }
+    if (!config_.has_slave_op_timeout_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_op_timeout_ms: " << id_;
+        return false;
+    }
+    if (config_.slave_op_timeout_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_op_timeout_ms must be positive: "
+                        << id_;
+        return false;
+    }
+    if (!config_.has_slave_state_poll_period_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_state_poll_period_ms: "
+                        << id_;
+        return false;
+    }
+    if (config_.slave_state_poll_period_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_state_poll_period_ms must be positive: "
+                        << id_;
+        return false;
+    }
     if (!hasValidPdoMapping_()) {
         CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] PDO mapping is not configured: "
                         << id_;
@@ -84,6 +104,11 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
         }
     }
 
+    if (!configureDc_()) {
+        releaseMaster_();
+        return false;
+    }
+
     initialized_ = true;
     return true;
 }
@@ -122,6 +147,12 @@ bool EthercatMotorBusRuntime::start()
     running_.store(true);
     cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this);
     started_ = true;
+
+    if (!waitSlavesOperational_()) {
+        stop();
+        return false;
+    }
+
     CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_;
     return true;
 }
@@ -304,24 +335,249 @@ bool EthercatMotorBusRuntime::configureSlave_(SlaveRuntime& slave)
     return true;
 }
 
+bool EthercatMotorBusRuntime::configureDc_()
+{
+    if (!config_.has_dc()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing explicit DC config: "
+                        << id_ << ". Add dc { enable: false } or a complete enabled DC config.";
+        return false;
+    }
+
+    const auto& dc = config_.dc();
+    if (!dc.has_enable()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC enable: " << id_;
+        return false;
+    }
+    if (!dc.enable()) {
+        return true;
+    }
+
+    if (!dc.has_reference_motor_id()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC reference_motor_id: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync0_cycle_us()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_cycle_us: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync0_shift_us()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_shift_us: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync_reference_clock_period()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_reference_clock_period: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_assign_activate()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC assign_activate: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync_monitor_period_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_monitor_period_ms: "
+                        << id_;
+        return false;
+    }
+    if (dc.reference_motor_id() <= 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference_motor_id must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.sync0_cycle_us() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync0_cycle_us must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.sync_reference_clock_period() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_reference_clock_period must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.assign_activate() == 0 || dc.assign_activate() > 0xFFFFU) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC assign_activate must be in [1, 0xFFFF]: "
+                        << id_ << ", value=" << hexIndex_(dc.assign_activate());
+        return false;
+    }
+    if (dc.sync_monitor_period_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_monitor_period_ms must be positive: "
+                        << id_;
+        return false;
+    }
+
+    const auto reference_it = slaves_by_motor_id_.find(dc.reference_motor_id());
+    if (reference_it == slaves_by_motor_id_.end() || !reference_it->second.slave_config) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference motor is not configured: "
+                        << id_ << ", reference_motor_id=" << dc.reference_motor_id();
+        return false;
+    }
+
+    const int select_result = ecrt_master_select_reference_clock(
+        master_, reference_it->second.slave_config);
+    if (select_result != 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to select DC reference clock: "
+                        << id_ << ", reference_motor_id=" << dc.reference_motor_id()
+                        << ", result=" << select_result;
+        return false;
+    }
+
+    const auto assign_activate = static_cast<std::uint16_t>(dc.assign_activate());
+    const auto sync0_cycle_ns = usToNs_(dc.sync0_cycle_us());
+    const auto sync0_shift_ns = usToNs_(dc.sync0_shift_us());
+    for (auto& [motor_id, slave] : slaves_by_motor_id_) {
+        const int result = ecrt_slave_config_dc(slave.slave_config,
+                                                assign_activate,
+                                                sync0_cycle_ns,
+                                                sync0_shift_ns,
+                                                0,
+                                                0);
+        if (result != 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure DC: "
+                            << id_ << ", motor_id=" << motor_id
+                            << ", position=" << slave.cfg.position()
+                            << ", result=" << result;
+            return false;
+        }
+    }
+
+    CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] configured DC: "
+                   << id_
+                   << ", reference_motor_id=" << dc.reference_motor_id()
+                   << ", sync0_cycle_ns=" << sync0_cycle_ns
+                   << ", sync0_shift_ns=" << sync0_shift_ns
+                   << ", sync_reference_clock_period=" << dc.sync_reference_clock_period()
+                   << ", assign_activate=" << hexIndex_(assign_activate)
+                   << ", sync_monitor_period_ms=" << dc.sync_monitor_period_ms();
+    return true;
+}
+
+bool EthercatMotorBusRuntime::waitSlavesOperational_()
+{
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::milliseconds(config_.slave_op_timeout_ms());
+    const auto poll_period =
+        std::chrono::milliseconds(config_.slave_state_poll_period_ms());
+
+    ec_domain_state_t last_domain_state{};
+    int last_domain_result = 0;
+    do {
+        bool all_slaves_operational = true;
+        for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
+            (void)motor_id;
+            ec_slave_config_state_t state{};
+            const int result = ecrt_slave_config_state(slave.slave_config, &state);
+            if (result != 0 ||
+                !state.online ||
+                !state.operational ||
+                state.al_state != EC_AL_STATE_OP) {
+                all_slaves_operational = false;
+                break;
+            }
+        }
+
+        last_domain_result = ecrt_domain_state(domain_, &last_domain_state);
+        const bool domain_complete =
+            last_domain_result == 0 &&
+            last_domain_state.wc_state == EC_WC_COMPLETE;
+
+        if (all_slaves_operational && domain_complete) {
+            CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: "
+                           << id_
+                           << ", working_counter=" << last_domain_state.working_counter;
+            return true;
+        }
+
+        std::this_thread::sleep_for(poll_period);
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] timeout waiting for EtherCAT slaves OP: "
+                    << id_
+                    << ", timeout_ms=" << config_.slave_op_timeout_ms()
+                    << ", domain_result=" << last_domain_result
+                    << ", domain_wc_state=" << static_cast<int>(last_domain_state.wc_state)
+                    << ", working_counter=" << last_domain_state.working_counter;
+
+    for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
+        ec_slave_config_state_t state{};
+        const int result = ecrt_slave_config_state(slave.slave_config, &state);
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave state: "
+                        << id_
+                        << ", motor_id=" << motor_id
+                        << ", position=" << slave.cfg.position()
+                        << ", result=" << result
+                        << ", online=" << state.online
+                        << ", operational=" << state.operational
+                        << ", al_state=" << static_cast<int>(state.al_state);
+    }
+    return false;
+}
+
 void EthercatMotorBusRuntime::cyclicLoop_()
 {
     const auto period = std::chrono::microseconds(config_.cycle_us());
-    auto next_time = std::chrono::steady_clock::now();
+    const bool dc_enabled = config_.dc().enable();
+    const auto dc_sync_period = dc_enabled ? config_.dc().sync_reference_clock_period() : 0U;
+    const auto dc_monitor_period =
+        dc_enabled ? std::chrono::milliseconds(config_.dc().sync_monitor_period_ms())
+                   : std::chrono::milliseconds(0);
+    std::uint32_t dc_sync_counter = 0;
+    bool dc_monitor_queued = false;
+    auto cycle_time = std::chrono::steady_clock::now();
+    auto next_dc_monitor_time = cycle_time + dc_monitor_period;
     while (running_.load()) {
-        next_time += period;
+        if (dc_enabled) {
+            ecrt_master_application_time(master_, timePointNs_(cycle_time));
+        }
 
         ecrt_master_receive(master_);
         ecrt_domain_process(domain_);
+        if (dc_enabled && dc_monitor_queued) {
+            const std::uint32_t dc_sync_diff_ns =
+                ecrt_master_sync_monitor_process(master_);
+            if (dc_sync_diff_ns == static_cast<std::uint32_t>(-1)) {
+                CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] DC sync monitor failed: "
+                                  << id_;
+            } else {
+                CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] dc_sync_diff_ns="
+                               << dc_sync_diff_ns
+                               << ", group=" << id_;
+            }
+            dc_monitor_queued = false;
+        }
         {
             std::lock_guard<std::mutex> lock(data_mutex_);
             readFeedbackLocked_();
             writeCommandsLocked_();
         }
         ecrt_domain_queue(domain_);
+        if (dc_enabled) {
+            ++dc_sync_counter;
+            if (dc_sync_counter >= dc_sync_period) {
+                dc_sync_counter = 0;
+                ecrt_master_sync_reference_clock(master_);
+            }
+            ecrt_master_sync_slave_clocks(master_);
+
+            if (cycle_time >= next_dc_monitor_time) {
+                const int monitor_result = ecrt_master_sync_monitor_queue(master_);
+                if (monitor_result == 0) {
+                    dc_monitor_queued = true;
+                } else {
+                    CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] failed to queue DC sync monitor: "
+                                      << id_ << ", result=" << monitor_result;
+                }
+                do {
+                    next_dc_monitor_time += dc_monitor_period;
+                } while (cycle_time >= next_dc_monitor_time);
+            }
+        }
         ecrt_master_send(master_);
 
-        std::this_thread::sleep_until(next_time);
+        cycle_time += period;
+        std::this_thread::sleep_until(cycle_time);
     }
 }
 
@@ -672,4 +928,27 @@ void EthercatMotorBusRuntime::writeEntryValue_(std::uint8_t* domain_data,
     }
 }
 
+std::uint64_t EthercatMotorBusRuntime::steadyTimeNs_()
+{
+    return timePointNs_(std::chrono::steady_clock::now());
+}
+
+std::uint64_t EthercatMotorBusRuntime::timePointNs_(
+    const std::chrono::steady_clock::time_point time_point)
+{
+    const auto time_since_epoch = time_point.time_since_epoch();
+    return static_cast<std::uint64_t>(
+        std::chrono::duration_cast<std::chrono::nanoseconds>(time_since_epoch).count());
+}
+
+std::uint32_t EthercatMotorBusRuntime::usToNs_(const std::uint32_t value_us)
+{
+    return value_us * 1000U;
+}
+
+std::int32_t EthercatMotorBusRuntime::usToNs_(const std::int32_t value_us)
+{
+    return value_us * 1000;
+}
+
 } // namespace cmvr::device

cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp

@@ -28,6 +28,17 @@ config::MotorGroupConfig createSingleSlaveGroup()
     auto* ethercat = group.mutable_ethercat();
     ethercat->set_master_index(0);
     ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
+
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(true);
+    dc->set_reference_motor_id(1);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
 
     auto* slave = ethercat->add_slaves();
     slave->set_motor_id(1);

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp

@@ -1,5 +1,12 @@
+#include <array>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdint>
 #include <iostream>
+#include <limits>
 #include <memory>
+#include <thread>
 
 #include <gtest/gtest.h>
 
@@ -15,6 +22,16 @@ namespace {
 constexpr const char* kMotorManagerId = "ethercat_motors";
 constexpr const char* kMotorConfigFile =
     "devices/motor/ethercat_motors_two_real_test.pb.txt";
+constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
+constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
+constexpr std::chrono::milliseconds kPrintPeriod{100};
+constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
+constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
+constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kFourMotorAmplitudeRad = 0.2;
+constexpr double kFourMotorPeriodS = 1.0;
+constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
 
 class DeviceManagerDestroyGuard {
 public:
@@ -24,6 +41,68 @@ public:
     }
 };
 
+struct TrackingErrorStats {
+    std::int64_t sample_count{0};
+    double sum_error{0.0};
+    double sum_error_sq{0.0};
+    double max_abs_error{0.0};
+    double sin_projection{0.0};
+    double cos_projection{0.0};
+
+    void add(const double error, const double theta)
+    {
+        ++sample_count;
+        sum_error += error;
+        sum_error_sq += error * error;
+        max_abs_error = std::max(max_abs_error, std::fabs(error));
+        sin_projection += error * std::sin(theta);
+        cos_projection += error * std::cos(theta);
+    }
+
+    double mean() const
+    {
+        return sample_count > 0 ? sum_error / static_cast<double>(sample_count) : 0.0;
+    }
+
+    double rms() const
+    {
+        return sample_count > 0
+                   ? std::sqrt(sum_error_sq / static_cast<double>(sample_count))
+                   : 0.0;
+    }
+
+    double fundamentalAmplitude() const
+    {
+        if (sample_count == 0) {
+            return 0.0;
+        }
+        const double scale = 2.0 / static_cast<double>(sample_count);
+        return scale * std::sqrt(sin_projection * sin_projection +
+                                 cos_projection * cos_projection);
+    }
+
+    double phaseRad() const
+    {
+        return std::atan2(cos_projection, sin_projection);
+    }
+};
+
+double normalizePhaseRad(double phase)
+{
+    while (phase > kPi) {
+        phase -= 2.0 * kPi;
+    }
+    while (phase < -kPi) {
+        phase += 2.0 * kPi;
+    }
+    return phase;
+}
+
+double radToDeg(const double rad)
+{
+    return rad * 180.0 / kPi;
+}
+
 config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
 {
     config::DeviceManagerConfig config;
@@ -66,7 +145,7 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
     //     printMotorState(motor_id, motor_manager->getMotor(motor_id));
     // }
 
-    auto motor = motor_manager->getMotor(6);
+    auto motor = motor_manager->getMotor(4);
     motor->calibrateZeroQ();
 
     ASSERT_TRUE(motor->torqueOn());
@@ -85,4 +164,146 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
 
 }
 
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
+{
+    ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
+    DeviceManagerDestroyGuard guard;
+
+    auto& device_manager =
+        DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
+    auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
+    ASSERT_NE(motor_manager, nullptr);
+
+    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
+    for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motors[i], nullptr);
+        printMotorState(motor_id, motors[i]);
+    }
+
+    std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        std::cout << "before calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+        ASSERT_TRUE(motors[i]->calibrateZeroQ());
+        std::cout << "after calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+    }
+
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOn());
+    }
+    for (const auto& motor : motors) {
+        motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    }
+
+    std::array<double, kFourMotorIds.size()> center_q{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        center_q[i] = motors[i]->getQ();
+    }
+
+    const double omega = 2.0 * kPi / kFourMotorPeriodS;
+    std::cout << "command four motors in CSP, duration="
+              << kFourMotorTrajectoryDuration.count()
+              << " ms, command_period=" << kCyclicCommandPeriod.count()
+              << " ms, amplitude=" << kFourMotorAmplitudeRad
+              << " rad, period=" << kFourMotorPeriodS
+              << " s" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
+    std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    double max_error_spread_rad = 0.0;
+    double sum_error_spread_sq = 0.0;
+    std::int64_t error_spread_sample_count = 0;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+
+        std::array<double, kFourMotorIds.size()> target_q{};
+        std::array<double, kFourMotorIds.size()> target_qd{};
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            const double theta = omega * t_s + kFourMotorPhaseRad[i];
+            target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
+            target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
+        }
+
+        if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
+            double min_error = std::numeric_limits<double>::max();
+            double max_error = std::numeric_limits<double>::lowest();
+            for (std::size_t i = 0; i < motors.size(); ++i) {
+                const double theta = omega * t_s + kFourMotorPhaseRad[i];
+                const double error = motors[i]->getQ() - target_q[i];
+                error_stats[i].add(error, theta);
+                min_error = std::min(min_error, error);
+                max_error = std::max(max_error, error);
+            }
+            const double error_spread = max_error - min_error;
+            max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
+            sum_error_spread_sq += error_spread * error_spread;
+            ++error_spread_sample_count;
+        }
+
+        // if (elapsed.count() % kPrintPeriod.count() == 0) {
+        //     std::cout << "t=" << elapsed.count() << " ms" << std::endl;
+        //     for (std::size_t i = 0; i < motors.size(); ++i) {
+        //         std::cout << "  motor_id=" << kFourMotorIds[i]
+        //                   << ", target_q=" << target_q[i]
+        //                   << " rad, target_qd=" << target_qd[i]
+        //                   << " rad/s, q=" << motors[i]->getQ()
+        //                   << " rad, qd=" << motors[i]->getQd()
+        //                   << " rad/s" << std::endl;
+        //     }
+        // }
+
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    const auto hold_start_time = std::chrono::steady_clock::now();
+    const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
+        }
+        std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "after four motor CSP trajectory" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        printMotorState(kFourMotorIds[i], motors[i]);
+    }
+
+    const double reference_phase = error_stats.front().phaseRad();
+    const double rms_error_spread =
+        error_spread_sample_count > 0
+            ? std::sqrt(sum_error_spread_sq / static_cast<double>(error_spread_sample_count))
+            : 0.0;
+    std::cout << "four motor CSP tracking error statistics, sample_period="
+              << kStatsSamplePeriod.count()
+              << " ms, samples=" << error_stats.front().sample_count
+              << ", max_error_spread=" << max_error_spread_rad
+              << " rad, rms_error_spread=" << rms_error_spread
+              << " rad" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const double phase = error_stats[i].phaseRad();
+        const double relative_phase = normalizePhaseRad(phase - reference_phase);
+        const double relative_phase_ms = relative_phase / omega * 1000.0;
+        std::cout << "  motor_id=" << kFourMotorIds[i]
+                  << ", mean_error=" << error_stats[i].mean()
+                  << " rad, rms_error=" << error_stats[i].rms()
+                  << " rad, max_abs_error=" << error_stats[i].max_abs_error
+                  << " rad, error_fundamental_amp="
+                  << error_stats[i].fundamentalAmplitude()
+                  << " rad, error_phase=" << phase
+                  << " rad (" << radToDeg(phase)
+                  << " deg), relative_phase_to_motor1=" << relative_phase
+                  << " rad (" << radToDeg(relative_phase)
+                  << " deg, " << relative_phase_ms
+                  << " ms)" << std::endl;
+    }
+}
+
 } // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp

@@ -42,6 +42,8 @@ config::MotorGroupConfig createSingleSlaveGroup()
     auto* ethercat = group.mutable_ethercat();
     ethercat->set_master_index(0);
     ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
 
     auto* cia402 = ethercat->mutable_cia402();
     cia402->set_profile_position_trigger_delay_ms(2);
@@ -50,6 +52,15 @@ config::MotorGroupConfig createSingleSlaveGroup()
     cia402->set_status_poll_period_ms(10);
     cia402->set_stopped_velocity_tolerance_rad_s(0.001);
 
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(true);
+    dc->set_reference_motor_id(kMotorId);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
+
     auto* slave = ethercat->add_slaves();
     slave->set_motor_id(kMotorId);
     slave->set_alias(0);

protos/cmvr/config/motor_config/motor_config.proto

@@ -32,6 +32,16 @@ message Cia402ProtocolConfig {
   double stopped_velocity_tolerance_rad_s = 5;
 }
 
+message EtherCATDcConfig {
+  optional bool enable = 1;
+  optional int32 reference_motor_id = 2;
+  optional uint32 sync0_cycle_us = 3;
+  optional int32 sync0_shift_us = 4;
+  optional uint32 sync_reference_clock_period = 5;
+  optional uint32 assign_activate = 6;
+  optional uint32 sync_monitor_period_ms = 7;
+}
+
 message SocketCanConfig {
   string dev_id = 1;
   int32 channel_id = 2;
@@ -41,6 +51,9 @@ message EtherCATConfig {
   uint32 master_index = 1;
   int32 cycle_us = 2;
   Cia402ProtocolConfig cia402 = 3;
+  EtherCATDcConfig dc = 4;
+  optional uint32 slave_op_timeout_ms = 5;
+  optional uint32 slave_state_poll_period_ms = 6;
   repeated EthercatSlaveConfig slaves = 10;
 }