master/fsm_pdo_mapping.c
changeset 814 a51f857b1b2d
parent 808 1feddbd65608
child 821 c6536d28db56
equal deleted inserted replaced
813:bfc3f1ab52de 814:a51f857b1b2d
    30  *  Technology, IP and trade marks.
    30  *  Technology, IP and trade marks.
    31  *
    31  *
    32  *****************************************************************************/
    32  *****************************************************************************/
    33 
    33 
    34 /** \file
    34 /** \file
    35  * EtherCAT PDO mapping state machine.
    35  * EtherCAT Pdo mapping state machine.
    36  */
    36  */
    37 
    37 
    38 /*****************************************************************************/
    38 /*****************************************************************************/
    39 
    39 
    40 #include "globals.h"
    40 #include "globals.h"
    78 {
    78 {
    79 }
    79 }
    80 
    80 
    81 /*****************************************************************************/
    81 /*****************************************************************************/
    82 
    82 
    83 /** Start PDO mapping configuration state machine.
    83 /** Start Pdo mapping configuration state machine.
    84  */
    84  */
    85 void ec_fsm_pdo_mapping_start(
    85 void ec_fsm_pdo_mapping_start(
    86         ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */
    86         ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */
    87         ec_slave_t *slave /**< slave to configure */
    87         ec_slave_t *slave /**< slave to configure */
    88         )
    88         )
   193         fsm->state = ec_fsm_pdo_mapping_state_end;
   193         fsm->state = ec_fsm_pdo_mapping_state_end;
   194         return;
   194         return;
   195     }
   195     }
   196 
   196 
   197     if (fsm->slave->master->debug_level) {
   197     if (fsm->slave->master->debug_level) {
   198         EC_DBG("Configuring PDO mapping for SM%u of slave %u.\n",
   198         EC_DBG("Configuring Pdo mapping for SM%u of slave %u.\n",
   199                 fsm->sync->index, fsm->slave->ring_position);
   199                 fsm->sync->index, fsm->slave->ring_position);
   200     }
   200     }
   201 
   201 
   202     // set mapped PDO count to zero
   202     // set mapped Pdo count to zero
   203     fsm->sdodata.index = 0x1C10 + fsm->sync->index;
   203     fsm->sdodata.index = 0x1C10 + fsm->sync->index;
   204     fsm->sdodata.subindex = 0; // mapped PDO count
   204     fsm->sdodata.subindex = 0; // mapped Pdo count
   205     EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped
   205     EC_WRITE_U8(&fsm->sdo_value, 0); // zero Pdos mapped
   206     fsm->sdodata.size = 1;
   206     fsm->sdodata.size = 1;
   207     if (fsm->slave->master->debug_level)
   207     if (fsm->slave->master->debug_level)
   208         EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index);
   208         EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index);
   209 
   209 
   210     fsm->state = ec_fsm_pdo_mapping_state_zero_count;
   210     fsm->state = ec_fsm_pdo_mapping_state_zero_count;
   211     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   211     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   212     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   212     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   213 }
   213 }
   214 
   214 
   215 /*****************************************************************************/
   215 /*****************************************************************************/
   216 
   216 
   217 /** Process mapping of next PDO.
   217 /** Process mapping of next Pdo.
   218  */
   218  */
   219 ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(
   219 ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(
   220         const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */
   220         const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */
   221         const struct list_head *list /**< current PDO list item */
   221         const struct list_head *list /**< current Pdo list item */
   222         )
   222         )
   223 {
   223 {
   224     list = list->next; 
   224     list = list->next; 
   225     if (list == &fsm->mapping->pdos)
   225     if (list == &fsm->mapping->pdos)
   226         return NULL; // no next PDO
   226         return NULL; // no next Pdo
   227     return list_entry(list, ec_pdo_t, list);
   227     return list_entry(list, ec_pdo_t, list);
   228 }
   228 }
   229 
   229 
   230 /*****************************************************************************/
   230 /*****************************************************************************/
   231 
   231 
   238     fsm->sdodata.subindex = fsm->pdo_count;
   238     fsm->sdodata.subindex = fsm->pdo_count;
   239     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
   239     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
   240     fsm->sdodata.size = 2;
   240     fsm->sdodata.size = 2;
   241 
   241 
   242     if (fsm->slave->master->debug_level)
   242     if (fsm->slave->master->debug_level)
   243         EC_DBG("Mapping PDO 0x%04X at position %u.\n",
   243         EC_DBG("Mapping Pdo 0x%04X at position %u.\n",
   244                 fsm->pdo->index, fsm->sdodata.subindex);
   244                 fsm->pdo->index, fsm->sdodata.subindex);
   245     
   245     
   246     fsm->state = ec_fsm_pdo_mapping_state_add_pdo;
   246     fsm->state = ec_fsm_pdo_mapping_state_add_pdo;
   247     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   247     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   248     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   248     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   249 }
   249 }
   250 
   250 
   251 /*****************************************************************************/
   251 /*****************************************************************************/
   252 
   252 
   253 /** Set the number of mapped PDOs to zero.
   253 /** Set the number of mapped Pdos to zero.
   254  */
   254  */
   255 void ec_fsm_pdo_mapping_state_zero_count(
   255 void ec_fsm_pdo_mapping_state_zero_count(
   256         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   256         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   257         )
   257         )
   258 {
   258 {
   259     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   259     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   260 
   260 
   261     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   261     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   262         EC_ERR("Failed to clear PDO mapping for slave %u.\n",
   262         EC_ERR("Failed to clear Pdo mapping for slave %u.\n",
   263                 fsm->slave->ring_position);
   263                 fsm->slave->ring_position);
   264         fsm->state = ec_fsm_pdo_mapping_state_error;
   264         fsm->state = ec_fsm_pdo_mapping_state_error;
   265         return;
   265         return;
   266     }
   266     }
   267 
   267 
   268     // map all PDOs belonging to the current sync manager
   268     // map all Pdos belonging to the current sync manager
   269     
   269     
   270     // find first PDO
   270     // find first Pdo
   271     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) {
   271     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) {
   272         if (fsm->slave->master->debug_level)
   272         if (fsm->slave->master->debug_level)
   273             EC_DBG("No PDOs to map for SM%u of slave %u.\n",
   273             EC_DBG("No Pdos to map for SM%u of slave %u.\n",
   274                     fsm->sync->index, fsm->slave->ring_position);
   274                     fsm->sync->index, fsm->slave->ring_position);
   275         ec_fsm_pdo_mapping_next_sync(fsm);
   275         ec_fsm_pdo_mapping_next_sync(fsm);
   276         return;
   276         return;
   277     }
   277     }
   278 
   278 
   279     // add first PDO to mapping
   279     // add first Pdo to mapping
   280     fsm->pdo_count = 1;
   280     fsm->pdo_count = 1;
   281     ec_fsm_pdo_mapping_add_pdo(fsm);
   281     ec_fsm_pdo_mapping_add_pdo(fsm);
   282 }
   282 }
   283 
   283 
   284 /*****************************************************************************/
   284 /*****************************************************************************/
   285 
   285 
   286 /** Add a PDO to the sync managers mapping.
   286 /** Add a Pdo to the sync managers mapping.
   287  */
   287  */
   288 void ec_fsm_pdo_mapping_state_add_pdo(
   288 void ec_fsm_pdo_mapping_state_add_pdo(
   289         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   289         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   290         )
   290         )
   291 {
   291 {
   292     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   292     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   293 
   293 
   294     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   294     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   295         EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n",
   295         EC_ERR("Failed to map Pdo 0x%04X for SM%u of slave %u.\n",
   296                 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position);
   296                 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position);
   297         fsm->state = ec_fsm_pdo_mapping_state_error;
   297         fsm->state = ec_fsm_pdo_mapping_state_error;
   298         return;
   298         return;
   299     }
   299     }
   300 
   300 
   301     // find next PDO
   301     // find next Pdo
   302     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) {
   302     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) {
   303         // no more PDOs to map. write PDO count
   303         // no more Pdos to map. write Pdo count
   304         fsm->sdodata.subindex = 0;
   304         fsm->sdodata.subindex = 0;
   305         EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count);
   305         EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count);
   306         fsm->sdodata.size = 1;
   306         fsm->sdodata.size = 1;
   307 
   307 
   308         if (fsm->slave->master->debug_level)
   308         if (fsm->slave->master->debug_level)
   309             EC_DBG("Setting number of mapped PDOs to %u.\n",
   309             EC_DBG("Setting number of mapped Pdos to %u.\n",
   310                     fsm->pdo_count);
   310                     fsm->pdo_count);
   311         
   311         
   312         fsm->state = ec_fsm_pdo_mapping_state_pdo_count;
   312         fsm->state = ec_fsm_pdo_mapping_state_pdo_count;
   313         ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   313         ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   314         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   314         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   315         return;
   315         return;
   316     }
   316     }
   317 
   317 
   318     // add next PDO to mapping
   318     // add next Pdo to mapping
   319     fsm->pdo_count++;
   319     fsm->pdo_count++;
   320     ec_fsm_pdo_mapping_add_pdo(fsm);
   320     ec_fsm_pdo_mapping_add_pdo(fsm);
   321 }
   321 }
   322 
   322 
   323 /*****************************************************************************/
   323 /*****************************************************************************/
   324 
   324 
   325 /** Set the number of mapped PDOs.
   325 /** Set the number of mapped Pdos.
   326  */
   326  */
   327 void ec_fsm_pdo_mapping_state_pdo_count(
   327 void ec_fsm_pdo_mapping_state_pdo_count(
   328         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   328         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */
   329         )
   329         )
   330 {
   330 {
   331     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   331     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   332 
   332 
   333     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   333     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   334         EC_ERR("Failed to set number of mapped PDOs for slave %u.\n",
   334         EC_ERR("Failed to set number of mapped Pdos for slave %u.\n",
   335                 fsm->slave->ring_position);
   335                 fsm->slave->ring_position);
   336         fsm->state = ec_fsm_pdo_mapping_state_error;
   336         fsm->state = ec_fsm_pdo_mapping_state_error;
   337         return;
   337         return;
   338     }
   338     }
   339 
   339 
   340     if (fsm->slave->master->debug_level)
   340     if (fsm->slave->master->debug_level)
   341         EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n",
   341         EC_DBG("Successfully set Pdo mapping for SM%u of slave %u.\n",
   342                 fsm->sync->index, fsm->slave->ring_position);
   342                 fsm->sync->index, fsm->slave->ring_position);
   343 
   343 
   344     // mapping configuration for this sync manager complete.
   344     // mapping configuration for this sync manager complete.
   345     ec_fsm_pdo_mapping_next_sync(fsm);
   345     ec_fsm_pdo_mapping_next_sync(fsm);
   346 }
   346 }