master/fsm_coe_map.c
changeset 1035 7fafc1a7451e
parent 1034 36cd81362f10
child 1036 dc2008bc8113
equal deleted inserted replaced
1034:36cd81362f10 1035:7fafc1a7451e
    42 #include "master.h"
    42 #include "master.h"
    43 #include "mailbox.h"
    43 #include "mailbox.h"
    44 #include "sdo_request.h"
    44 #include "sdo_request.h"
    45 #include "fsm_coe_map.h"
    45 #include "fsm_coe_map.h"
    46 
    46 
    47 /** Retry reading the number of assigned Pdos if this time is not elapsed.
       
    48  */
       
    49 #define EC_FSM_COE_MAP_TIMEOUT 5
       
    50 
       
    51 /*****************************************************************************/
    47 /*****************************************************************************/
    52 
    48 
    53 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);
    49 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);
    54 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);
    50 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);
    55 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);
    51 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);
   184                     fsm->sync->index, slave->ring_position);
   180                     fsm->sync->index, slave->ring_position);
   185 
   181 
   186         ec_pdo_list_clear_pdos(&fsm->pdos);
   182         ec_pdo_list_clear_pdos(&fsm->pdos);
   187 
   183 
   188         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);
   184         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);
       
   185         fsm->request.response_timeout = 10000;
   189         ecrt_sdo_request_read(&fsm->request);
   186         ecrt_sdo_request_read(&fsm->request);
   190         fsm->jiffies_start = jiffies;
       
   191         fsm->state = ec_fsm_coe_map_state_pdo_count;
   187         fsm->state = ec_fsm_coe_map_state_pdo_count;
   192         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   188         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   193         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   189         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   194         return;
   190         return;
   195     }
   191     }
   212         )
   208         )
   213 {
   209 {
   214     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   210     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   215 
   211 
   216     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   212     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   217         if (jiffies - fsm->jiffies_start < HZ * EC_FSM_COE_MAP_TIMEOUT) {
       
   218             if (fsm->slave->master->debug_level)
       
   219                 EC_DBG("Failed to read number of assigned Pdos from "
       
   220                         "slave %u. Retrying...\n", fsm->slave->ring_position);
       
   221             ecrt_sdo_request_read(&fsm->request);
       
   222             ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
       
   223             ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
       
   224             return;
       
   225         }
       
   226         EC_ERR("Failed to read number of assigned Pdos from slave %u.\n",
   213         EC_ERR("Failed to read number of assigned Pdos from slave %u.\n",
   227                 fsm->slave->ring_position);
   214                 fsm->slave->ring_position);
   228         fsm->state = ec_fsm_coe_map_state_error;
   215         fsm->state = ec_fsm_coe_map_state_error;
   229         return;
   216         return;
   230     }
   217     }
   250         )
   237         )
   251 {
   238 {
   252     if (fsm->sync_subindex <= fsm->sync_subindices) {
   239     if (fsm->sync_subindex <= fsm->sync_subindices) {
   253         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,
   240         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,
   254                 fsm->sync_subindex);
   241                 fsm->sync_subindex);
       
   242         fsm->request.response_timeout = 0;
   255         ecrt_sdo_request_read(&fsm->request);
   243         ecrt_sdo_request_read(&fsm->request);
   256         fsm->state = ec_fsm_coe_map_state_pdo;
   244         fsm->state = ec_fsm_coe_map_state_pdo;
   257         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   245         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   258         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   246         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   259         return;
   247         return;
   307         EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
   295         EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
   308 
   296 
   309     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
   297     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
   310 
   298 
   311     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
   299     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
       
   300     fsm->request.response_timeout = 0;
   312     ecrt_sdo_request_read(&fsm->request);
   301     ecrt_sdo_request_read(&fsm->request);
   313     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
   302     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
   314     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   303     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   315     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   304     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   316 }
   305 }
   354         ec_fsm_coe_map_t *fsm /**< finite state machine */
   343         ec_fsm_coe_map_t *fsm /**< finite state machine */
   355         )
   344         )
   356 {
   345 {
   357     if (fsm->pdo_subindex <= fsm->pdo_subindices) {
   346     if (fsm->pdo_subindex <= fsm->pdo_subindices) {
   358         ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);
   347         ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);
       
   348         fsm->request.response_timeout = 0;
   359         ecrt_sdo_request_read(&fsm->request);
   349         ecrt_sdo_request_read(&fsm->request);
   360         fsm->state = ec_fsm_coe_map_state_pdo_entry;
   350         fsm->state = ec_fsm_coe_map_state_pdo_entry;
   361         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   351         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   362         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   352         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   363         return;
   353         return;