关于Igh-EthercatMaster如何使用从站作为dc参考时钟

代码基于xenomai2和qt

我igh修改了的源码

把ecrt_master_reference_clock_time(ighMaster,(uint64_t*)&refTime) 函数返回的refTime改成了uint64_t

#include "IghEthercatMaster.h"
#include "EthercatDevice.h"
#include "GlobalFunction.h"
#include "RtThread.h"
#include "RtMaster.h"
#include <utility>
#include <QThread>

//#define Q_OS_LINUX

#if defined(Q_OS_LINUX)

#include <sys/mman.h>
#include <sys/perm.h>
#include <sys/prctl.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <signal.h>
#include <rtdm/rtdm.h>
#include <native/task.h>
#include <native/sem.h>
#include <native/mutex.h>
#include <native/timer.h>
#include <rtdk.h>


struct IghEthercatMaster::Pimp{
    ec_master_state_t  masterState;
    ec_master_t*       ighMaster=nullptr;
    int64_t refTime=0;
    int64_t freqShift=0;
    int64_t dcDiffNs=0;
    int64_t refDiffNs=0;
    bool    dcErrored=false;
    bool    stateErrored=false;
};

IghEthercatMaster::IghEthercatMaster(){
    pimp.reset(new Pimp);
}

IghEthercatMaster::~IghEthercatMaster(){
}

bool IghEthercatMaster::init(){
    ec_master_state_t& masterState = pimp->masterState;
    ec_master_t*& ighMaster = pimp->ighMaster;
    for(int i=0;i<5;i++){
        if(ighMaster){
            ecrt_release_master(ighMaster);
            ighMaster=nullptr;
            QThread::msleep(5000);
        }
        ighMaster = ecrt_request_master(0);
        if(ighMaster==nullptr){
            printf("Ethercat master request failed, no network card for ethercat");
            return false;
        }
        ecrt_master_state(ighMaster,&masterState);
        if(masterState.slaves_responding!=devices.size()){
            printf("mismatch slave count=%d,expected=%d\r\n",masterState.slaves_responding,devices.size());
            continue;
        }
        ec_slave_info_t slaveInfo;
        for(uint32_t i=0; i<masterState.slaves_responding; i++){
            ecrt_master_get_slave(ighMaster,i,&slaveInfo);
            EthercatDevice* dev=devices.at(i);
            dev->ethercatMaster=this;
            dev->ighMaster=ighMaster;
            if(slaveInfo.vendor_id!=dev->VID || slaveInfo.product_code!=dev->PID){
                printf("%s: configure:%X,%X , but device:%X,%X\r\n",__FUNCTION__,dev->VID,dev->PID,slaveInfo.vendor_id,slaveInfo.product_code);
                continue;
            }
            dev->slaveConfig=ecrt_master_slave_config(ighMaster, 0, slaveInfo.position, slaveInfo.vendor_id, slaveInfo.product_code);
            if(dev->slaveConfig==nullptr){
                printf("ecrt_master_slave_config failed,device=%d\r\n",i);
                continue;
            }
            dev->config();
            //printf("ec:device%d domainOffset=%d\r\n",i,dev->pdoDomainOffset);
        }
        ecrt_master_setup_domain_memory(ighMaster);
        //rt_printf("ec:totalDomainSize=%u\r\n",pdoDomainSize);
        for(size_t i=0; i<devices.size(); i++){
            devices.at(i)->activeDomain();
        }
        int ret=ecrt_master_activate(pimp->ighMaster);
        if(ret!=0){
            return false;
        }
        state = State::SYNCING;
        return true;
    }
    return false;
}

