/** Prepare a read request (RRQ) with filename * * \return Zero on success, otherwise a negative error code. */ int ec_foe_prepare_rrq_send( ec_fsm_foe_t *fsm, /**< Finite state machine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { size_t current_size; uint8_t *data; current_size = fsm->rx_filename_len; data = ec_slave_mbox_prepare_send(fsm->slave, datagram, EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE); if (IS_ERR(data)) { return -1; } EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request EC_WRITE_U32(data + 2, 0x00000000); // no passwd memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size); if (fsm->slave->master->debug_level) { EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n"); ec_print_data(data, current_size + EC_FOE_HEADER_SIZE); } return 0; }
/** Sends a file or the next fragment. * * \return Zero on success, otherwise a negative error code. */ int ec_foe_prepare_data_send( ec_fsm_foe_t *fsm, /**< Finite state machine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { size_t remaining_size, current_size; uint8_t *data; remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset; if (remaining_size < fsm->slave->configured_tx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) { current_size = remaining_size; fsm->tx_last_packet = 1; } else { current_size = fsm->slave->configured_tx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE; } data = ec_slave_mbox_prepare_send(fsm->slave, datagram, EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE); if (IS_ERR(data)) { return -1; } EC_WRITE_U16(data, EC_FOE_OPCODE_DATA); // OpCode = DataBlock req. EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_buffer + fsm->tx_buffer_offset, current_size); fsm->tx_current_size = current_size; return 0; }
/** Prepare a write request (WRQ) with filename * * \return Zero on success, otherwise a negative error code. */ int ec_foe_prepare_wrq_send( ec_fsm_foe_t *fsm, /**< Finite state machine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { size_t current_size; uint8_t *data; fsm->tx_buffer_offset = 0; fsm->tx_current_size = 0; fsm->tx_packet_no = 0; fsm->tx_last_packet = 0; current_size = fsm->tx_filename_len; data = ec_slave_mbox_prepare_send(fsm->slave, datagram, EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE); if (IS_ERR(data)) { return -1; } EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request EC_WRITE_U32( data + 2, fsm->tx_packet_no ); memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size); return 0; }
/** Prepare to send an acknowledge. * * \return Zero on success, otherwise a negative error code. */ int ec_foe_prepare_send_ack( ec_fsm_foe_t *fsm, /**< FoE statemachine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { uint8_t *data; data = ec_slave_mbox_prepare_send(fsm->slave, datagram, EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE); if (IS_ERR(data)) { return -1; } EC_WRITE_U16(data, EC_FOE_OPCODE_ACK); EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no); return 0; }
void write_sdo(ec_sdo_request_t *req, unsigned data) { EC_WRITE_U32(ecrt_sdo_request_data(req), data&0xffffffff); switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_write(req); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "SDO write still busy...\n"); break; case EC_REQUEST_SUCCESS: logmsg(1, "SDO value written: 0x%X\n", data); ecrt_sdo_request_write(req); // trigger next read ??? break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to write SDO!\n"); ecrt_sdo_request_write(req); // retry writing break; } }
/** Initializes an FMMU configuration page. * * The referenced memory (\a data) must be at least EC_FMMU_PAGE_SIZE bytes. */ void ec_fmmu_config_page( const ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */ const ec_sync_t *sync, /**< Sync manager. */ uint8_t *data /**> Configuration page memory. */ ) { EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, Size %3u," " PhysAddr 0x%04X, SM%u, Dir %s\n", fmmu->logical_start_address, fmmu->data_size, sync->physical_start_address, fmmu->sync_index, fmmu->dir == EC_DIR_INPUT ? "in" : "out"); EC_WRITE_U32(data, fmmu->logical_start_address); EC_WRITE_U16(data + 4, fmmu->data_size); // size of fmmu EC_WRITE_U8 (data + 6, 0x00); // logical start bit EC_WRITE_U8 (data + 7, 0x07); // logical end bit EC_WRITE_U16(data + 8, sync->physical_start_address); EC_WRITE_U8 (data + 10, 0x00); // physical start bit EC_WRITE_U8 (data + 11, fmmu->dir == EC_DIR_INPUT ? 0x01 : 0x02); EC_WRITE_U16(data + 12, 0x0001); // enable EC_WRITE_U16(data + 14, 0x0000); // reserved }
/** Starts to add a PDO entry. */ void ec_fsm_pdo_entry_conf_action_map( ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { uint32_t value; EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)" " at position %u.\n", fsm->entry->index, fsm->entry->subindex, fsm->entry->bit_length, fsm->entry_pos); value = fsm->entry->index << 16 | fsm->entry->subindex << 8 | fsm->entry->bit_length; EC_WRITE_U32(fsm->request.data, value); fsm->request.data_size = 4; ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, fsm->entry_pos); ecrt_sdo_request_write(&fsm->request); fsm->state = ec_fsm_pdo_entry_conf_state_map_entry; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately }
/** Prepare a set IP parameters operation. * * \return 0 on success, otherwise a negative error code. */ int ec_fsm_eoe_prepare_set( ec_fsm_eoe_t *fsm, /**< finite state machine */ ec_datagram_t *datagram /**< Datagram to use. */ ) { uint8_t *data, *cur; ec_slave_t *slave = fsm->slave; ec_master_t *master = slave->master; ec_eoe_request_t *req = fsm->request; size_t size = 8; if (req->mac_address_included) { size += ETH_ALEN; } if (req->ip_address_included) { size += 4; } if (req->subnet_mask_included) { size += 4; } if (req->gateway_included) { size += 4; } if (req->dns_included) { size += 4; } if (req->name_included) { size += EC_MAX_HOSTNAME_SIZE; } data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE, size); if (IS_ERR(data)) { return PTR_ERR(data); } EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req. EC_WRITE_U8(data + 1, 0x01); // last fragment, no timestamps EC_WRITE_U16(data + 2, 0x0000); // fragment no., offset, frame no. EC_WRITE_U32(data + 4, ((req->mac_address_included != 0) << 0) | ((req->ip_address_included != 0) << 1) | ((req->subnet_mask_included != 0) << 2) | ((req->gateway_included != 0) << 3) | ((req->dns_included != 0) << 4) | ((req->name_included != 0) << 5) ); cur = data + 8; if (req->mac_address_included) { memcpy(cur, req->mac_address, ETH_ALEN); cur += ETH_ALEN; } if (req->ip_address_included) { memcpy(cur, &req->ip_address, 4); cur += 4; } if (req->subnet_mask_included) { memcpy(cur, &req->subnet_mask, 4); cur += 4; } if (req->gateway_included) { memcpy(cur, &req->gateway, 4); cur += 4; } if (req->dns_included) { memcpy(cur, &req->dns, 4); cur += 4; } if (req->name_included) { memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE); cur += EC_MAX_HOSTNAME_SIZE; } if (master->debug_level) { EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n"); ec_print_data(data, cur - data); } fsm->request->jiffies_sent = jiffies; return 0; }
void pdo_handle_ecat(master_setup_variables_t *master_setup, ctrlproto_slv_handle *slv_handles, unsigned int total_no_of_slaves) { int slv; if(sig_alarms == user_alarms) pause(); while (sig_alarms != user_alarms) { /* sync the dc clock of the slaves */ // ecrt_master_sync_slave_clocks(master); // receive process data ecrt_master_receive(master_setup->master); ecrt_domain_process(master_setup->domain); // check process data state (optional) //check_domain1_state(master_setup); // check for master state (optional) //check_master_state(master_setup); // check for islave configuration state(s) (optional) // check_slave_config_states(); for(slv=0;slv<total_no_of_slaves;++slv) { /* Read process data */ slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]); slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]); slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]); slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]); slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]); } /* printf("\n%x", slv_handles[slv].motorctrl_status_in); printf("\n%x", slv_handles[slv].operation_mode_disp); printf("\n%x", slv_handles[slv].position_in); printf("\n%x", slv_handles[slv].speed_in); printf("\n%x", slv_handles[slv].torque_in); */ for(slv=0;slv<total_no_of_slaves;++slv) { EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff); EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff); EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff); EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint); EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint); } // send process data ecrt_domain_queue(master_setup->domain); ecrt_master_send(master_setup->master); //Check for master und domain state ecrt_master_state(master_setup->master, &master_setup->master_state); ecrt_domain_state(master_setup->domain, &master_setup->domain_state); if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag) { //printf("System up!\n"); master_setup->op_flag = 1; } else { if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag) { //printf("System down!\n"); master_setup->op_flag = 0; } } user_alarms++; } }
void cyclic_task() { /* sync the dc clock of the slaves */ ecrt_master_sync_slave_clocks(master); // receive process data ecrt_master_receive(master); ecrt_domain_process(domain1); // check process data state (optional) check_domain1_state(); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; // calculate new process data blink = !blink; // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); #if SDO_ACCESS // read process data SDO read_sdo(sdo); read_sdo(request[0]); read_sdo(request[1]); read_sdo(request[2]); write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */ #endif } /* Read process data */ unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in); unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in); unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in); unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in); unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in); logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n", sn_status, sn_modes, sn_position, sn_velocity, sn_torque); #if 0 // read process data printf("AnaIn: state %u value %u\n", EC_READ_U8(domain1_pd + off_ana_in_status), EC_READ_U16(domain1_pd + off_ana_in_value)); #endif // write process data //EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09); #ifdef CIA402 #define STATUSW1 0x88AA #define STATUSW2 0xAA88 #define OPMODES1 0xf1 #define OPMODES2 0x1f #define TORVAL1 0xabab #define TORVAL2 0xbaba #define VELVAL1 0x2d2d4d4d #define VELVAL2 0xd4d4d2d2 #define POSVAL1 0xe4e4e2e2 #define POSVAL2 0x2e2e4e4e EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff); EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff); EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff); EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2); EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2); #else #define TESTWORD1 0xdead #define TESTWORD2 0xbeef #define TESTWORD3 0xfefe #define TESTWORD4 0xa5a5 EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2); EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4); #endif // send process data ecrt_domain_queue(domain1); ecrt_master_send(master); //printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2); }