master/fsm_mapping.c
changeset 801 6b1e2b0ed8bc
parent 792 3778920f61e4
equal deleted inserted replaced
800:e0b8cbc8a88d 801:6b1e2b0ed8bc
   148     if (!fsm->slave->config) {
   148     if (!fsm->slave->config) {
   149         fsm->state = ec_fsm_mapping_state_end;
   149         fsm->state = ec_fsm_mapping_state_end;
   150         return;
   150         return;
   151     }
   151     }
   152 
   152 
   153     fsm->dir = EC_DIR_OUTPUT;
   153     fsm->sync = NULL;
   154     ec_fsm_mapping_next_sync(fsm);
   154     ec_fsm_mapping_next_sync(fsm);
   155 }
   155 }
   156 
   156 
   157 /*****************************************************************************/
   157 /*****************************************************************************/
   158 
   158 
   160  */
   160  */
   161 void ec_fsm_mapping_next_sync(
   161 void ec_fsm_mapping_next_sync(
   162         ec_fsm_mapping_t *fsm /**< mapping state machine */
   162         ec_fsm_mapping_t *fsm /**< mapping state machine */
   163         )
   163         )
   164 {
   164 {
   165     while (1) {
   165     ec_direction_t dir;
   166         if (fsm->dir > EC_DIR_INPUT) {
   166     const ec_sync_t *sync;
   167             // no more directions to configure mappings for
   167     const ec_pdo_mapping_t *map;
   168             fsm->state = ec_fsm_mapping_state_end;
   168 
   169             return;
   169     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) {
   170         }
   170         if (!(sync = ec_slave_get_pdo_sync(fsm->slave, dir))) {
   171 
   171             EC_WARN("No sync manager for direction %u!\n", dir);
   172         if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) {
       
   173             EC_WARN("next_sync(): No sync manager!\n");
       
   174             fsm->dir++;
       
   175             continue;
   172             continue;
   176         }
   173         }
   177 
   174 
   178         fsm->mapping = &fsm->slave->config->mapping[fsm->dir];
   175         if (fsm->sync) { // there is a last SM
   179 
   176             if (sync == fsm->sync) // this is the last SM
   180         if (ec_pdo_mapping_equal(&fsm->sync->mapping, fsm->mapping)) {
   177                 fsm->sync = NULL; // take the next one
   181             // the mapping for this direction does not have to be altered
       
   182             fsm->dir++;
       
   183             continue;
       
   184         } else {
   178         } else {
   185             fsm->dir++;
   179             map = &fsm->slave->config->mapping[dir];
       
   180             if (ec_pdo_mapping_equal(&sync->mapping, map))
       
   181                 continue;
       
   182 
       
   183             fsm->sync = sync;
       
   184             fsm->mapping = map;
   186             break;
   185             break;
   187         }
   186         }
       
   187     }
       
   188 
       
   189     if (!sync) {
       
   190         if (fsm->slave->master->debug_level)
       
   191             EC_DBG("Pdo mapping finished for slave %u.\n",
       
   192                     fsm->slave->ring_position);
       
   193         fsm->state = ec_fsm_mapping_state_end;
       
   194         return;
   188     }
   195     }
   189 
   196 
   190     if (fsm->slave->master->debug_level) {
   197     if (fsm->slave->master->debug_level) {
   191         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",
   192                 fsm->sync->index, fsm->slave->ring_position);
   199                 fsm->sync->index, fsm->slave->ring_position);
   216 {
   223 {
   217     list = list->next; 
   224     list = list->next; 
   218     if (list == &fsm->mapping->pdos)
   225     if (list == &fsm->mapping->pdos)
   219         return NULL; // no next PDO
   226         return NULL; // no next PDO
   220     return list_entry(list, ec_pdo_t, list);
   227     return list_entry(list, ec_pdo_t, list);
       
   228 }
       
   229 
       
   230 /*****************************************************************************/
       
   231 
       
   232 /** Add a Pdo to the mapping.
       
   233  */
       
   234 void ec_fsm_mapping_add_pdo(
       
   235         ec_fsm_mapping_t *fsm /**< mapping state machine */
       
   236         )
       
   237 {
       
   238     fsm->sdodata.subindex = fsm->pdo_count;
       
   239     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
       
   240     fsm->sdodata.size = 2;
       
   241 
       
   242     if (fsm->slave->master->debug_level)
       
   243         EC_DBG("Mapping PDO 0x%04X at position %u.\n",
       
   244                 fsm->pdo->index, fsm->sdodata.subindex);
       
   245     
       
   246     fsm->state = ec_fsm_mapping_state_add_pdo;
       
   247     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
       
   248     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
   221 }
   249 }
   222 
   250 
   223 /*****************************************************************************/
   251 /*****************************************************************************/
   224 
   252 
   225 /** Set the number of mapped PDOs to zero.
   253 /** Set the number of mapped PDOs to zero.
   248         return;
   276         return;
   249     }
   277     }
   250 
   278 
   251     // add first PDO to mapping
   279     // add first PDO to mapping
   252     fsm->pdo_count = 1;
   280     fsm->pdo_count = 1;
   253     fsm->sdodata.subindex = fsm->pdo_count;
   281     ec_fsm_mapping_add_pdo(fsm);
   254     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
       
   255     fsm->sdodata.size = 2;
       
   256 
       
   257     if (fsm->slave->master->debug_level)
       
   258         EC_DBG("Mapping PDO 0x%04X at position %u.\n",
       
   259                 fsm->pdo->index, fsm->sdodata.subindex);
       
   260     
       
   261     fsm->state = ec_fsm_mapping_state_add_pdo;
       
   262     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
       
   263     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
       
   264 }
   282 }
   265 
   283 
   266 /*****************************************************************************/
   284 /*****************************************************************************/
   267 
   285 
   268 /** Add a PDO to the sync managers mapping.
   286 /** Add a PDO to the sync managers mapping.
   297         return;
   315         return;
   298     }
   316     }
   299 
   317 
   300     // add next PDO to mapping
   318     // add next PDO to mapping
   301     fsm->pdo_count++;
   319     fsm->pdo_count++;
   302     fsm->sdodata.subindex = fsm->pdo_count;
   320     ec_fsm_mapping_add_pdo(fsm);
   303     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);
       
   304     fsm->sdodata.size = 2;
       
   305 
       
   306     if (fsm->slave->master->debug_level)
       
   307         EC_DBG("Mapping PDO 0x%04X at position %u.\n",
       
   308                 fsm->pdo->index, fsm->sdodata.subindex);
       
   309 
       
   310     ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata);
       
   311     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
       
   312 }
   321 }
   313 
   322 
   314 /*****************************************************************************/
   323 /*****************************************************************************/
   315 
   324 
   316 /** Set the number of mapped PDOs.
   325 /** Set the number of mapped PDOs.