int64_t IghEthercatMaster::process(){
    if(state==State::INITING || state==State::ERROR){
        return 0;
    }
    ec_master_state_t& masterState = pimp->masterState;
    ec_master_t* ighMaster = pimp->ighMaster;
    int64_t phaseOffset=0;
    int64_t cycleCount=getCycleCount();

    ecrt_master_receive(ighMaster);
    for(size_t i=0;i<devices.size();i++){
        devices[i]->processRx();
    }
    for(size_t i=0;i<devices.size();i++){
        devices[i]->processTx();
    }
    {// process dc distribute clock
        if(cycleCount%200==50){
            ecrt_master_sync_monitor_queue(ighMaster);
        }else if(cycleCount%200==51){
            pimp->dcDiffNs=ecrt_master_sync_monitor_process(ighMaster);
        }
        if(cycleCount>=32){
            phaseOffset=getPhaseOffsetNs();
        }
        ecrt_master_application_time(ighMaster, rtMaster->getNextWakeupNs());
    }
    ecrt_master_sync_slave_clocks(ighMaster);    //set slaves_timer to ref_timer
    ecrt_master_send(ighMaster);

    if(cycleCount % 2000==0){  //check error
        ecrt_master_state(ighMaster,&masterState);
        if((masterState.al_states&0x07)==0){
            if(state == State::SYNCING){
                state=State::OPERATING;
            }
            pimp->stateErrored=false;
        }else if(cycleCount%40000==38000 && (( state > SYNCING && ( masterState.link_up==0) ) || state==SYNCING)){
            if(!pimp->stateErrored){
                pimp->stateErrored=true;
                rtdebug(__FUNCTION__,QString("check master warning: link:%1, num:%2 expected:%3, al_state:0x%4").arg(masterState.link_up).arg(masterState.slaves_responding).arg(devices.size()).arg(masterState.al_states,2,16,QChar('0')).toLocal8Bit().constData());
            }else{
                rtdebug(__FUNCTION__,QString("check master failed: link:%1, num:%2 expected:%3, al_state:0x%4").arg(masterState.link_up).arg(masterState.slaves_responding).arg(devices.size()).arg(masterState.al_states,2,16,QChar('0')).toLocal8Bit().constData());
                state=ERROR;
            }
        }
    }
    traceStatus->freqShift = pimp->freqShift;
    traceStatus->dcDiff = pimp->dcDiffNs;
    traceStatus->slaveNum = pimp->masterState.slaves_responding;
    traceStatus->alStatus = pimp->masterState.al_states;
    traceStatus->inited = state > State::INITING;
    traceStatus->linkup = pimp->masterState.link_up;
    traceStatus->synced = state==State::OPERATING;
    traceStatus->error  = state==State::ERROR;
    return phaseOffset;
}

void IghEthercatMaster::onExit(){
    /*for(size_t i=0;i<devices.size();i++){
        if(devices[i]->slaveConfig){
            ecrt_slave_config_dc(devices[i]->slaveConfig, 0, 0, 0, 0, 0);
        }
    }*/
    ecrt_master_deactivate_slaves(pimp->ighMaster);
    /*for(int i=0;i<10;i++){
        RtThread::sleepNs(getCycleNs());
        process();
    }*/
    if(pimp->ighMaster){
        ecrt_release_master(pimp->ighMaster);
        pimp->ighMaster=nullptr;
    }
}

int64_t IghEthercatMaster::getPhaseOffsetNs(){
    ec_master_t* ighMaster = pimp->ighMaster;
    int64_t& refTime=pimp->refTime;
    int64_t& freqShift=pimp->freqShift;
    int64_t cycleNs=getCycleNs();
    int64_t cycleCount=getCycleCount();
    bool& dcErrored=pimp->dcErrored;

    int ret=ecrt_master_reference_clock_time(ighMaster,(uint64_t*)&refTime);
    int64_t errorNs=0;
    int64_t limit=cycleNs/10;
    if(ret==0){
        errorNs = (pimp->refTime + cycleNs/2)%cycleNs - cycleNs/2;
        pimp->refDiffNs = errorNs;
        if(errorNs > limit){
            rt_printf("IghEthercatMaster:bigError refTime=%lld,errUs=%lld,freqShift=%lld,dcDiff=%lld @ %lld\r\n",refTime/1000,errorNs/1000,freqShift/1000,pimp->dcDiffNs,cycleCount);
            errorNs = limit;
        }else if(errorNs < -limit){
            rt_printf("IghEthercatMaster:bigError refTime=%lld,errUs=%lld,freqShift=%lld,dcDiff=%lld @ %lld\r\n",refTime/1000,errorNs/1000,freqShift/1000,pimp->dcDiffNs,cycleCount);
            errorNs = -limit;
        }
        freqShift+=errorNs/100;
        if(freqShift > limit){
            freqShift = limit;
        }else if(freqShift < -limit){
            freqShift = -limit;
        }
        if( (cycleCount%3000==0 && cycleCount<30000) || (cycleCount<100 && cycleCount%3==0)){
            rt_printf("IghEthercatMaster:log refTime=%lld,currentTime=%lld,errUs=%lld,freqShift=%lld,dcDiff=%lld @ %lld\r\n",refTime/1000,rtMaster->getNextWakeupNs()/1000,errorNs/1000,freqShift/1000,pimp->dcDiffNs,cycleCount);
        }
        if(dcErrored){
            dcErrored=false;
            rt_printf("ecrt_master_reference_clock_time resumed @ tick:",cycleCount);
        }
    }else{
        if(!dcErrored){
            dcErrored=true;
            rt_printf("ecrt_master_reference_clock_time return err @ tick:%2",cycleCount);
        }
    }
    return -(errorNs/10 + freqShift);
}

