Ethercat系列---IGH DC模式测试电机程序（Preempt-RT）

先上代码

linux内核版本：4.19-rt
Ethercat: DC模式
伺服运行模式: CSP（周期位置同步）

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */

/****************************************************************************/

#include "ecrt.h"

/****************************************************************************/

// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define MEASURE_TIMING

#define TARGET_POSITION         0 /*target position*/
#define CYCLIC_POSITION            8   /*Operation mode for 0x6060:0*/

/****************************************************************************/

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)    /*本次设置周期PERIOD_NS为1ms*/

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
        (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {
   };

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {
   };

static ec_slave_config_t *sc = NULL;
static ec_slave_config_state_t sc_state = {
   };

/****************************************************************************/

// process data
static uint8_t *domain1_pd = NULL;

#define PANASONIC        0,0                        /*EtherCAT address on the bus*/

#define VID_PID          0x0000066F,0x515050a1   /*Vendor ID, product code*/

/*Offsets for PDO entries*/
static struct{
   
    unsigned int ctrl_word;
    unsigned int operation_mode;
    unsigned int target_position;
    unsigned int touch_probe_function;
    unsigned int error_code;
    unsigned int status_word;
    unsigned int modes_of_operation_display;
    unsigned int position_actual_value;
    unsigned int touch_probe_status;
    unsigned int touch_probe_pos1_pos_value;
    unsigned int following_error_actual_value;
    unsigned int digital_inputs;
}offset;

const static ec_pdo_entry_reg_t domain1_regs[] = {
   
    {
   PANASONIC, VID_PID, 0x6040, 0, &offset.ctrl_word},
    {
   PANASONIC, VID_PID, 0x6060, 0, &offset.operation_mode },
    {
   PANASONIC, VID_PID, 0x607A, 0, &offset.target_position},
    {
   PANASONIC, VID_PID, 0x60B8, 0, &offset.touch_probe_function},
    {
   PANASONIC, VID_PID, 0x603F, 0, &offset.error_code},
    {
   PANASONIC, VID_PID, 0x6041, 0, &offset.status_word},
    {
   PANASONIC, VID_PID, 0x6061, 0, &offset.modes_of_operation_display},
    {
   PANASONIC, VID_PID, 0x6064, 0, &offset.position_actual_value},
    {
   PANASONIC, VID_PID, 0x60B9, 0, &offset.touch_probe_status},
    {
   PANASONIC, VID_PID, 0x60BA, 0, &offset.touch_probe_pos1_pos_value},
    {
   PANASONIC, VID_PID, 0x60F4, 0, &offset.following_error_actual_value},
    {
   PANASONIC