pdort2free.comp

component pdort2free "Ethercat free run mode";

pin io u32 controlWord; 
pin io s32 targetVelocity;

pin out u32 statusWord; 
pin out s32 actualVelocity;


option extra_setup;
option extra_cleanup;

//option constructable no;

//function update nofp;
function update nofp;
license "GPL";

;;

//#include "/opt/ethercat/include/ecrt.h"
#include "/usr/include/ecrt.h"

#include <native/sem.h>
#include <native/timer.h>



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

// EtherCAT
static ec_master_t *master = NULL;
static ec_domain_t *domain1 = NULL;
static ec_slave_config_t *sc = NULL; // slave config for DC clock
static RT_SEM  	master_sem;
static ec_master_state_t master_status, old_status = {};
unsigned int sync_ref_counter = 0;

// process data
static uint8_t *domain1_pd = NULL;

// pdo definitions may be read from autogenerated file
//#include "ethercat_cstruct.c"


/* Master 0, Slave 0, "AEM-090-14"
 * Vendor ID:       0x000000AB
 * Product code:    0x00001030
 * Revision number: 0x00010003
 */
#define AEM_090_14 0x000000ab, 0x00001030   /* Copley AEM-090-14 */

#define PROFILE_VELOCITY 3     /* Operation mode for 0x6060:0*/

#define STS_FAULT  0x08        /* Fault. If set, a latched fault condition is present in the amplifier.*/
#define STS_QSTOP  0x20        /* Quick Stop. When clear, the amplifier is performing a quick stop.*/
#define STS_ILIMIT 0x800       /* Internal Limit Active. This bit is set when one of the amplifier limits (current, voltage, velocity or position) is active.*/

#define CTRL_ENA_QSTOP  0x00   /* enable quick stop   */
#define CTRL_DIS_QSTOP  0x06   /* disable quick stop  */
#define CTRL_ENA_SWION  0x07   /* enable switched  on */
#define CTRL_ENA_OP     0x0F   /* enable amplifier    */
#define CTRL_CLR_FAULT  0x80   /* clear STS_FAULT    */

/* Offsets for PDO entries */
static unsigned int off_aem_control;
static unsigned int off_aem_target_velocity;
static unsigned int off_aem_status;
static unsigned int off_aem_actual_velocity;

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    /* RxPdo 0x1600 */
    {0x6040, 0x00, 16},  /* ctrl word */
    {0x60FF, 0x00, 32},  /* target velocity */
	
	 /* TxPDO 0x1a00 */
    {0x6041, 0x00, 16}, /* status word */
    {0x606C, 0x00, 32}, /* actual velocity */
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1600, 2, slave_0_pdo_entries + 0},
    {0x1a00, 2, slave_0_pdo_entries + 2},
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_DISABLE}, // disable watchdog for RxPDO?
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},  // disable watchdog for TxPDO?
    {0xff}
};

const static ec_pdo_entry_reg_t domain1_pdo_regs[] = {
        { 255, 0,      AEM_090_14, 0x6040, 0, &off_aem_control, NULL},
        { 255, 0,      AEM_090_14, 0x60FF, 0, &off_aem_target_velocity, NULL},
        { 255, 0,      AEM_090_14, 0x6041, 0, &off_aem_status, NULL},
        { 255, 0,      AEM_090_14, 0x606C, 0, &off_aem_actual_velocity, NULL},

    {}
};

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/




