master/fsm_pdo_entry.c
changeset 1823 2bf892fe8080
parent 1543 373cd456dc26
child 1921 d9cf40facbc4
equal deleted inserted replaced
1822:74e6ac2369f4 1823:2bf892fe8080
    82     ec_sdo_request_clear(&fsm->request);
    82     ec_sdo_request_clear(&fsm->request);
    83 }
    83 }
    84 
    84 
    85 /*****************************************************************************/
    85 /*****************************************************************************/
    86 
    86 
       
    87 /** Print the current and desired PDO mapping.
       
    88  */
       
    89 void ec_fsm_pdo_entry_print(
       
    90         ec_fsm_pdo_entry_t *fsm /**< PDO configuration state machine. */
       
    91         )
       
    92 {
       
    93     printk("Currently mapped PDO entries: ");
       
    94     ec_pdo_print_entries(fsm->cur_pdo);
       
    95     printk(". Entries to map: ");
       
    96     ec_pdo_print_entries(fsm->source_pdo);
       
    97     printk("\n");
       
    98 }
       
    99 
       
   100 /*****************************************************************************/
       
   101 
    87 /** Start reading a PDO's entries.
   102 /** Start reading a PDO's entries.
    88  */
   103  */
    89 void ec_fsm_pdo_entry_start_reading(
   104 void ec_fsm_pdo_entry_start_reading(
    90         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
   105         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
    91         ec_slave_t *slave, /**< slave to configure */
   106         ec_slave_t *slave, /**< slave to configure */
   105 /** Start PDO mapping state machine.
   120 /** Start PDO mapping state machine.
   106  */
   121  */
   107 void ec_fsm_pdo_entry_start_configuration(
   122 void ec_fsm_pdo_entry_start_configuration(
   108         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
   123         ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
   109         ec_slave_t *slave, /**< slave to configure */
   124         ec_slave_t *slave, /**< slave to configure */
   110         const ec_pdo_t *pdo /**< PDO with the desired entries. */
   125         const ec_pdo_t *pdo, /**< PDO with the desired entries. */
       
   126         const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
   111         )
   127         )
   112 {
   128 {
   113     fsm->slave = slave;
   129     fsm->slave = slave;
   114     fsm->source_pdo = pdo;
   130     fsm->source_pdo = pdo;
       
   131     fsm->cur_pdo = cur_pdo;
       
   132 
       
   133     if (fsm->slave->master->debug_level) {
       
   134         EC_DBG("Changing mapping of PDO 0x%04X.\n", pdo->index);
       
   135         EC_DBG(""); ec_fsm_pdo_entry_print(fsm);
       
   136     }
   115 
   137 
   116     fsm->state = ec_fsm_pdo_entry_conf_state_start;
   138     fsm->state = ec_fsm_pdo_entry_conf_state_start;
   117 }
   139 }
   118 
   140 
   119 /*****************************************************************************/
   141 /*****************************************************************************/
   308     if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
   330     if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
   309             || (fsm->slave->sii.has_general
   331             || (fsm->slave->sii.has_general
   310                 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) {
   332                 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) {
   311         EC_WARN("Slave %u does not support changing the PDO mapping!\n",
   333         EC_WARN("Slave %u does not support changing the PDO mapping!\n",
   312                 fsm->slave->ring_position);
   334                 fsm->slave->ring_position);
       
   335         EC_WARN(""); ec_fsm_pdo_entry_print(fsm);
   313         fsm->state = ec_fsm_pdo_entry_state_error;
   336         fsm->state = ec_fsm_pdo_entry_state_error;
   314         return;
   337         return;
   315     }
   338     }
   316 
   339 
   317     if (ec_sdo_request_alloc(&fsm->request, 4)) {
   340     if (ec_sdo_request_alloc(&fsm->request, 4)) {
   359     if (ec_fsm_coe_exec(fsm->fsm_coe))
   382     if (ec_fsm_coe_exec(fsm->fsm_coe))
   360         return;
   383         return;
   361 
   384 
   362     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   385     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   363         EC_WARN("Failed to clear PDO mapping.\n");
   386         EC_WARN("Failed to clear PDO mapping.\n");
       
   387         EC_WARN(""); ec_fsm_pdo_entry_print(fsm);
   364         fsm->state = ec_fsm_pdo_entry_state_error;
   388         fsm->state = ec_fsm_pdo_entry_state_error;
   365         return;
   389         return;
   366     }
   390     }
   367 
   391 
   368     // find first entry
   392     // find first entry
   420 
   444 
   421     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   445     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   422         EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to "
   446         EC_WARN("Failed to map PDO entry 0x%04X:%02X (%u bit) to "
   423                 "position %u.\n", fsm->entry->index, fsm->entry->subindex,
   447                 "position %u.\n", fsm->entry->index, fsm->entry->subindex,
   424                 fsm->entry->bit_length, fsm->entry_pos);
   448                 fsm->entry->bit_length, fsm->entry_pos);
       
   449         EC_WARN(""); ec_fsm_pdo_entry_print(fsm);
   425         fsm->state = ec_fsm_pdo_entry_state_error;
   450         fsm->state = ec_fsm_pdo_entry_state_error;
   426         return;
   451         return;
   427     }
   452     }
   428 
   453 
   429     // find next entry
   454     // find next entry
   459         )
   484         )
   460 {
   485 {
   461     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   486     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
   462 
   487 
   463     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   488     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
   464         EC_ERR("Failed to set number of entries.\n");
   489         EC_WARN("Failed to set number of entries.\n");
       
   490         EC_WARN(""); ec_fsm_pdo_entry_print(fsm);
   465         fsm->state = ec_fsm_pdo_entry_state_error;
   491         fsm->state = ec_fsm_pdo_entry_state_error;
   466         return;
   492         return;
   467     }
   493     }
   468 
   494 
   469     if (fsm->slave->master->debug_level)
   495     if (fsm->slave->master->debug_level)