API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 3rd, 2011
168
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 16.96 KB | None | 0 0
raw download clone embed print report
  1. /******************************************************************************
  2.  
  3.  *
  4.  
  5.  *  RTAI sample for the IgH EtherCAT master.
  6.  
  7.  *
  8.  
  9.  *  $Id$
  10.  
  11.  *
  12.  
  13.  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
  14.  
  15.  *
  16.  
  17.  *  This file is part of the IgH EtherCAT Master.
  18.  
  19.  *
  20.  
  21.  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
  22.  
  23.  *  modify it under the terms of the GNU General Public License version 2, as
  24.  
  25.  *  published by the Free Software Foundation.
  26.  
  27.  *
  28.  
  29.  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
  30.  
  31.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  32.  
  33.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
  34.  
  35.  *  Public License for more details.
  36.  
  37.  *
  38.  
  39.  *  You should have received a copy of the GNU General Public License along
  40.  
  41.  *  with the IgH EtherCAT Master; if not, write to the Free Software
  42.  
  43.  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  44.  
  45.  *
  46.  
  47.  *  ---
  48.  
  49.  *
  50.  
  51.  *  The license mentioned above concerns the source code only. Using the
  52.  
  53.  *  EtherCAT technology and brand is only permitted in compliance with the
  54.  
  55.  *  industrial property and similar rights of Beckhoff Automation GmbH.
  56.  
  57.  *
  58.  
  59.  *****************************************************************************/
  60.  
  61. // Linux
  62.  
  63. #include <linux/module.h>
  64.  
  65. #include <linux/err.h>
  66.  
  67.  
  68.  
  69. // RTAI
  70.  
  71. #include <rtai_sched.h>
  72.  
  73. #include <rtai_sem.h>
  74.  
  75.  
  76.  
  77. // EtherCAT
  78.  
  79. #include "../../include/ecrt.h"
  80.  
  81.  
  82.  
  83. /*****************************************************************************/
  84.  
  85.  
  86.  
  87. // Module parameters
  88.  
  89.  
  90.  
  91. #define FREQUENCY 2000 // task frequency in Hz
  92.  
  93. #define INHIBIT_TIME 20
  94.  
  95.  
  96.  
  97. #define TIMERTICKS (1000000000 / FREQUENCY)
  98.  
  99.  
  100.  
  101. // Optional features (comment to disable)
  102.  
  103. #define CONFIGURE_PDOS
  104.  
  105.  
  106.  
  107. #define PFX "ec_rtai_sample: "
  108.  
  109.  
  110.  
  111.  
  112.  
  113. #define WAIT   1000000000
  114.  
  115. /*****************************************************************************/
  116.  
  117.  
  118.  
  119. // EtherCAT
  120.  
  121. static ec_master_t *master = NULL;
  122.  
  123. static ec_master_state_t master_state = {};
  124.  
  125.  
  126.  
  127. static ec_domain_t *domain1 = NULL;
  128.  
  129. static ec_domain_state_t domain1_state = {};
  130.  
  131.  
  132.  
  133. static ec_slave_config_t *sc_ana_in = NULL;
  134.  
  135. static ec_slave_config_state_t sc_ana_in_state = {};
  136.  
  137.  
  138.  
  139. // RTAI
  140.  
  141. static RT_TASK task;
  142.  
  143. static SEM master_sem;
  144.  
  145. static cycles_t t_last_cycle = 0, t_critical;
  146.  
  147. static RTIME warte;
  148.  
  149.  
  150.  
  151. /********************************************************************************/
  152.  
  153. /*  Teile dem Master die erwartete Bustopologie mit durch Erzeugen von*******/
  154.  
  155. /*          Slave-configurations=expected slaves            */
  156.  
  157. /********************************************************************************/
  158.  
  159.  
  160.  
  161. // process data
  162.  
  163. static uint8_t *domain1_pd; // process data memory
  164.  
  165.  
  166.  
  167. #define BusCouplerPos  0, 0
  168.  
  169. #define DigInSlavePos 0, 2
  170.  
  171. #define DigOutSlavePos 0, 3
  172.  
  173. #define AnaInSlavePos 0, 1
  174.  
  175.  
  176.  
  177. #define Beckhoff_EK1101 0x00000002, 0x044d2c52
  178.  
  179. #define Beckhoff_EL1004 0x00000002, 0x03ec3052
  180.  
  181. #define Beckhoff_EL2004 0x00000002, 0x07D43052
  182.  
  183. #define Beckhoff_EL3054 0x00000002, 0x0bee3052
  184.  
  185.  
  186.  
  187. //static unsigned int *off_dig_out, *off_dig_in, *off_ana_in_over,*off_ana_in_val;
  188.  
  189. static unsigned int off_dig_out, off_dig_in, off_ana_in_over,off_ana_in_val;
  190.  
  191.  
  192.  
  193. /********************************************************************************/
  194.  
  195. /*      Erzeuge eine Domäne um Process-Data-Image zu verwalten     */
  196.  
  197. /**************************(Registriere Slaves)**********************************/
  198.  
  199. /* DOMÄNE: erlauben gruppierten PDO-Austausch                  */
  200.  
  201. /* PDOs: Slaves bieten ihre In- und Outputs dem master an mit Hilfe von PDOs.   */
  202.  
  203. /* Die zur Verfügung stehenden PDOs müssen aus den slaves gelesen werden  */
  204.  
  205. /*(ethercat xml)                                */
  206.  
  207. /* PROCESS-DATA-IMAGE: Die Applikation kann die PDO-Einträge für den Austausch  */
  208.  
  209. /* während zyklischer Operation in der Domäne registrieren.           */
  210.  
  211. /* Die Summe aller registrierten PDO-Einträge definiert das Process-Data-Image */     
  212.  
  213. /********************************************************************************/
  214.  
  215.  
  216.  
  217.  
  218.  
  219. const static ec_pdo_entry_reg_t domain1_regs[] = {
  220.  
  221.     {DigInSlavePos, Beckhoff_EL1004, 0x6000, 1, &off_dig_in},
  222.  
  223.     {DigOutSlavePos, Beckhoff_EL2004, 0x7000, 1, &off_dig_out},
  224.  
  225.     {AnaInSlavePos, Beckhoff_EL3054, 0x6000, 17, &off_ana_in_val},
  226.  
  227. //  {AnaInSlavePos, Beckhoff_EL3054, 0x6000, 2, &off_ana_in_over},
  228.  
  229. //  {alias,position,vendorID,product code,index,subindex,offset,bit_position}  
  230.  
  231.     {}
  232.  
  233. };
  234.  
  235.  
  236.  
  237. /********************************************************************************/
  238.  
  239. /*              Registriere PDOs                */
  240.  
  241. /********************************************************************************/
  242.  
  243.  
  244.  
  245. #ifdef CONFIGURE_PDOS
  246.  
  247.  
  248.  
  249. /************************************************/
  250.  
  251. /*      DIGITAL OUTPUTS         */
  252.  
  253. /*      (EL2004)            */
  254.  
  255. /************************************************/
  256.  
  257.  
  258.  
  259. //Registriere 4 PDOs
  260.  
  261. static ec_pdo_entry_info_t el2004_pdos[] = {
  262.  
  263.     {0x7000, 1, 1}, // registriere PDO  mit Index 0x7000  
  264.  
  265.     {0x7010, 1, 1},  
  266.  
  267.     {0x7020, 1, 1},  
  268.  
  269.     {0x7030, 1, 1},  
  270.  
  271. //  {PDO entry index,PDO entry subindex,Size of PDO entry in bit}
  272.  
  273. };
  274.  
  275.  
  276.  
  277. //Alle 4 PDOs sind auf 4 unterschiedlichen Channels
  278.  
  279. static ec_pdo_info_t el2004_channels[] = {
  280.  
  281.     {0x1600, 1, &el2004_pdos[0]}, //Auf Channel 0 bei Adresse 0x1600 ist PDO mit ID 0x7000
  282.  
  283.     {0x1601, 1, &el2004_pdos[1]},
  284.  
  285.     {0x1602, 1, &el2004_pdos[2]},
  286.  
  287.     {0x1603, 1, &el2004_pdos[3]}
  288.  
  289. //  {PDO index, Number of PDO entries in entries to map, Array of PDO entries to map}
  290.  
  291. };
  292.  
  293.  
  294.  
  295. static ec_sync_info_t el2004_syncs[] = {
  296.  
  297.     {1, EC_DIR_OUTPUT, 4, el2004_channels},
  298.  
  299. //  {Sync Manager Index, Sync Manager Direction, Number of PDOs in pdos,Array with PDOs to assign, watchdog modules}
  300.  
  301.   {0xff}
  302.  
  303. };
  304.  
  305.  
  306.  
  307. /************************************************/
  308.  
  309. /*      ANALOG INPUTS           */
  310.  
  311. /*      (EL3054)            */
  312.  
  313. /************************************************/
  314.  
  315.  
  316.  
  317. //Registriere PDOs
  318.  
  319. static ec_pdo_entry_info_t el3054_pdos_ch0[] = {
  320.  
  321. //    {0x6000, 1, 1},  //Underrange event active
  322.  
  323. //    {0x6000, 2, 1},  //Overrange event active
  324.  
  325. //    {0x6000, 3, 2},  //Bit0: Value smaller/equal Limit1 Bit1: Value bigger/equal Limit1
  326.  
  327. //    {0x6000, 5, 2},  //Bit0: Value smaller/equal Limit2 Bit1: Value bigger/equal Limit2
  328.  
  329. //    {0x6000, 7, 1},  //Bit set when Over- or Underrange
  330.  
  331. //    {0x1800, 7, 1},  //TRUE when this TxPDO is not valid
  332.  
  333. //    {0x1800, 9, 1},  //Bit toggles everytime when new value available
  334.  
  335.     {0x6000, 17, 16},//Value  
  336.  
  337. //  {PDO entry index,PDO entry subindex,Size of PDO entry in bit}
  338.  
  339. };
  340.  
  341.  
  342.  
  343. //Alle PDOs sind auf dem selben Channel (0x1a00)
  344.  
  345. static ec_pdo_info_t el3054_channel0[] = {
  346.  
  347. //  {0x1a00, 8, el3054_pdos_ch0},
  348.  
  349.     {0x1a00, 1, &el3054_pdos_ch0[0]},
  350.  
  351. //    {0x1a00, 1, &el3054_pdos_ch0[1]},
  352.  
  353. //    {0x1a00, 1, &el3054_pdos_ch0[2]},
  354.  
  355. //    {0x1a00, 1, &el3054_pdos_ch0[3]},
  356.  
  357. //    {0x1a00, 1, &el3054_pdos_ch0[4]},
  358.  
  359. //    {0x1a00, 1, &el3054_pdos_ch0[5]},
  360.  
  361. //    {0x1a00, 1, &el3054_pdos_ch0[6]},
  362.  
  363. //    {0x1a00, 1, &el3054_pdos_ch0[7]},
  364.  
  365. //  {PDO index, Number of PDO entries in entries to map, Array of PDO entries to map}
  366.  
  367. };
  368.  
  369.  
  370.  
  371. static ec_sync_info_t el3054_syncs[] = {
  372.  
  373.     {1, EC_DIR_INPUT, 1, el3054_channel0},
  374.  
  375. //  {Sync Manager Index, Sync Manager Direction, Number of PDOs in pdos,Array with PDOs to assign, watchdog modules}
  376.  
  377.   {0xff}
  378.  
  379. };
  380.  
  381.  
  382.  
  383.  
  384.  
  385. /************************************************/
  386.  
  387. /*      DIGITAL INPUTS          */
  388.  
  389. /*      (EL1004)            */
  390.  
  391. /************************************************/
  392.  
  393.  
  394.  
  395. //Registriere 4 PDOs
  396.  
  397. static ec_pdo_entry_info_t el1004_pdos[] = {
  398.  
  399.     {0x6000, 1, 1},
  400.  
  401.     {0x6010, 1, 1},
  402.  
  403.     {0x6020, 1, 1},
  404.  
  405.     {0x6030, 1, 1},
  406.  
  407. };
  408.  
  409.  
  410.  
  411. //Alle 4 PDOs sind auf unterschiedlichen Channels
  412.  
  413. static ec_pdo_info_t el1004_channels[] = {
  414.  
  415.     {0x1a00, 1, &el1004_pdos[0]},
  416.  
  417.     {0x1a01, 1, &el1004_pdos[1]},
  418.  
  419.     {0x1a02, 1, &el1004_pdos[2]},
  420.  
  421.     {0x1a03, 1, &el1004_pdos[3]}
  422.  
  423. };
  424.  
  425.  
  426.  
  427. static ec_sync_info_t el1004_syncs[] = {
  428.  
  429.     {1, EC_DIR_INPUT,4,el1004_channels},
  430.  
  431. //  {Sync Manager Index, Sync Manager Direction, Number of PDOs in pdos,Array with PDOs to assign, watchdog modules}
  432.  
  433.     {0xff}
  434.  
  435. };
  436.  
  437.  
  438.  
  439. #endif
  440.  
  441.  
  442.  
  443. /*****************************************************************************/
  444.  
  445.  
  446.  
  447. void check_domain1_state(void)
  448.  
  449. {
  450.  
  451.     ec_domain_state_t ds;
  452.  
  453.  
  454.  
  455.     rt_sem_wait(&master_sem);
  456.  
  457.     ecrt_domain_state(domain1, &ds);
  458.  
  459.     rt_sem_signal(&master_sem);
  460.  
  461.  
  462.  
  463.     if (ds.working_counter != domain1_state.working_counter)
  464.  
  465.         printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter);
  466.  
  467.     if (ds.wc_state != domain1_state.wc_state)
  468.  
  469.         printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state);
  470.  
  471.  
  472.  
  473.     domain1_state = ds;
  474.  
  475. }
  476.  
  477.  
  478.  
  479. /*****************************************************************************/
  480.  
  481.  
  482.  
  483. void check_master_state(void)
  484.  
  485. {
  486.  
  487.     ec_master_state_t ms;
  488.  
  489.  
  490.  
  491.     rt_sem_wait(&master_sem);
  492.  
  493.     ecrt_master_state(master, &ms);
  494.  
  495.     rt_sem_signal(&master_sem);
  496.  
  497.  
  498.  
  499.     if (ms.slaves_responding != master_state.slaves_responding)
  500.  
  501.         printk(KERN_INFO PFX "%u slave(s).\n", ms.slaves_responding);
  502.  
  503.     if (ms.al_states != master_state.al_states)
  504.  
  505.         printk(KERN_INFO PFX "AL states: 0x%02X.\n", ms.al_states);
  506.  
  507.     if (ms.link_up != master_state.link_up)
  508.  
  509.         printk(KERN_INFO PFX "Link is %s.\n", ms.link_up ? "up" : "down");
  510.  
  511.  
  512.  
  513.     master_state = ms;
  514.  
  515. }
  516.  
  517.  
  518.  
  519. /*****************************************************************************/
  520.  
  521.  
  522.  
  523. void check_slave_config_states(void)
  524.  
  525. {
  526.  
  527.     ec_slave_config_state_t s;
  528.  
  529.  
  530.  
  531.     rt_sem_wait(&master_sem);
  532.  
  533.     ecrt_slave_config_state(sc_ana_in, &s);
  534.  
  535.     rt_sem_signal(&master_sem);
  536.  
  537.  
  538.  
  539.     if (s.al_state != sc_ana_in_state.al_state)
  540.  
  541.         printk(KERN_INFO PFX "AnaIn: State 0x%02X.\n", s.al_state);
  542.  
  543.     if (s.online != sc_ana_in_state.online)
  544.  
  545.         printk(KERN_INFO PFX "AnaIn: %s.\n", s.online ? "online" : "offline");
  546.  
  547.     if (s.operational != sc_ana_in_state.operational)
  548.  
  549.         printk(KERN_INFO PFX "AnaIn: %soperational.\n",
  550.  
  551.                 s.operational ? "" : "Not ");
  552.  
  553.  
  554.  
  555.     sc_ana_in_state = s;
  556.  
  557. }
  558.  
  559.  
  560.  
  561. /*****************************************************************************/
  562.  
  563.  
  564.  
  565. void run(long data)
  566.  
  567. {
  568.  
  569.     //while (1)
  570.  
  571.     //{
  572.  
  573.         t_last_cycle = get_cycles();
  574.  
  575.  
  576.  
  577.         // receive process data
  578.  
  579.         rt_sem_wait(&master_sem);
  580.  
  581.         ecrt_master_receive(master);
  582.  
  583.         ecrt_domain_process(domain1);
  584.  
  585.         rt_sem_signal(&master_sem);
  586.  
  587.  
  588.  
  589.         // check process data state (optional)
  590.  
  591.         check_domain1_state();
  592.  
  593.  
  594.  
  595.         EC_WRITE_BIT(domain1_pd + off_dig_out, 0,1);
  596.  
  597.         EC_WRITE_BIT(domain1_pd + off_dig_out, 1,1);
  598.  
  599.         EC_WRITE_BIT(domain1_pd + off_dig_out, 2,1);
  600.  
  601.         EC_WRITE_BIT(domain1_pd + off_dig_out, 3,1);
  602.  
  603.  
  604.  
  605.     /*  printk("---------------------BINÄR----------------------\n");
  606.  
  607.         printk("EL1004 Eingabe 2^0:%d\n", EC_READ_BIT(domain1_pd+off_dig_in,0));
  608.  
  609.         printk("EL1004 Eingabe 2^1:%d\n", EC_READ_BIT(domain1_pd+off_dig_in,1));
  610.  
  611.         printk("EL1004 Eingabe 2^2:%d\n", EC_READ_BIT(domain1_pd+off_dig_in,2));
  612.  
  613.         printk("EL1004 Eingabe 2^3:%d\n", EC_READ_BIT(domain1_pd+off_dig_in,3));
  614.  
  615.     */
  616.  
  617.         rt_sem_wait(&master_sem);
  618.  
  619.         ecrt_domain_queue(domain1);
  620.  
  621.         ecrt_master_send(master);
  622.  
  623.         rt_sem_signal(&master_sem);
  624.  
  625.    
  626.  
  627.     //      rt_task_wait_period();
  628.  
  629.     //}
  630.  
  631. }
  632.  
  633.  
  634.  
  635. /*****************************************************************************/
  636.  
  637.  
  638.  
  639. void send_callback(void *cb_data)
  640.  
  641. {
  642.  
  643.     ec_master_t *m = (ec_master_t *) cb_data;
  644.  
  645.  
  646.  
  647.     // too close to the next real time cycle: deny access...
  648.  
  649.     if (get_cycles() - t_last_cycle <= t_critical) {
  650.  
  651.         rt_sem_wait(&master_sem);
  652.  
  653.         ecrt_master_send_ext(m);
  654.  
  655.         rt_sem_signal(&master_sem);
  656.  
  657.     }
  658.  
  659. }
  660.  
  661.  
  662.  
  663. /*****************************************************************************/
  664.  
  665.  
  666.  
  667. void receive_callback(void *cb_data)
  668.  
  669. {
  670.  
  671.     ec_master_t *m = (ec_master_t *) cb_data;
  672.  
  673.  
  674.  
  675.     // too close to the next real time cycle: deny access...
  676.  
  677.     if (get_cycles() - t_last_cycle <= t_critical) {
  678.  
  679.         rt_sem_wait(&master_sem);
  680.  
  681.         ecrt_master_receive(m);
  682.  
  683.         rt_sem_signal(&master_sem);
  684.  
  685.     }
  686.  
  687. }
  688.  
  689.  
  690.  
  691. /*****************************************************************************/
  692.  
  693.  
  694.  
  695. int __init init_mod(void)
  696.  
  697. {
  698.  
  699.     int ret = -1;
  700.  
  701.     RTIME tick_period, requested_ticks, now;
  702.  
  703. #ifdef CONFIGURE_PDOS
  704.  
  705.     ec_slave_config_t *sc;
  706.  
  707. #endif
  708.  
  709.  
  710.  
  711.     printk(KERN_INFO PFX "Starting...\n");
  712.  
  713.  
  714.  
  715.     rt_sem_init(&master_sem, 1);
  716.  
  717.  
  718.  
  719.     t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
  720.  
  721.  
  722.  
  723.     master = ecrt_request_master(0);
  724.  
  725.     if (!master) {
  726.  
  727.         ret = -EBUSY;
  728.  
  729.         printk(KERN_ERR PFX "Requesting master 0 failed!\n");
  730.  
  731.         goto out_return;
  732.  
  733.     }
  734.  
  735.  
  736.  
  737.     ecrt_master_callbacks(master, send_callback, receive_callback, master);
  738.  
  739.  
  740.  
  741.     printk(KERN_INFO PFX "Registering domain...\n");
  742.  
  743.     if (!(domain1 = ecrt_master_create_domain(master))) {
  744.  
  745.         printk(KERN_ERR PFX "Domain creation failed!\n");
  746.  
  747.         goto out_release_master;
  748.  
  749.     }
  750.  
  751.  
  752.  
  753. #ifdef CONFIGURE_PDOS
  754.  
  755.     printk(KERN_INFO PFX "Configuring PDOs...\n");
  756.  
  757.  
  758.  
  759.     if (!(sc = ecrt_master_slave_config(master, DigOutSlavePos, Beckhoff_EL2004))) {
  760.  
  761.         printk(KERN_ERR PFX "Failed to get slave configuration.\n");
  762.  
  763.         goto out_release_master;
  764.  
  765.     }
  766.  
  767.  
  768.  
  769.     if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) {
  770.  
  771.         printk(KERN_ERR PFX "Failed to configure PDOs.\n");
  772.  
  773.         goto out_release_master;
  774.  
  775.     }
  776.  
  777.  
  778.  
  779.     if (!(sc = ecrt_master_slave_config(master, AnaInSlavePos, Beckhoff_EL3054))) {
  780.  
  781.         printk(KERN_ERR PFX "Failed to get slave configuration.\n");
  782.  
  783.         goto out_release_master;
  784.  
  785.     }
  786.  
  787.  
  788.  
  789.     if (ecrt_slave_config_pdos(sc, EC_END, el3054_syncs)) {
  790.  
  791.         printk(KERN_ERR PFX "Failed to configure PDOs.\n");
  792.  
  793.         goto out_release_master;
  794.  
  795.     }
  796.  
  797.  
  798.  
  799.  
  800.  
  801.    
  802.  
  803.    if (!(sc = ecrt_master_slave_config(master, DigInSlavePos, Beckhoff_EL1004))) {
  804.  
  805.         printk(KERN_ERR PFX "Failed to get slave configuration.\n");
  806.  
  807.         goto out_release_master;
  808.  
  809.     }
  810.  
  811.     if (ecrt_slave_config_pdos(sc, EC_END, el1004_syncs)) {
  812.  
  813.         printk(KERN_ERR PFX "Failed to configure PDO_1004.\n");
  814.  
  815.         goto out_release_master;
  816.  
  817.     }  
  818.  
  819.  
  820.  
  821.   // Create configuration for bus coupler
  822.  
  823.     sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1101);
  824.  
  825.     if (!sc)
  826.  
  827.         return -1;
  828.  
  829.  
  830.  
  831.     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
  832.  
  833.         return -1;
  834.  
  835.     }
  836.  
  837.  
  838.  
  839.  
  840.  
  841.  
  842.  
  843. #endif
  844.  
  845.  
  846.  
  847.     printk(KERN_INFO PFX "Registering PDO entries...\n");
  848.  
  849.     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
  850.  
  851.         printk(KERN_ERR PFX "PDO entry registration failed!\n");
  852.  
  853.         goto out_release_master;
  854.  
  855.     }
  856.  
  857.  
  858.  
  859.     printk(KERN_INFO PFX "Activating master...\n");
  860.  
  861.     if (ecrt_master_activate(master)) {
  862.  
  863.         printk(KERN_ERR PFX "Failed to activate master!\n");
  864.  
  865.         goto out_release_master;
  866.  
  867.     }
  868.  
  869.  
  870.  
  871.     // Get internal process data for domain
  872.  
  873.     domain1_pd = ecrt_domain_data(domain1);
  874.  
  875.  
  876.  
  877.     printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
  878.  
  879.     requested_ticks = nano2count(TIMERTICKS);
  880.  
  881.     tick_period = start_rt_timer(requested_ticks);
  882.  
  883.     printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
  884.  
  885.            (int) tick_period, (int) requested_ticks);
  886.  
  887.  
  888.  
  889.     if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
  890.  
  891.         printk(KERN_ERR PFX "Failed to init RTAI task!\n");
  892.  
  893.         goto out_stop_timer;
  894.  
  895.     }
  896.  
  897.  
  898.  
  899.  
  900.  
  901.  
  902.  
  903.     now = rt_get_time();
  904.  
  905.     if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
  906.  
  907.         printk(KERN_ERR PFX "Failed to run RTAI task!\n");
  908.  
  909.         goto out_stop_task;
  910.  
  911.     }
  912.  
  913.  
  914.  
  915.     printk(KERN_INFO PFX "Initialized.\n");
  916.  
  917.     return 0;
  918.  
  919.  
  920.  
  921.  out_stop_task:
  922.  
  923.     rt_task_delete(&task);
  924.  
  925.  out_stop_timer:
  926.  
  927.     stop_rt_timer();
  928.  
  929.  out_release_master:
  930.  
  931.     printk(KERN_ERR PFX "Releasing master...\n");
  932.  
  933.     ecrt_release_master(master);
  934.  
  935.  out_return:
  936.  
  937.     rt_sem_delete(&master_sem);
  938.  
  939.     printk(KERN_ERR PFX "Failed to load. Aborting.\n");
  940.  
  941.     return ret;
  942.  
  943. }
  944.  
  945.  
  946.  
  947. /*****************************************************************************/
  948.  
  949.  
  950.  
  951. void __exit cleanup_mod(void)
  952.  
  953. {
  954.  
  955.     printk(KERN_INFO PFX "Stopping...\n");
  956.  
  957.  
  958.  
  959.     rt_task_delete(&task);
  960.  
  961.     stop_rt_timer();
  962.  
  963.     ecrt_release_master(master);
  964.  
  965.     rt_sem_delete(&master_sem);
  966.  
  967.  
  968.  
  969.     printk(KERN_INFO PFX "Unloading.\n");
  970.  
  971. }
  972.  
  973.  
  974.  
  975. /*****************************************************************************/
  976.  
  977.  
  978.  
  979. MODULE_LICENSE("GPL");
  980.  
  981. MODULE_AUTHOR("Florian Pose <fp@igh-essen.com>");
  982.  
  983. MODULE_DESCRIPTION("EtherCAT RTAI sample module");
  984.  
  985.  
  986.  
  987. module_init(init_mod);
  988.  
  989. module_exit(cleanup_mod);
  990.  
  991.  
  992.  
  993. /*****************************************************************************/
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!