#endif

外部的实时线程会以1ms为周期调用 int64_t IghEthercatMaster::process() 这个函数，并根据这个函数的返回值在下一周期额外的延时ns时间

void RtMaster::threadEntry(){
    pimp->wakeupTime=RtThread::getTimeNs();
    pimp->wakeupTime += 2*pimp->cycleNs - pimp->wakeupTime%pimp->cycleNs + 30*1000;
    while(pimp->exitRequest==0){
        RtThread::sleepUntilNs(pimp->wakeupTime);
        tracer.reset(pimp->wakeupTime/1000);
        recordTrace(rtStatus->avgWake,rtStatus->maxWake,tracer.trace(RtThread::getTimeUs(),"wakeup"));
        if(ethercatMaster){
            int64_t phaseShift=ethercatMaster->process();
            pimp->wakeupTime+=phaseShift;
        }
        pimp->wakeupTime+=pimp->cycleNs;
        tracer.trace(RtThread::getTimeUs(),"ethercat");
        for(size_t i=0;i<axes.size();i++){
            axes[i]->processRx();
        }
        for(size_t i=0;i<axisGroups.size();i++){
            axisGroups[i]->processRx();
        }
        recordTrace(rtStatus->avgRx,rtStatus->maxRx,tracer.trace(RtThread::getTimeUs(),"processRx"));
        for(size_t i=0;i<preMotionDevices.size();i++){
            preMotionDevices[i]->processRx();
        }
        for(size_t i=0;i<preMotionDevices.size();i++){
            preMotionDevices[i]->processTx();
        }
        for(size_t i=0;i<axisGroups.size();i++){
            axisGroups[i]->processTx();
        }
        for(size_t i=0;i<axes.size();i++){
            axes[i]->processTx();
        }
        for(size_t i=0;i<postMotionDevices.size();i++){
            postMotionDevices[i]->processRx();
        }
        for(size_t i=0;i<postMotionDevices.size();i++){
            postMotionDevices[i]->processTx();
        }
        recordTrace(rtStatus->avgTx,rtStatus->maxTx,tracer.trace(RtThread::getTimeUs(),"processTx"));
        pimp->cycleCount++;
        rtStatus->cycleCount=pimp->cycleCount;
        if(rtStatus->resetTrace){
            memset(rtStatus,0,sizeof(RT_STATUS));
        }
    }
    for(size_t i=0;i<preMotionDevices.size();i++){
        preMotionDevices[i]->onExit();
    }
    for(size_t i=0;i<axisGroups.size();i++){
        axisGroups[i]->onExit();
    }
    for(size_t i=0;i<axes.size();i++){
        axes[i]->onExit();
    }
    for(size_t i=0;i<postMotionDevices.size();i++){
        postMotionDevices[i]->onExit();
    }
    if(ethercatMaster){
        ethercatMaster->onExit();
    }
    pimp->exitRequest=2;
}

还有一点是dev->config()函数

void EthercatCoolDriveR4::config(){
    domain = ecrt_master_create_domain(ighMaster);
    if ( ecrt_slave_config_pdos( slaveConfig, ARRAY_SIZE(syncs), syncs ) ) {
        rtdebug(__FUNCTION__,"error: ecrt_slave_config_pdos failed");
        return;
    }
    for(size_t i=0;i<ARRAY_SIZE(pdoEntries);i++){
        ecrt_slave_config_reg_pdo_entry(slaveConfig,pdoEntries[i].index,pdoEntries[i].subindex,domain,0);
    }
    ecrt_slave_config_dc(slaveConfig, 0x300, ethercatMaster->getCycleNs(), ethercatMaster->getCycleNs()*100 + ethercatMaster->getCycleNs()/2, 0, 0);
}

ecrt_slave_config_dc的shift参数被设置成cycle/2，这和getPhaseOffsetNs里的cycle/2是一样的