/*****************************************************************************/
EXTRA_SETUP() {


     

    rtapi_print_msg(RTAPI_MSG_INFO,"Starting...\n");
   

    if (!(master = ecrt_request_master(0))) {
	rtapi_print_msg(RTAPI_MSG_ERR,"Requesting master 0 failed!\n");
        //printk(KERN_ERR PFX "Requesting master 0 failed!\n");
        goto out_return;
    }

	
	int ret;
	ret=rt_sem_create (&master_sem, NULL, 1, S_FIFO ); // Xenomai Native
	if( ret<0) {
     	rtapi_print_msg(RTAPI_MSG_ERR,"Semaphore creation failed: %d!\n", ret);
        //printk(KERN_ERR PFX "Domain creation failed!\n");
        goto out_release_master;
    };
	

	rtapi_print_msg(RTAPI_MSG_INFO,"Registering domain...\n");
    if (!(domain1 = ecrt_master_create_domain(master))) {
	rtapi_print_msg(RTAPI_MSG_ERR,"Domain creation failed!\n");
       
        goto out_release_master;
    }
    
	/* Configure DC and app time
	   https://github.com/sittner/linuxcnc-ethercat/blob/master/src/lcec_main.c#L465
	*/
    rtapi_print_msg(RTAPI_MSG_INFO,"Registering PDOs...\n");
	if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_pdo_regs)!=0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,"PDO registration failed!\n");
        goto out_release_master;
    }
	
	   // create configuration for reference clock FIXME
       if (!(sc = ecrt_master_slave_config(master, 0, 0, AEM_090_14))) {
	       rtapi_print_msg(RTAPI_MSG_ERR,"Obtaining slave config for DC failed!\n");
           goto out_release_master;
       }
	   
	   
	   

	   
	   if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
           rtapi_print_msg(RTAPI_MSG_ERR,"PDO configuration failed!\n");
           goto out_release_master;
       }
	   
	/*rtapi_print_msg(RTAPI_MSG_INFO,"Registering PDOs...\n");
	if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_pdo_regs)!=0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,"PDO registration failed!\n");
        goto out_release_master;
    }*/
  
	   
	 // Set Operational Mode
	 /* ethercat download --type uint16 0x2300 0 30
	  AMP_MODE=30 In servo mode, the position loop is driven by the CANopen interface. */
	 // Have to check if this is necessary
	 ecrt_slave_config_sdo16( sc, 0x2300, 0, 30);  
	 
	 ecrt_slave_config_sdo8( sc, 0x6060, 0, PROFILE_VELOCITY );
	
	 // configure SYNC signals for this slave
     ecrt_slave_config_dc(sc, 0x0000, 0, 0, 0, 0); // free run mode

	rtapi_print_msg(RTAPI_MSG_INFO,"Activating master...\n");
    if (ecrt_master_activate(master)) {
	    rtapi_print_msg(RTAPI_MSG_ERR,"Failed to activate master!\n");
        goto out_release_master;
    }

    /* In userspace context: This method has to be called after ecrt_master_activate()
       to get the mapped domain process data memory. */
    domain1_pd = ecrt_domain_data(domain1);
	if (domain1_pd==NULL){
    	rtapi_print_msg(RTAPI_MSG_ERR,"Domain's process data unavailable.\n");
		goto  out_release_master;
		}
		
	
    rtapi_print_msg(RTAPI_MSG_INFO,"Started.\n");
    return 0;

 out_release_master:
	rtapi_print_msg(RTAPI_MSG_INFO,"Releasing master...\n");
    ecrt_release_master(master);
    master = NULL;
	return -1;
 out_return:
	rtapi_print_msg(RTAPI_MSG_ERR,"Failed to load. Aborting.\n");
    //printk(KERN_ERR PFX "Failed to load. Aborting.\n");
    return -1;
}

EXTRA_CLEANUP() {
	
	rtapi_print_msg(RTAPI_MSG_INFO,"Stopping...\n");
	
	if (master) {
		rtapi_print_msg(RTAPI_MSG_INFO,"Releasing master...\n");
		ecrt_release_master(master);
	 //	ecrt_master_deactivate(master->master);
	}
	
    rt_sem_delete (&master_sem); // delete master semaphore only after releasing the master?
	
	rtapi_print_msg(RTAPI_MSG_INFO,"Unloading.\n");
}

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

/*****************************************************/
FUNCTION(update) { 

      rt_timer_set_mode	(TM_ONESHOT);
      RTIME time_begin = rt_timer_read();
	 
	  

   // receive
   rt_sem_p(&master_sem, TM_INFINITE);
   ecrt_master_receive(master);
   ecrt_domain_process(domain1);
   rt_sem_v(&master_sem);
		
/*pin in u32 controlWord; 
pin in s32 targetVelocity;
pin out u32 statusWord; 
pin out s32 actualVelocity;*/

	statusWord =(uint32_t) EC_READ_U16( domain1_pd+off_aem_status);
	actualVelocity = EC_READ_S32( domain1_pd+off_aem_actual_velocity);	

	//EC_WRITE_U32(domain1_pd+off_aem_control, controlWord); // or rather EC_WRITE_U16 uint16_t
	
	/* DS402 CANopen over EtherCAT status machine */
	
	if ( (statusWord & STS_FAULT ) >> 3) {
		rtapi_print_msg(RTAPI_MSG_INFO,"Latched Fault Condition\n");
        controlWord = (uint32_t) CTRL_CLR_FAULT;
    }
	else {
	    if ( (statusWord & STS_QSTOP ) >> 7) {
		    rtapi_print_msg(RTAPI_MSG_INFO,"Quick Stop bit set\n");
			controlWord = CTRL_DIS_QSTOP;
			}
			else {
			     controlWord = CTRL_ENA_OP;
			}
	}
   
	EC_WRITE_U16(domain1_pd+off_aem_control, (uint16_t) controlWord);
	EC_WRITE_S32(domain1_pd+off_aem_target_velocity, targetVelocity);
	

        rt_sem_p(&master_sem, TM_INFINITE);
        ecrt_master_state(master, &master_status);
        rt_sem_v(&master_sem);

        if (master_status.al_states != old_status.al_states) {
		rtapi_print_msg(RTAPI_MSG_INFO,"bus status changed to %i.\n", master_status.al_states);
        }
        if (master_status.slaves_responding !=
                old_status.slaves_responding) {
			rtapi_print_msg(RTAPI_MSG_INFO,"slaves_responding changed to %u.\n", master_status.slaves_responding);
        }

        old_status = master_status;


    	// send
    	rt_sem_p(&master_sem, TM_INFINITE);
		
    	ecrt_domain_queue(domain1);
    //	rt_sem_v(&master_sem);

     //	rt_sem_p(&master_sem, TM_INFINITE);
    	ecrt_master_send(master);
    	rt_sem_v(&master_sem);
		
		RTIME time_end = rt_timer_read();
		rtapi_print_msg(RTAPI_MSG_INFO,"Cycle %d ns.\n", time_end-time_begin);

}