Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
examples/xenomai/main.c
changeset 2055 d246ab6b50d2
parent 2054 3417bbc4ad2f
child 2056 a92e8f119723
equal deleted inserted replaced
2054:3417bbc4ad2f 2055:d246ab6b50d2 
    43 #include <rtdm/rtdm.h>
 
    43 #include <rtdm/rtdm.h>
 
    44 #include <native/task.h>
 
    44 #include <native/task.h>
 
    45 #include <native/sem.h>
 
    45 #include <native/sem.h>
 
    46 #include <native/mutex.h>
 
    46 #include <native/mutex.h>
 
    47 #include <native/timer.h>
 
    47 #include <native/timer.h>
 
       
    48 #include <rtdk.h>
 
    48 #include <pthread.h>
 
    49 #include <pthread.h>
 
    49 
    50 
    50 /****************************************************************************/
 
    51 /****************************************************************************/
 
    51 
    52 
    52 #include "../../include/ecrt.h"
 
    53 #include "../../include/ecrt.h"
 
    56 RT_TASK my_task;
 
    57 RT_TASK my_task;
 
    57 
    58 
    58 int rt_fd = -1;
 
    59 int rt_fd = -1;
 
    59 int run=0;
 
    60 int run=0;
 
    60 
    61 
    61 struct timeval tv;
 
    62 //struct timeval tv;
 
    62 unsigned int sync_ref_counter = 0;
 
    63 unsigned int sync_ref_counter = 0;
 
    63 
    64 
    64 CstructMstrAttach MstrAttach;
 
    65 CstructMstrAttach MstrAttach;
 
    65 
    66 
    66 /****************************************************************************/
 
    67 /****************************************************************************/
 
    67 
    68 
    68 // Application parameters
 
    69 // Application parameters
 
    69 #define FREQUENCY 1000
 
    70 //#define FREQUENCY 1000
 
    70 #define PRIORITY 1
 
    71 //#define PRIORITY 1
 
    71 
    72 
    72 // Optional features
 
    73 // Optional features
 
    73 #define CONFIGURE_PDOS  1
 
    74 #define CONFIGURE_PDOS  1
 
    74 //#define SDO_ACCESS      1
 
    75 //#define SDO_ACCESS      1
 
    75 
    76 
   100 #define DigInSlave01_Pos    0, 3
 
   101 #define DigInSlave01_Pos    0, 3
 
   101 #define AnaOutSlave01_Pos   0, 4
 
   102 #define AnaOutSlave01_Pos   0, 4
 
   102 #define AnaInSlave01_Pos    0, 5
 
   103 #define AnaInSlave01_Pos    0, 5
 
   103 #define BusCoupler02_Pos    0, 6
 
   104 #define BusCoupler02_Pos    0, 6
 
   104 #define AnaInSlave02_Pos    0, 7
 
   105 #define AnaInSlave02_Pos    0, 7
 
   105 
   106 #define DPSlave01_Pos       0, 8
 
   106 
   107 
   107 
   108 
   108 #define Beckhoff_EK1100 0x00000002, 0x044c2c52
 
   109 #define Beckhoff_EK1100 0x00000002, 0x044c2c52
 
   109 #define Beckhoff_EL1014 0x00000002, 0x03f63052
 
   110 #define Beckhoff_EL1014 0x00000002, 0x03f63052
 
   110 #define Beckhoff_EL2004 0x00000002, 0x07d43052
 
   111 #define Beckhoff_EL2004 0x00000002, 0x07d43052
 
   427 
   428 
   428 
   429 
   429 void rt_check_domain_state(void)
 
   430 void rt_check_domain_state(void)
 
   430 {
 
   431 {
 
   431     ec_domain_state_t ds;
 
   432     ec_domain_state_t ds;
 
   432     unsigned int printed=0;
 
       
   433 
   433 
   434     if (rt_fd>=0)
 
   434     if (rt_fd>=0)
 
   435       {
 
   435       {
 
   436           ecrt_rtdm_domain_state(rt_fd,&ds);  
   436           ecrt_rtdm_domain_state(rt_fd,&ds);  
   437       }
 
   437       }
 
   438 
   438 
   439 /*    if (ds.working_counter != domain1_state.working_counter)
 
   439     if (ds.working_counter != domain1_state.working_counter)
 
   440      {
 
   440      {
 
   441         printf("Domain1: WC %u.\n", ds.working_counter);
 
   441         rt_printf("Domain1: WC %u.\n", ds.working_counter);
 
   442         printed=1;
 
       
   443 
       
   444      }
 
   442      }
 
   445     if (ds.wc_state != domain1_state.wc_state)
 
   443     if (ds.wc_state != domain1_state.wc_state)
 
   446      {
 
   444      {
 
   447     	printf("Domain1: State %u.\n", ds.wc_state);
 
   445     	rt_printf("Domain1: State %u.\n", ds.wc_state);
 
   448         printed=1;
 
       
   449 
       
   450      }
 
   446      }
 
   451     if (printed)
 
   447 
   452     {
 
       
   453     	int ret;
 
       
   454 
       
   455         // return to realtime mode after print
 
       
   456 
       
   457         ret = rt_task_set_mode(0, T_PRIMARY, NULL);
 
       
   458         if (ret)
 
       
   459          {
 
       
   460             printf("error while rt_task_set_mode, code %d\n",ret);
 
       
   461          }
 
       
   462     }
 
       
   463 */
 
       
   464     domain1_state = ds;
 
   448     domain1_state = ds;
 
   465 }
 
   449 }
 
   466 
   450 
   467 void rt_check_master_state(void)
 
   451 void rt_check_master_state(void)
 
   468 {
 
   452 {
 
   469     ec_master_state_t ms;
 
   453     ec_master_state_t ms;
 
   470     unsigned int printed=0;
 
       
   471 
   454 
   472     if (rt_fd>=0)
 
   455     if (rt_fd>=0)
 
   473       {
 
   456       {
 
   474           ecrt_rtdm_master_state(rt_fd,&ms);
 
   457           ecrt_rtdm_master_state(rt_fd,&ms);
 
   475       }
 
   458       }
 
   476 
   459 
   477 /*
 
       
   478     if (ms.slaves_responding != master_state.slaves_responding)
 
   460     if (ms.slaves_responding != master_state.slaves_responding)
 
   479     {
 
   461     {
 
   480         printf("%u slave(s).\n", ms.slaves_responding);
 
   462         rt_printf("%u slave(s).\n", ms.slaves_responding);
 
   481         printed=1;
 
       
   482 
       
   483     }
 
   463     }
 
   484     if (ms.al_states != master_state.al_states)
 
   464     if (ms.al_states != master_state.al_states)
 
   485     {
 
   465     {
 
   486         printf("AL states: 0x%02X.\n", ms.al_states);
 
   466         rt_printf("AL states: 0x%02X.\n", ms.al_states);
 
   487         printed=1;
 
       
   488 
       
   489     }
 
   467     }
 
   490     if (ms.link_up != master_state.link_up)
 
   468     if (ms.link_up != master_state.link_up)
 
   491     {
 
   469     {
 
   492         printf("Link is %s.\n", ms.link_up ? "up" : "down");
 
   470         rt_printf("Link is %s.\n", ms.link_up ? "up" : "down");
 
   493         printed=1;
 
   471     }
 
   494 
       
   495     }
 
       
   496     if (printed)
 
       
   497     {
 
       
   498     	int ret;
 
       
   499 
       
   500         // return to realtime mode after print
 
       
   501 
       
   502         ret = rt_task_set_mode(0, T_PRIMARY, NULL);
 
       
   503         if (ret)
 
       
   504          {
 
       
   505             printf("error while rt_task_set_mode, code %d\n",ret);
 
       
   506          }
 
       
   507          }*/
 
       
   508     master_state = ms;
 
   472     master_state = ms;
 
   509 }
 
   473 }
 
   510 
   474 
   511 void rt_receive()
 
   475 
   512 {
 
   476 
   513 
       
   514 	if (rt_fd>=0)
 
       
   515 	{
 
       
   516 		ecrt_rtdm_master_recieve(rt_fd);
 
       
   517 	}
 
       
   518 }
 
       
   519 
       
   520 void rt_send()
 
       
   521 {
 
       
   522 
       
   523 	if (rt_fd>=0)
 
       
   524 	{
 
       
   525 		ecrt_rtdm_master_send(rt_fd);
 
       
   526 	}
 
       
   527 }
 
       
   528 
   477 
   529 void rt_sync()
 
   478 void rt_sync()
 
   530 {
 
   479 {
 
   531   RTIME now;
 
   480   RTIME now;
 
   532   int ret;
 
       
   533   now = rt_timer_read();
 
   481   now = rt_timer_read();
 
   534   //now -= 946684800ULL * 1000000000ULL;
 
   482 
   535 
       
   536   printf("Write Sync Time %i\n",now);
 
       
   537   ret = rt_task_set_mode(0, T_PRIMARY, NULL);
 
       
   538   if (ret)
 
       
   539       {
 
       
   540           printf("error while rt_task_set_mode, code %d\n",ret);
 
       
   541       }
 
       
   542 
   483 
   543   if (rt_fd>=0)
 
   484   if (rt_fd>=0)
 
   544   {
 
   485   {
 
   545       ecrt_rtdm_master_application_time(rt_fd, &now);
 
   486       ecrt_rtdm_master_application_time(rt_fd, &now);
 
   546   }
 
   487   }
 
   563 /*****************************************************************************/
 
   504 /*****************************************************************************/
 
   564 
   505 
   565 #if SDO_ACCESS
 
   506 #if SDO_ACCESS
 
   566 void read_sdo(void)
 
   507 void read_sdo(void)
 
   567 {
 
   508 {
 
   568   switch (ecrt_sdo_request_state(sdo))
 
   509     switch (ecrt_sdo_request_state(sdo))
 
   569   {
 
   510         {
 
   570     case EC_REQUEST_UNUSED: // request was not used yet
 
   511         case EC_REQUEST_UNUSED: // request was not used yet
 
   571         ecrt_sdo_request_read(sdo); // trigger first read
 
   512             ecrt_sdo_request_read(sdo); // trigger first read
 
   572         break;
 
   513             break;
 
   573     case EC_REQUEST_BUSY:
 
   514         case EC_REQUEST_BUSY:
 
   574         fprintf(stderr, "Still busy...\n");
 
   515             fprintf(stderr, "Still busy...\n");
 
   575         break;
 
   516             break;
 
   576     case EC_REQUEST_SUCCESS:
 
   517         case EC_REQUEST_SUCCESS:
 
   577         fprintf(stderr, "SDO value: 0x%04X\n",
 
   518             fprintf(stderr, "SDO value: 0x%04X\n",
 
   578                 EC_READ_U16(ecrt_sdo_request_data(sdo)));
 
   519                     EC_READ_U16(ecrt_sdo_request_data(sdo)));
 
   579         ecrt_sdo_request_read(sdo); // trigger next read
 
   520             ecrt_sdo_request_read(sdo); // trigger next read
 
   580         break;
 
   521             break;
 
   581     case EC_REQUEST_ERROR:
 
   522         case EC_REQUEST_ERROR:
 
   582         fprintf(stderr, "Failed to read SDO!\n");
 
   523             fprintf(stderr, "Failed to read SDO!\n");
 
   583         ecrt_sdo_request_read(sdo); // retry reading
 
   524             ecrt_sdo_request_read(sdo); // retry reading
 
   584         break;
 
   525             break;
 
   585   }
 
   526         }
 
   586 }
 
   527 }
 
   587 
   528 
   588 void  PrintSDOState(void)
 
   529 void  PrintSDOState(void)
 
   589 {
 
   530 {
 
   590   switch (ecrt_sdo_request_state(sdo))
 
   531     switch (ecrt_sdo_request_state(sdo))
 
   591   {
 
   532         {
 
   592     case EC_REQUEST_UNUSED: // request was not used yet
 
   533         case EC_REQUEST_UNUSED: // request was not used yet
 
   593       fprintf(stderr, "SDO State: EC_REQUEST_UNUSED\n"); // trigger first read
 
   534             fprintf(stderr, "SDO State: EC_REQUEST_UNUSED\n"); // trigger first read
 
   594       break;
 
   535             break;
 
   595     case EC_REQUEST_BUSY:
 
   536         case EC_REQUEST_BUSY:
 
   596       fprintf(stderr, "SDO State: EC_REQUEST_BUSY\n");
 
   537             fprintf(stderr, "SDO State: EC_REQUEST_BUSY\n");
 
   597       break;
 
   538             break;
 
   598     case EC_REQUEST_SUCCESS:
 
   539         case EC_REQUEST_SUCCESS:
 
   599       fprintf(stderr, "SDO State: EC_REQUEST_SUCCESS\n");
 
   540             fprintf(stderr, "SDO State: EC_REQUEST_SUCCESS\n");
 
   600       break;
 
   541             break;
 
   601     case EC_REQUEST_ERROR:
 
   542         case EC_REQUEST_ERROR:
 
   602       fprintf(stderr, "SDO State: EC_REQUEST_ERROR\n");
 
   543             fprintf(stderr, "SDO State: EC_REQUEST_ERROR\n");
 
   603       break;
 
   544             break;
 
   604     default:
 
   545         default:
 
   605       fprintf(stderr, "SDO State: undefined\n");
 
   546             fprintf(stderr, "SDO State: undefined\n");
 
   606       break;
 
   547             break;
 
   607   }
 
   548   }
 
   608 }
 
   549 }
 
   609 #endif
 
   550 #endif
 
   610 
   551 
   611 
   552 
   644 
   585 
   645   run=1;
 
   586   run=1;
 
   646 
   587 
   647   ret = rt_task_set_mode(0, T_PRIMARY, NULL);
 
   588   ret = rt_task_set_mode(0, T_PRIMARY, NULL);
 
   648   if (ret) {
 
   589   if (ret) {
 
   649       printf("error while rt_task_set_mode, code %d\n",ret);
 
   590       rt_printf("error while rt_task_set_mode, code %d\n",ret);
 
   650       return;
 
   591       return;
 
   651   }
 
   592   }
 
   652   
   593   
   653 
   594 
   654   while (run) {
 
   595   while (run) {
 
   659           run=0;
 
   600           run=0;
 
   660           return;
 
   601           return;
 
   661       }
 
   602       }
 
   662       
   603       
   663       // receive ethercat
 
   604       // receive ethercat
 
   664       rt_receive();
 
   605       ecrt_rtdm_master_recieve(rt_fd);
 
       
   606       ecrt_rtdm_domain_process(rt_fd);
 
       
   607 
   665       rt_check_domain_state();
 
   608       rt_check_domain_state();
 
   666       
   609       
   667       if (divcounter ==0)
 
   610       if (divcounter ==0)
 
   668           {
 
   611           {
 
   669               divcounter=divider;
 
   612               divcounter=divider;
 
   682       
   625       
   683       //sync DC
 
   626       //sync DC
 
   684       rt_sync();
 
   627       rt_sync();
 
   685       
   628       
   686       // send process data
 
   629       // send process data
 
   687       rt_send();
 
   630       ecrt_rtdm_domain_queque(rt_fd);
 
       
   631       ecrt_rtdm_master_send(rt_fd);
 
   688   }
 
   632   }
 
   689   
   633   
   690 }
 
   634 }
 
   691 
   635 
   692 
   636 
   722 
   666 
   723     int rtstatus;
 
   667     int rtstatus;
 
   724 
   668 
   725     mlockall(MCL_CURRENT | MCL_FUTURE);
 
   669     mlockall(MCL_CURRENT | MCL_FUTURE);
 
   726 
   670 
       
   671     /* Perform auto-init of rt_print buffers if the task doesn't do so */
 
       
   672     rt_print_auto_init(1);
 
       
   673 
   727     signal(SIGTERM, catch_signal);
 
   674     signal(SIGTERM, catch_signal);
 
   728     signal(SIGINT, catch_signal);
 
   675     signal(SIGINT, catch_signal);
 
   729 
   676 
   730     MstrAttach.masterindex = 0;
 
   677     MstrAttach.masterindex = 0;
 
   731     
   678     
   849     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
 
   796     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
 
   850         fprintf(stderr, "PDO entry registration failed!\n");
 
   797         fprintf(stderr, "PDO entry registration failed!\n");
 
   851         return -1;
 
   798         return -1;
 
   852     }
 
   799     }
 
   853 #endif
 
   800 #endif
 
       
   801 
       
   802 
       
   803     printf("Get Configuring EL6731...\n");
 
       
   804     sc_dpslv_01 = ecrt_master_slave_config(master, DPSlave01_Pos, Beckhoff_EL6731);
 
       
   805     if (!sc_dpslv_01) {
 
       
   806         fprintf(stderr, "Failed to get slave configuration.\n");
 
       
   807         return -1;
 
       
   808     }
 
       
   809     
       
   810     printf("Configuring EL6731...\n");
 
       
   811     if (ecrt_slave_config_pdos(sc_dpslv_01, EC_END, slave_7_syncs))
 
       
   812         {
 
       
   813             fprintf(stderr, "Failed to configure PDOs.\n");
 
       
   814             return -1;
 
       
   815         }
 
       
   816     
       
   817 #if SDO_ACCESS
 
       
   818     
       
   819     
       
   820     // DP Slave Parameter Set
 
       
   821     fprintf(stderr, "Creating SDO requests...\n");
 
       
   822     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) {
 
       
   823         fprintf(stderr, "Failed to create SDO request.\n");
 
       
   824         return -1;
 
       
   825     }
 
       
   826     ecrt_sdo_request_timeout(sdo, 500); // ms
 
       
   827     EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0);
 
       
   828     PrintSDOState();
 
       
   829     ecrt_sdo_request_write(sdo);
 
       
   830     PrintSDOState();
 
       
   831     
       
   832     // Station Address
 
       
   833     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 1, 2))) {
 
       
   834         fprintf(stderr, "Failed to create SDO request.\n");
 
       
   835         return -1;
 
       
   836     }
 
       
   837     ecrt_sdo_request_timeout(sdo, 500); // ms
 
       
   838     EC_WRITE_U16(ecrt_sdo_request_data(sdo), 5);
 
       
   839     //EC_WRITE_U8(ecrt_sdo_request_data(sdo), 00);
 
       
   840     //EC_WRITE_U8(ecrt_sdo_request_data(sdo)+1, 10);
 
       
   841     PrintSDOState();
 
       
   842     ecrt_sdo_request_write(sdo);
 
       
   843     PrintSDOState();
 
       
   844     
       
   845     // Device Type (DP Ident Number)
 
       
   846     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 4, 4))) {
 
       
   847         fprintf(stderr, "Failed to create SDO request.\n");
 
       
   848         return -1;
 
       
   849     }
 
       
   850     ecrt_sdo_request_timeout(sdo, 500); // ms
 
       
   851     sdo_adr = ecrt_sdo_request_data(sdo);
 
       
   852     EC_WRITE_U32(sdo_adr, 0x095F);
 
       
   853     //EC_WRITE_U8(sdo_ad, 0x00); // Device Type
 
       
   854     //EC_WRITE_U8(sdo_adr+1, 0x00);
 
       
   855     //EC_WRITE_U8(sdo_adr+2, 0x09);
 
       
   856     //EC_WRITE_U8(sdo_adr+3, 0x5F);
 
       
   857     PrintSDOState();
 
       
   858     ecrt_sdo_request_write(sdo);
 
       
   859     PrintSDOState();
 
       
   860     
       
   861     // DP CfgData Slave
 
       
   862     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8002, 0, 244))) {
 
       
   863         fprintf(stderr, "Failed to create SDO request.\n");
 
       
   864         return -1;
 
       
   865     }
 
       
   866     ecrt_sdo_request_timeout(sdo, 500); // ms
 
       
   867     sdo_adr = ecrt_sdo_request_data(sdo);
 
       
   868     EC_WRITE_U8(sdo_adr, 0x10); // Device Type
 
       
   869     EC_WRITE_U8(sdo_adr+1, 0x20);
 
       
   870     PrintSDOState();
 
       
   871     ecrt_sdo_request_write(sdo);
 
       
   872     PrintSDOState();
 
       
   873     
       
   874     // DP Slave Parameter Set
 
       
   875     if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) {
 
       
   876         fprintf(stderr, "Failed to create SDO request.\n");
 
       
   877         return -1;
 
       
   878     }
 
       
   879     
       
   880     ecrt_sdo_request_timeout(sdo, 500); // ms
 
       
   881     
       
   882     EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0x33); // DP Slave Parameter Set
 
       
   883     PrintSDOState();
 
       
   884     ecrt_sdo_request_write(sdo);
 
       
   885     PrintSDOState();
 
       
   886 #endif
 
   854     
   887     
   855 
   888 
   856     
   889     
   857     sprintf(&rt_dev_file[0],"%s%u",EC_RTDM_DEV_FILE_NAME,0);
 
   890     sprintf(&rt_dev_file[0],"%s%u",EC_RTDM_DEV_FILE_NAME,0);
 
   858     
   891
etherlabmaster