void lcec_el95xx_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el95xx_data_t *hal_data = (lcec_el95xx_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
// wait for slave to be operational
if (!slave->state.operational) {
return;
}
// check inputs
*(hal_data->power_ok) = EC_READ_BIT(&pd[hal_data->power_ok_pdo_os], hal_data->power_ok_pdo_bp);
*(hal_data->overload) = EC_READ_BIT(&pd[hal_data->overload_pdo_os], hal_data->overload_pdo_bp);
}
/** Implementation of update.\n
* Fetch process data for this terminal.
* @param domain_pd Offset for overlaying domains process data
*/
void EL3202::update( uint8_t *domain_pd )
{
uint8_t *totOffs = domain_pd + _offset;
//Update underranges
underranges[0].setValue( EC_READ_BIT( totOffs, 0 ) );
underranges[1].setValue( EC_READ_BIT( totOffs + 4, 0 ) );
//Update overranges
overranges[0].setValue( EC_READ_BIT( totOffs, 1 ) );
overranges[1].setValue( EC_READ_BIT( totOffs + 4, 1 ) );
//Update limit 1
std::bitset<2> tmp;
tmp.set( 0, EC_READ_BIT( totOffs, 2 ) );
tmp.set( 1, EC_READ_BIT( totOffs, 3 ) );
limits1[0].setValue( tmp.to_ulong() );
tmp.set( 0, EC_READ_BIT( totOffs + 4, 2 ) );
tmp.set( 1, EC_READ_BIT( totOffs + 4, 3 ) );
limits1[1].setValue( tmp.to_ulong() );
//Update limit 1
tmp.set( 0, EC_READ_BIT( totOffs, 4 ) );
tmp.set( 1, EC_READ_BIT( totOffs, 5 ) );
limits2[0].setValue( tmp.to_ulong() );
tmp.set( 0, EC_READ_BIT( totOffs + 4, 4 ) );
tmp.set( 1, EC_READ_BIT( totOffs + 4, 5 ) );
limits2[1].setValue( tmp.to_ulong() );
//Update errors
errors[0].setValue( EC_READ_BIT( totOffs, 6 ) );
errors[1].setValue( EC_READ_BIT( totOffs + 4, 6 ) );
//Update TxPDO States
tx_pdo_states[0].setValue( EC_READ_BIT( totOffs + 1, 6 ) );
tx_pdo_states[1].setValue( EC_READ_BIT( totOffs + 4 + 1, 6 ) );
//Update TxPDO Toggles
tx_pdo_toggles[0].setValue( EC_READ_BIT( totOffs + 1, 7 ) );
tx_pdo_toggles[1].setValue( EC_READ_BIT( totOffs + 4 + 1, 7 ) );
//Update temperature values
values[0].setValue( EC_READ_S16( totOffs + 2 ) );
values[1].setValue( EC_READ_S16( totOffs + 4 + 2 ) );
//TODO handle high resolution mode
fValues[0].setValue( (double)values[0].value() / 10.0f );
fValues[1].setValue( (double)values[1].value() / 10.0f );
}
void lcec_el7342_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el7342_data_t *hal_data = (lcec_el7342_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i;
lcec_el7342_chan_t *chan;
int16_t raw_count, raw_latch, raw_delta;
// wait for slave to be operational
if (!slave->state.operational) {
hal_data->last_operational = 0;
return;
}
// check inputs
for (i=0; i<LCEC_EL7342_CHANS; i++) {
chan = &hal_data->chans[i];
// check for change in scale value
if (*(chan->pos_scale) != chan->enc_old_scale) {
// scale value has changed, test and update it
if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->pos_scale) = 1.0;
}
// save new scale to detect future changes
chan->enc_old_scale = *(chan->pos_scale);
// we actually want the reciprocal
chan->enc_scale_recip = 1.0 / *(chan->pos_scale);
}
// get bit states
*(chan->ina) = EC_READ_BIT(&pd[chan->ina_pdo_os], chan->ina_pdo_bp);
*(chan->inb) = EC_READ_BIT(&pd[chan->inb_pdo_os], chan->inb_pdo_bp);
*(chan->inext) = EC_READ_BIT(&pd[chan->inext_pdo_os], chan->inext_pdo_bp);
*(chan->sync_err) = EC_READ_BIT(&pd[chan->sync_err_pdo_os], chan->sync_err_pdo_bp);
*(chan->expol_stall) = EC_READ_BIT(&pd[chan->expol_stall_pdo_os], chan->expol_stall_pdo_bp);
*(chan->tx_toggle) = EC_READ_BIT(&pd[chan->tx_toggle_pdo_os], chan->tx_toggle_pdo_bp);
*(chan->count_overflow) = EC_READ_BIT(&pd[chan->count_overflow_pdo_os], chan->count_overflow_pdo_bp);
*(chan->count_underflow) = EC_READ_BIT(&pd[chan->count_underflow_pdo_os], chan->count_underflow_pdo_bp);
*(chan->latch_ext_valid) = EC_READ_BIT(&pd[chan->latch_ext_valid_pdo_os], chan->latch_ext_valid_pdo_bp);
*(chan->dcm_ready_to_enable) = EC_READ_BIT(&pd[chan->dcm_ready_to_enable_pdo_os], chan->dcm_ready_to_enable_pdo_bp);
*(chan->dcm_ready) = EC_READ_BIT(&pd[chan->dcm_ready_pdo_os], chan->dcm_ready_pdo_bp);
*(chan->dcm_warning) = EC_READ_BIT(&pd[chan->dcm_warning_pdo_os], chan->dcm_warning_pdo_bp);
*(chan->dcm_error) = EC_READ_BIT(&pd[chan->dcm_error_pdo_os], chan->dcm_error_pdo_bp);
*(chan->dcm_move_pos) = EC_READ_BIT(&pd[chan->dcm_move_pos_pdo_os], chan->dcm_move_pos_pdo_bp);
*(chan->dcm_move_neg) = EC_READ_BIT(&pd[chan->dcm_move_neg_pdo_os], chan->dcm_move_neg_pdo_bp);
*(chan->dcm_torque_reduced) = EC_READ_BIT(&pd[chan->dcm_torque_reduced_pdo_os], chan->dcm_torque_reduced_pdo_bp);
*(chan->dcm_din1) = EC_READ_BIT(&pd[chan->dcm_din1_pdo_os], chan->dcm_din1_pdo_bp);
*(chan->dcm_din2) = EC_READ_BIT(&pd[chan->dcm_din2_pdo_os], chan->dcm_din2_pdo_bp);
*(chan->dcm_sync_err) = EC_READ_BIT(&pd[chan->dcm_sync_err_pdo_os], chan->dcm_sync_err_pdo_bp);
*(chan->dcm_tx_toggle) = EC_READ_BIT(&pd[chan->dcm_tx_toggle_pdo_os], chan->dcm_tx_toggle_pdo_bp);
// read raw values
raw_count = EC_READ_S16(&pd[chan->count_pdo_os]);
raw_latch = EC_READ_S16(&pd[chan->latch_pdo_os]);
// read raw info values
*(chan->dcm_raw_info1) = EC_READ_S16(&pd[chan->dcm_info1_pdo_os]);
*(chan->dcm_raw_info2) = EC_READ_S16(&pd[chan->dcm_info2_pdo_os]);
// dispatch info values
lcec_el7342_set_info(chan, chan->dcm_raw_info1, chan->dcm_sel_info1);
lcec_el7342_set_info(chan, chan->dcm_raw_info2, chan->dcm_sel_info2);
// check for operational change of slave
if (!hal_data->last_operational) {
chan->enc_last_count = raw_count;
}
// check for counter set done
if (EC_READ_BIT(&pd[chan->set_count_done_pdo_os], chan->set_count_done_pdo_bp)) {
chan->enc_last_count = raw_count;
*(chan->set_raw_count) = 0;
}
// update raw values
if (! *(chan->set_raw_count)) {
*(chan->raw_count) = raw_count;
}
// handle initialization
if (chan->enc_do_init || *(chan->reset)) {
chan->enc_do_init = 0;
chan->enc_last_count = raw_count;
*(chan->count) = 0;
}
// handle index
if (*(chan->latch_ext_valid)) {
*(chan->raw_latch) = raw_latch;
chan->enc_last_count = raw_latch;
*(chan->count) = 0;
*(chan->ena_latch_ext_pos) = 0;
*(chan->ena_latch_ext_neg) = 0;
}
// compute net counts
raw_delta = raw_count - chan->enc_last_count;
chan->enc_last_count = raw_count;
*(chan->count) += raw_delta;
// scale count to make floating point position
*(chan->pos) = *(chan->count) * chan->enc_scale_recip;
}
hal_data->last_operational = 1;
}
void lcec_el7041_1000_read(struct lcec_slave *s, long period) {
lcec_master_t *m = s->master;
lcec_el7041_1000_data_t *hd = (lcec_el7041_1000_data_t *) s->hal_data;
uint8_t *pd = m->process_data;
int16_t raw_count, raw_latch, raw_delta;
// wait for slave to be operational
if (!s->state.operational) {
hd->last_operational = 0;
return;
}
// check inputs
// check for change in scale value
if (*(hd->pos_scale) != hd->enc_old_scale) {
// scale value has changed, test and update it
if ((*(hd->pos_scale) < 1e-20) && (*(hd->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(hd->pos_scale) = 1.0;
}
// save new scale to detect future changes
hd->enc_old_scale = *(hd->pos_scale);
// we actually want the reciprocal
hd->enc_scale_recip = 1.0 / *(hd->pos_scale);
}
// get bit states
*(hd->ina) = EC_READ_BIT(&pd[hd->ina_pdo_os], hd->ina_pdo_bp);
*(hd->inb) = EC_READ_BIT(&pd[hd->inb_pdo_os], hd->inb_pdo_bp);
*(hd->inc) = EC_READ_BIT(&pd[hd->inc_pdo_os], hd->inc_pdo_bp);
*(hd->inext) = EC_READ_BIT(&pd[hd->inext_pdo_os], hd->inext_pdo_bp);
*(hd->sync_err) = EC_READ_BIT(&pd[hd->sync_err_pdo_os], hd->sync_err_pdo_bp);
*(hd->expol_stall) = EC_READ_BIT(&pd[hd->expol_stall_pdo_os], hd->expol_stall_pdo_bp);
*(hd->tx_toggle) = EC_READ_BIT(&pd[hd->tx_toggle_pdo_os], hd->tx_toggle_pdo_bp);
*(hd->count_overflow) = EC_READ_BIT(&pd[hd->count_overflow_pdo_os], hd->count_overflow_pdo_bp);
*(hd->count_underflow) = EC_READ_BIT(&pd[hd->count_underflow_pdo_os], hd->count_underflow_pdo_bp);
*(hd->latch_c_valid) = EC_READ_BIT(&pd[hd->latch_c_valid_pdo_os], hd->latch_c_valid_pdo_bp);
*(hd->latch_ext_valid) = EC_READ_BIT(&pd[hd->latch_ext_valid_pdo_os], hd->latch_ext_valid_pdo_bp);
*(hd->dcm_ready_to_enable) = EC_READ_BIT(&pd[hd->dcm_ready_to_enable_pdo_os], hd->dcm_ready_to_enable_pdo_bp);
*(hd->dcm_ready) = EC_READ_BIT(&pd[hd->dcm_ready_pdo_os], hd->dcm_ready_pdo_bp);
*(hd->dcm_warning) = EC_READ_BIT(&pd[hd->dcm_warning_pdo_os], hd->dcm_warning_pdo_bp);
*(hd->dcm_error) = EC_READ_BIT(&pd[hd->dcm_error_pdo_os], hd->dcm_error_pdo_bp);
*(hd->dcm_move_pos) = EC_READ_BIT(&pd[hd->dcm_move_pos_pdo_os], hd->dcm_move_pos_pdo_bp);
*(hd->dcm_move_neg) = EC_READ_BIT(&pd[hd->dcm_move_neg_pdo_os], hd->dcm_move_neg_pdo_bp);
*(hd->dcm_torque_reduced) = EC_READ_BIT(&pd[hd->dcm_torque_reduced_pdo_os], hd->dcm_torque_reduced_pdo_bp);
*(hd->dcm_din1) = EC_READ_BIT(&pd[hd->dcm_din1_pdo_os], hd->dcm_din1_pdo_bp);
*(hd->dcm_din2) = EC_READ_BIT(&pd[hd->dcm_din2_pdo_os], hd->dcm_din2_pdo_bp);
*(hd->dcm_sync_err) = EC_READ_BIT(&pd[hd->dcm_sync_err_pdo_os], hd->dcm_sync_err_pdo_bp);
*(hd->dcm_tx_toggle) = EC_READ_BIT(&pd[hd->dcm_tx_toggle_pdo_os], hd->dcm_tx_toggle_pdo_bp);
hd->internal_fault = *(hd->dcm_error);
// read raw values
raw_count = EC_READ_S16(&pd[hd->count_pdo_os]);
raw_latch = EC_READ_S16(&pd[hd->latch_pdo_os]);
// check for operational change of slave
if (!hd->last_operational) {
hd->enc_last_count = raw_count;
}
// check for counter set done
if (EC_READ_BIT(&pd[hd->set_count_done_pdo_os], hd->set_count_done_pdo_bp)) {
hd->enc_last_count = raw_count;
*(hd->set_raw_count) = 0;
}
// update raw values
if (! *(hd->set_raw_count)) {
*(hd->raw_count) = raw_count;
}
// handle initialization
if (hd->enc_do_init || *(hd->reset)) {
hd->enc_do_init = 0;
hd->enc_last_count = raw_count;
*(hd->count) = 0;
}
// clear pending fault reset if no fault
if (!hd->internal_fault) {
hd->fault_reset_retry = 0;
}
// generate gated fault
if (hd->fault_reset_retry > 0) {
if (hd->fault_reset_cycle < 1) {
hd->fault_reset_cycle++;
} else {
hd->fault_reset_cycle = 0;
hd->fault_reset_state = !hd->fault_reset_state;
if (hd->fault_reset_state) {
hd->fault_reset_retry--;
}
}
*(hd->fault) = 0;
} else {
*(hd->fault) = hd->internal_fault;
}
// handle index
if (*(hd->latch_ext_valid)) {
*(hd->raw_latch) = raw_latch;
hd->enc_last_count = raw_latch;
*(hd->count) = 0;
*(hd->ena_latch_ext_pos) = 0;
*(hd->ena_latch_ext_neg) = 0;
}
// compute net counts
raw_delta = raw_count - hd->enc_last_count;
hd->enc_last_count = raw_count;
*(hd->count) += raw_delta;
// scale count to make floating point position
*(hd->pos) = *(hd->count) * hd->enc_scale_recip;
hd->last_operational = 1;
}
void lcec_el5152_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el5152_data_t *hal_data = (lcec_el5152_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i, idx_flag;
lcec_el5152_chan_t *chan;
int32_t idx_count, raw_count, raw_delta;
uint32_t raw_period;
// wait for slave to be operational
if (!slave->state.operational) {
hal_data->last_operational = 0;
return;
}
// check inputs
for (i=0; i<LCEC_EL5152_CHANS; i++) {
chan = &hal_data->chans[i];
// check for change in scale value
if (*(chan->pos_scale) != chan->old_scale) {
// scale value has changed, test and update it
if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->pos_scale) = 1.0;
}
// save new scale to detect future changes
chan->old_scale = *(chan->pos_scale);
// we actually want the reciprocal
chan->scale = 1.0 / *(chan->pos_scale);
}
// get bit states
*(chan->ina) = EC_READ_BIT(&pd[chan->ina_pdo_os], chan->ina_pdo_bp);
*(chan->inb) = EC_READ_BIT(&pd[chan->inb_pdo_os], chan->inb_pdo_bp);
*(chan->expol_stall) = EC_READ_BIT(&pd[chan->expol_stall_pdo_os], chan->expol_stall_pdo_bp);
*(chan->tx_toggle) = EC_READ_BIT(&pd[chan->tx_toggle_pdo_os], chan->tx_toggle_pdo_bp);
// read raw values
raw_count = EC_READ_S32(&pd[chan->count_pdo_os]);
raw_period = EC_READ_U32(&pd[chan->period_pdo_os]);
// check for operational change of slave
if (!hal_data->last_operational) {
chan->last_count = raw_count;
}
// check for counter set done
if (EC_READ_BIT(&pd[chan->set_count_done_pdo_os], chan->set_count_done_pdo_bp)) {
chan->last_count = raw_count;
*(chan->set_raw_count) = 0;
}
// update raw values
if (! *(chan->set_raw_count)) {
*(chan->raw_count) = raw_count;
*(chan->raw_period) = raw_period;
}
// check for index edge
idx_flag = 0;
idx_count = 0;
if (*(chan->index) && !chan->last_index) {
idx_count = raw_count;
idx_flag = 1;
}
chan->last_index = *(chan->index);
// handle initialization
if (chan->do_init || *(chan->reset)) {
chan->do_init = 0;
chan->last_count = raw_count;
*(chan->count) = 0;
idx_flag = 0;
}
// handle index
if (idx_flag && *(chan->index_ena)) {
chan->last_count = idx_count;
*(chan->count) = 0;
*(chan->index_ena) = 0;
}
// compute net counts
raw_delta = raw_count - chan->last_count;
chan->last_count = raw_count;
*(chan->count) += raw_delta;
// scale count to make floating point position
*(chan->pos) = *(chan->count) * chan->scale;
// scale period
*(chan->period) = ((double) (*(chan->raw_period))) * LCEC_EL5152_PERIOD_SCALE;
}
hal_data->last_operational = 1;
}
