rt/msr_module.c
changeset 51 b3beaa00640f
parent 48 7e75772ea28c
child 54 7506e67dd122
equal deleted inserted replaced
50:458e704afc6e 51:b3beaa00640f
    60 
    60 
    61 static EtherCAT_slave_t ecat_slaves[] =
    61 static EtherCAT_slave_t ecat_slaves[] =
    62 {
    62 {
    63     // Block 1
    63     // Block 1
    64     ECAT_INIT_SLAVE(Beckhoff_EK1100, 0),
    64     ECAT_INIT_SLAVE(Beckhoff_EK1100, 0),
    65     ECAT_INIT_SLAVE(Beckhoff_EL4102, 0),
    65     ECAT_INIT_SLAVE(Beckhoff_EL3102, 0)
    66     ECAT_INIT_SLAVE(Beckhoff_EL1014, 0),
       
    67     ECAT_INIT_SLAVE(Beckhoff_EL3162, 0),
       
    68     ECAT_INIT_SLAVE(Beckhoff_EL2004, 0),
       
    69     ECAT_INIT_SLAVE(Beckhoff_EL3102, 0),
       
    70     ECAT_INIT_SLAVE(Beckhoff_EL2004, 0),
       
    71 
       
    72     // Block 2
       
    73     ECAT_INIT_SLAVE(Beckhoff_EK1100, 1),
       
    74     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    75     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    76     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    77     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    78     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    79     ECAT_INIT_SLAVE(Beckhoff_EL2004, 1),
       
    80     ECAT_INIT_SLAVE(Beckhoff_EL2004, 1),
       
    81     ECAT_INIT_SLAVE(Beckhoff_EL2004, 1),
       
    82     ECAT_INIT_SLAVE(Beckhoff_EL2004, 1),
       
    83     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    84     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1),
       
    85     ECAT_INIT_SLAVE(Beckhoff_EL1014, 1)
       
    86 };
    66 };
    87 
    67 
    88 #define ECAT_SLAVES_COUNT (sizeof(ecat_slaves) / sizeof(EtherCAT_slave_t))
    68 #define ECAT_SLAVES_COUNT (sizeof(ecat_slaves) / sizeof(EtherCAT_slave_t))
    89 
    69 
    90 #define USE_MSR_LIB
    70 #define USE_MSR_LIB
    94 int dig1;
    74 int dig1;
    95 #endif
    75 #endif
    96 
    76 
    97 /******************************************************************************
    77 /******************************************************************************
    98  *
    78  *
    99  * Function: next2004
       
   100  *
       
   101  *****************************************************************************/
       
   102 
       
   103 static int next2004(int *wrap)
       
   104 {
       
   105     static int i = 0;
       
   106     unsigned int j = 0;
       
   107 
       
   108     *wrap = 0;
       
   109 
       
   110     for (j = 0; j < ECAT_SLAVES_COUNT; j++)
       
   111     {
       
   112         i++;
       
   113 
       
   114         i %= ECAT_SLAVES_COUNT;
       
   115 
       
   116         if (i == 0) *wrap = 1;
       
   117 
       
   118         if (ecat_slaves[i].desc == Beckhoff_EL2004)
       
   119         {
       
   120             return i;
       
   121         }
       
   122     }
       
   123 
       
   124     return -1;
       
   125 }
       
   126 
       
   127 /******************************************************************************
       
   128  *
       
   129  * Function: msr_controller_run()
    79  * Function: msr_controller_run()
   130  *
    80  *
   131  *****************************************************************************/
    81  *****************************************************************************/
   132 
    82 
   133 static void msr_controller_run(void)
    83 static void msr_controller_run(void)
   134 {
    84 {
   135     static int ms = 0;
       
   136     static int cnt = 0;
       
   137     static int firstrun = 1;
       
   138     static unsigned int debug_counter = 0;
    85     static unsigned int debug_counter = 0;
   139 
       
   140     static int klemme = 0;
       
   141     static int kanal = 0;
       
   142     static int up_down = 0;
       
   143     int wrap = 0;
       
   144 
       
   145     unsigned long t_bus_start, t_bus_end;
       
   146 
    86 
   147     // Prozessdaten lesen
    87     // Prozessdaten lesen
   148     msr_jitter_run(MSR_ABTASTFREQUENZ);
    88     msr_jitter_run(MSR_ABTASTFREQUENZ);
   149 
    89 
   150     if (firstrun) {
    90     if (debug_counter == 0) {
   151         klemme = next2004(&wrap);
    91         ecat_master->debug_level = 2;
   152         firstrun = 0;
    92     }
   153     }
    93 
   154 
    94     // Prozessdaten schreiben
   155     ms++;
    95 
   156     ms %= 1000;
    96     if (EtherCAT_process_data_cycle(ecat_master, 0, 40) < 0)
   157     if (cnt++ > 200)
    97         ecat_timeouts++;
   158     {
       
   159         cnt = 0;
       
   160 
       
   161         if (++kanal > 3)
       
   162         {
       
   163             kanal = 0;
       
   164             klemme = next2004(&wrap);
       
   165 
       
   166             if (wrap == 1)
       
   167             {
       
   168                 if (up_down == 1) up_down = 0;
       
   169                 else up_down = 1;
       
   170             }
       
   171         }
       
   172     }
       
   173 
       
   174     if (klemme >= 0) {
       
   175         EtherCAT_write_value(&ecat_slaves[klemme], kanal, up_down);
       
   176     }
       
   177 
    98 
   178     if (debug_counter == 0) {
    99     if (debug_counter == 0) {
   179         ecat_master->debug_level = 0;
   100         ecat_master->debug_level = 0;
   180     }
   101     }
   181 
   102 
   182     // Prozessdaten schreiben
   103     value = EtherCAT_read_value(&ecat_slaves[1], 0);
   183 
       
   184     rdtscl(t_bus_start);
       
   185 
       
   186     if (EtherCAT_process_data_cycle(ecat_master, 0, 40) < 0)
       
   187         ecat_timeouts++;
       
   188 
       
   189     if (EtherCAT_process_data_cycle(ecat_master, 1, 40) < 0)
       
   190         ecat_timeouts++;
       
   191 
       
   192     rdtscl(t_bus_end);
       
   193     ecat_bus_time = TSC2US(t_bus_start, t_bus_end);
       
   194 
       
   195     if (debug_counter == 0) {
       
   196         ecat_master->debug_level = 0;
       
   197     }
       
   198 
       
   199     // Daten lesen und skalieren
       
   200 #ifdef USE_MSR_LIB
       
   201     value = EtherCAT_read_value(&ecat_slaves[5], 0) / 3276.0;
       
   202     dig1 = EtherCAT_read_value(&ecat_slaves[2], 0);
       
   203 #endif
       
   204 
   104 
   205     debug_counter++;
   105     debug_counter++;
   206     if (debug_counter >= MSR_ABTASTFREQUENZ * 5) debug_counter = 0;
   106     if (debug_counter >= MSR_ABTASTFREQUENZ * 5) debug_counter = 0;
   207 }
   107 }
   208 
   108