master/fsm_coe_map.c
changeset 879 9b395c5646ab
parent 859 233e32f428e1
child 903 d13004440b6d
equal deleted inserted replaced
878:40c379697ebf 879:9b395c5646ab
    31  *
    31  *
    32  *****************************************************************************/
    32  *****************************************************************************/
    33 
    33 
    34 /**
    34 /**
    35    \file
    35    \file
    36    EtherCAT CoE mapping state machines.
    36    EtherCAT Pdo information state machines.
    37 */
    37 */
    38 
    38 
    39 /*****************************************************************************/
    39 /*****************************************************************************/
    40 
    40 
    41 #include "globals.h"
    41 #include "globals.h"
    71         )
    71         )
    72 {
    72 {
    73     fsm->fsm_coe = fsm_coe;
    73     fsm->fsm_coe = fsm_coe;
    74     fsm->state = NULL;
    74     fsm->state = NULL;
    75     ec_sdo_request_init(&fsm->request);
    75     ec_sdo_request_init(&fsm->request);
    76     ec_pdo_mapping_init(&fsm->mapping);
    76     ec_pdo_list_init(&fsm->pdos);
    77 }
    77 }
    78 
    78 
    79 /*****************************************************************************/
    79 /*****************************************************************************/
    80 
    80 
    81 /**
    81 /**
    83 */
    83 */
    84 
    84 
    85 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)
    85 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)
    86 {
    86 {
    87     ec_sdo_request_clear(&fsm->request);
    87     ec_sdo_request_clear(&fsm->request);
    88     ec_pdo_mapping_clear(&fsm->mapping);
    88     ec_pdo_list_clear(&fsm->pdos);
    89 }
    89 }
    90 
    90 
    91 /*****************************************************************************/
    91 /*****************************************************************************/
    92 
    92 
    93 /**
    93 /**
   133 /******************************************************************************
   133 /******************************************************************************
   134  *  state functions
   134  *  state functions
   135  *****************************************************************************/
   135  *****************************************************************************/
   136 
   136 
   137 /**
   137 /**
   138  * Start reading mapping.
   138  * Start reading Pdo assignment.
   139  */
   139  */
   140 
   140 
   141 void ec_fsm_coe_map_state_start(
   141 void ec_fsm_coe_map_state_start(
   142         ec_fsm_coe_map_t *fsm /**< finite state machine */
   142         ec_fsm_coe_map_t *fsm /**< finite state machine */
   143         )
   143         )
   144 {
   144 {
   145     // read mapping for first direction
   145     // read Pdo assignment for first direction
   146     fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT
   146     fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT
   147     ec_fsm_coe_map_action_next_dir(fsm);
   147     ec_fsm_coe_map_action_next_dir(fsm);
   148 }
   148 }
   149 
   149 
   150 /*****************************************************************************/
   150 /*****************************************************************************/
   151 
   151 
   152 /**
   152 /**
   153  * Read mapping of next direction manager.
   153  * Read Pdo assignment of next direction manager.
   154  */
   154  */
   155 
   155 
   156 void ec_fsm_coe_map_action_next_dir(
   156 void ec_fsm_coe_map_action_next_dir(
   157         ec_fsm_coe_map_t *fsm /**< finite state machine */
   157         ec_fsm_coe_map_t *fsm /**< finite state machine */
   158         )
   158         )
   174         }
   174         }
   175 
   175 
   176         fsm->sync_sdo_index = 0x1C10 + fsm->sync->index;
   176         fsm->sync_sdo_index = 0x1C10 + fsm->sync->index;
   177 
   177 
   178         if (slave->master->debug_level)
   178         if (slave->master->debug_level)
   179             EC_DBG("Reading Pdo mapping of sync manager %u of slave %u.\n",
   179             EC_DBG("Reading Pdo assignment of sync manager %u of slave %u.\n",
   180                     fsm->sync->index, slave->ring_position);
   180                     fsm->sync->index, slave->ring_position);
   181 
   181 
   182         ec_pdo_mapping_clear_pdos(&fsm->mapping);
   182         ec_pdo_list_clear_pdos(&fsm->pdos);
   183 
   183 
   184         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);
   184         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);
   185         ecrt_sdo_request_read(&fsm->request);
   185         ecrt_sdo_request_read(&fsm->request);
   186         fsm->state = ec_fsm_coe_map_state_pdo_count;
   186         fsm->state = ec_fsm_coe_map_state_pdo_count;
   187         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   187         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   188         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   188         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   189         return;
   189         return;
   190     }
   190     }
   191 
   191 
   192     if (slave->master->debug_level)
   192     if (slave->master->debug_level)
   193         EC_DBG("Reading of Pdo mapping finished for slave %u.\n",
   193         EC_DBG("Reading of Pdo assignment finished for slave %u.\n",
   194                 slave->ring_position);
   194                 slave->ring_position);
   195 
   195 
   196     fsm->state = ec_fsm_coe_map_state_end;
   196     fsm->state = ec_fsm_coe_map_state_end;
   197 }
   197 }
   198 
   198 
   199 /*****************************************************************************/
   199 /*****************************************************************************/
   200 
   200 
   201 /**
   201 /**
   202  * Count mapped Pdos.
   202  * Count assigned Pdos.
   203  */
   203  */
   204 
   204 
   205 void ec_fsm_coe_map_state_pdo_count(
   205 void ec_fsm_coe_map_state_pdo_count(
   206         ec_fsm_coe_map_t *fsm /**< finite state machine */
   206         ec_fsm_coe_map_t *fsm /**< finite state machine */
   207         )
   207         )
   216     }
   216     }
   217 
   217 
   218     fsm->sync_subindices = EC_READ_U8(fsm->request.data);
   218     fsm->sync_subindices = EC_READ_U8(fsm->request.data);
   219 
   219 
   220     if (fsm->slave->master->debug_level)
   220     if (fsm->slave->master->debug_level)
   221         EC_DBG("  %u Pdos mapped.\n", fsm->sync_subindices);
   221         EC_DBG("  %u Pdos assigned.\n", fsm->sync_subindices);
   222 
   222 
   223     // read first Pdo
   223     // read first Pdo
   224     fsm->sync_subindex = 1;
   224     fsm->sync_subindex = 1;
   225     ec_fsm_coe_map_action_next_pdo(fsm);
   225     ec_fsm_coe_map_action_next_pdo(fsm);
   226 }
   226 }
   243         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   243         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   244         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   244         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   245         return;
   245         return;
   246     }
   246     }
   247 
   247 
   248     // finished reading Pdo mapping/configuration
   248     // finished reading Pdo assignment/mapping
   249     
   249     
   250     if (ec_pdo_mapping_copy(&fsm->sync->mapping, &fsm->mapping)) {
   250     if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {
   251         fsm->state = ec_fsm_coe_map_state_error;
   251         fsm->state = ec_fsm_coe_map_state_error;
   252         return;
   252         return;
   253     }
   253     }
   254 
   254 
   255     fsm->sync->mapping_source = EC_SYNC_MAPPING_COE;
   255     fsm->sync->assign_source = EC_ASSIGN_COE;
   256     ec_pdo_mapping_clear_pdos(&fsm->mapping);
   256     ec_pdo_list_clear_pdos(&fsm->pdos);
   257 
   257 
   258     // next direction
   258     // next direction
   259     ec_fsm_coe_map_action_next_dir(fsm);
   259     ec_fsm_coe_map_action_next_dir(fsm);
   260 }
   260 }
   261 
   261 
   270         )
   270         )
   271 {
   271 {
   272     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   272     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   273 
   273 
   274     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   274     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   275         EC_ERR("Failed to read mapped Pdo index from slave %u.\n",
   275         EC_ERR("Failed to read assigned Pdo index from slave %u.\n",
   276                 fsm->slave->ring_position);
   276                 fsm->slave->ring_position);
   277         fsm->state = ec_fsm_coe_map_state_error;
   277         fsm->state = ec_fsm_coe_map_state_error;
   278         return;
   278         return;
   279     }
   279     }
   280 
   280 
   290     fsm->pdo->dir = ec_sync_direction(fsm->sync);
   290     fsm->pdo->dir = ec_sync_direction(fsm->sync);
   291 
   291 
   292     if (fsm->slave->master->debug_level)
   292     if (fsm->slave->master->debug_level)
   293         EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
   293         EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
   294 
   294 
   295     list_add_tail(&fsm->pdo->list, &fsm->mapping.pdos);
   295     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
   296 
   296 
   297     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
   297     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
   298     ecrt_sdo_request_read(&fsm->request);
   298     ecrt_sdo_request_read(&fsm->request);
   299     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
   299     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
   300     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   300     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
   302 }
   302 }
   303 
   303 
   304 /*****************************************************************************/
   304 /*****************************************************************************/
   305 
   305 
   306 /**
   306 /**
   307  * Read number of Pdo entries.
   307  * Read number of mapped Pdo entries.
   308  */
   308  */
   309 
   309 
   310 void ec_fsm_coe_map_state_pdo_entry_count(
   310 void ec_fsm_coe_map_state_pdo_entry_count(
   311         ec_fsm_coe_map_t *fsm /**< finite state machine */
   311         ec_fsm_coe_map_t *fsm /**< finite state machine */
   312         )
   312         )
   365         )
   365         )
   366 {
   366 {
   367     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   367     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   368 
   368 
   369     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   369     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   370         EC_ERR("Failed to read index of mapped Pdo entry from slave %u.\n",
   370         EC_ERR("Failed to read mapped Pdo entry from slave %u.\n",
   371                 fsm->slave->ring_position);
   371                 fsm->slave->ring_position);
   372         fsm->state = ec_fsm_coe_map_state_error;
   372         fsm->state = ec_fsm_coe_map_state_error;
   373         return;
   373         return;
   374     }
   374     }
   375 
   375