master/fsm_pdo_config.c
changeset 854 f4f53be425ac
parent 835 23fd8b510803
child 858 69122084d066
equal deleted inserted replaced
853:726326d0aef4 854:f4f53be425ac
    63         ec_fsm_pdo_config_t *fsm, /**< pdo_config state machine */
    63         ec_fsm_pdo_config_t *fsm, /**< pdo_config state machine */
    64         ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */
    64         ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */
    65         )
    65         )
    66 {
    66 {
    67     fsm->fsm_coe = fsm_coe;
    67     fsm->fsm_coe = fsm_coe;
    68     fsm->sdodata.data = (uint8_t *) &fsm->sdo_value;
    68     ec_sdo_request_init(&fsm->request);
    69 }
    69 }
    70 
    70 
    71 /*****************************************************************************/
    71 /*****************************************************************************/
    72 
    72 
    73 /** Destructor.
    73 /** Destructor.
    74  */
    74  */
    75 void ec_fsm_pdo_config_clear(
    75 void ec_fsm_pdo_config_clear(
    76         ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */
    76         ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */
    77         )
    77         )
    78 {
    78 {
       
    79     ec_sdo_request_clear(&fsm->request);
    79 }
    80 }
    80 
    81 
    81 /*****************************************************************************/
    82 /*****************************************************************************/
    82 
    83 
    83 /** Start Pdo configuration state machine.
    84 /** Start Pdo configuration state machine.
   202     if (fsm->slave->master->debug_level) {
   203     if (fsm->slave->master->debug_level) {
   203         EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n",
   204         EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n",
   204                 fsm->pdo->index, fsm->slave->ring_position);
   205                 fsm->pdo->index, fsm->slave->ring_position);
   205     }
   206     }
   206 
   207 
       
   208     if (ec_sdo_request_alloc(&fsm->request, 4)) {
       
   209         fsm->state = ec_fsm_pdo_config_state_error;
       
   210         return;
       
   211     }
       
   212 
   207     // set mapped Pdo count to zero
   213     // set mapped Pdo count to zero
   208     fsm->sdodata.index = fsm->pdo->index;
   214     EC_WRITE_U8(&fsm->request.data, 0);
   209     fsm->sdodata.subindex = 0; // number of configured entries
   215     fsm->request.data_size = 1;
   210     EC_WRITE_U8(&fsm->sdo_value, 0);
   216     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
   211     fsm->sdodata.size = 1;
   217     ec_sdo_request_write(&fsm->request);
   212     if (fsm->slave->master->debug_level)
   218     if (fsm->slave->master->debug_level)
   213         EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n",
   219         EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n",
   214                 fsm->pdo->index);
   220                 fsm->pdo->index);
   215 
   221 
   216     fsm->state = ec_fsm_pdo_config_state_zero_count;
   222     fsm->state = ec_fsm_pdo_config_state_zero_count;
   217     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   223     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request);
   218     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   224     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   219 }
   225 }
   220 
   226 
   221 /*****************************************************************************/
   227 /*****************************************************************************/
   222 
   228 
   241         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */
   247         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */
   242         )
   248         )
   243 {
   249 {
   244     uint32_t value;
   250     uint32_t value;
   245 
   251 
   246     fsm->sdodata.subindex = fsm->entry_count;
       
   247     value = fsm->entry->index << 16
   252     value = fsm->entry->index << 16
   248         | fsm->entry->subindex << 8 | fsm->entry->bit_length;
   253         | fsm->entry->subindex << 8 | fsm->entry->bit_length;
   249     EC_WRITE_U32(&fsm->sdo_value, value);
   254     EC_WRITE_U32(&fsm->request.data, value);
   250     fsm->sdodata.size = 4;
   255     fsm->request.data_size = 4;
   251 
   256     ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count);
       
   257     ec_sdo_request_write(&fsm->request);
   252     if (fsm->slave->master->debug_level)
   258     if (fsm->slave->master->debug_level)
   253         EC_DBG("Configuring Pdo entry %08X at position %u.\n",
   259         EC_DBG("Configuring Pdo entry %08X at position %u.\n",
   254                 value, fsm->entry_count);
   260                 value, fsm->entry_count);
   255     
   261     
   256     fsm->state = ec_fsm_pdo_config_state_add_entry;
   262     fsm->state = ec_fsm_pdo_config_state_add_entry;
   257     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   263     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request);
   258     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   264     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   259 }
   265 }
   260 
   266 
   261 /*****************************************************************************/
   267 /*****************************************************************************/
   262 
   268 
   309     }
   315     }
   310 
   316 
   311     // find next entry
   317     // find next entry
   312     if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) {
   318     if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) {
   313         // No more entries to add. Write entry count.
   319         // No more entries to add. Write entry count.
   314         fsm->sdodata.subindex = 0;
   320         EC_WRITE_U8(&fsm->request.data, fsm->entry_count);
   315         EC_WRITE_U8(&fsm->sdo_value, fsm->entry_count);
   321         fsm->request.data_size = 1;
   316         fsm->sdodata.size = 1;
   322         ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);
   317 
   323         ec_sdo_request_write(&fsm->request);
   318         if (fsm->slave->master->debug_level)
   324         if (fsm->slave->master->debug_level)
   319             EC_DBG("Setting number of Pdo entries to %u.\n",
   325             EC_DBG("Setting number of Pdo entries to %u.\n",
   320                     fsm->entry_count);
   326                     fsm->entry_count);
   321         
   327         
   322         fsm->state = ec_fsm_pdo_config_state_entry_count;
   328         fsm->state = ec_fsm_pdo_config_state_entry_count;
   323         ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
   329         ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request);
   324         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   330         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   325         return;
   331         return;
   326     }
   332     }
   327 
   333 
   328     // add next entry
   334     // add next entry