/** Initializes a sync manager configuration page.
*
* The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes.
*/
void ec_sync_page(
const ec_sync_t *sync, /**< Sync manager. */
uint8_t sync_index, /**< Index of the sync manager. */
uint16_t data_size, /**< Data size. */
const ec_sync_config_t *sync_config, /**< Configuration. */
uint8_t pdo_xfer, /**< Non-zero, if PDOs will be transferred via this
sync manager. */
uint8_t *data /**> Configuration memory. */
)
{
// enable only if (SII enable is set or PDO xfer)
// and size is > 0 and SM is not virtual
uint16_t enable = ((sync->enable & 0x01) || pdo_xfer)
&& data_size
&& ((sync->enable & 0x04) == 0);
uint8_t control = sync->control_register;
if (sync_config) {
switch (sync_config->dir) {
case EC_DIR_OUTPUT:
case EC_DIR_INPUT:
EC_WRITE_BIT(&control, 2,
sync_config->dir == EC_DIR_OUTPUT ? 1 : 0);
EC_WRITE_BIT(&control, 3, 0);
break;
default:
break;
}
switch (sync_config->watchdog_mode) {
case EC_WD_ENABLE:
case EC_WD_DISABLE:
EC_WRITE_BIT(&control, 6,
sync_config->watchdog_mode == EC_WD_ENABLE);
break;
default:
break;
}
}
EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u,"
" Ctrl 0x%02X, En %u\n",
sync_index, sync->physical_start_address,
data_size, control, enable);
EC_WRITE_U16(data, sync->physical_start_address);
EC_WRITE_U16(data + 2, data_size);
EC_WRITE_U8 (data + 4, control);
EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
EC_WRITE_U16(data + 6, enable);
}
void lcec_el2202_write(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
uint8_t *pd = master->process_data;
lcec_el2202_data_t *hal_data = (lcec_el2202_data_t *) slave->hal_data;
lcec_el2202_chan_t *chan;
int i;
for (i=0; i<LCEC_EL2202_CHANS; i++) {
chan = &hal_data->chans[i];
// set output
EC_WRITE_BIT(&pd[chan->out_offs], chan->out_bitp, *(chan->out));
// set tristate
EC_WRITE_BIT(&pd[chan->tristate_offs], chan->tristate_bitp, *(chan->tristate));
}
}
void lcec_el5152_write(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;
lcec_el5152_chan_t *chan;
// set outputs
for (i=0; i<LCEC_EL5152_CHANS; i++) {
chan = &hal_data->chans[i];
// set output data
EC_WRITE_BIT(&pd[chan->set_count_pdo_os], chan->set_count_pdo_bp, *(chan->set_raw_count));
EC_WRITE_S32(&pd[chan->set_count_val_pdo_os], *(chan->set_raw_count_val));
}
}
void lcec_el7342_write(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;
double tmpval, tmpdc, raw_val;
// set outputs
for (i=0; i<LCEC_EL7342_CHANS; i++) {
chan = &hal_data->chans[i];
// validate duty cycle limits, both limits must be between
// 0.0 and 1.0 (inclusive) and max must be greater then min
if (*(chan->dcm_max_dc) > 1.0) {
*(chan->dcm_max_dc) = 1.0;
}
if (*(chan->dcm_min_dc) > *(chan->dcm_max_dc)) {
*(chan->dcm_min_dc) = *(chan->dcm_max_dc);
}
if (*(chan->dcm_min_dc) < -1.0) {
*(chan->dcm_min_dc) = -1.0;
}
if (*(chan->dcm_max_dc) < *(chan->dcm_min_dc)) {
*(chan->dcm_max_dc) = *(chan->dcm_min_dc);
}
// do scale calcs only when scale changes
if (*(chan->dcm_scale) != chan->dcm_old_scale) {
// validate the new scale value
if ((*(chan->dcm_scale) < 1e-20) && (*(chan->dcm_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->dcm_scale) = 1.0;
}
// get ready to detect future scale changes
chan->dcm_old_scale = *(chan->dcm_scale);
// we will need the reciprocal
chan->dcm_scale_recip = 1.0 / *(chan->dcm_scale);
}
// get command
tmpval = *(chan->dcm_value);
if (*(chan->dcm_absmode) && (tmpval < 0)) {
tmpval = -tmpval;
}
// convert value command to duty cycle
tmpdc = tmpval * chan->dcm_scale_recip + *(chan->dcm_offset);
if (tmpdc < *(chan->dcm_min_dc)) {
tmpdc = *(chan->dcm_min_dc);
}
if (tmpdc > *(chan->dcm_max_dc)) {
tmpdc = *(chan->dcm_max_dc);
}
// set output values
if (*(chan->dcm_enable) == 0) {
raw_val = 0;
*(chan->dcm_curr_dc) = 0;
} else {
raw_val = (double)0x7fff * tmpdc;
if (raw_val > (double)0x7fff) {
raw_val = (double)0x7fff;
}
if (raw_val < (double)-0x7fff) {
raw_val = (double)-0x7fff;
}
*(chan->dcm_curr_dc) = tmpdc;
}
// update value
*(chan->dcm_raw_val) = (int32_t)raw_val;
// set output data
EC_WRITE_BIT(&pd[chan->set_count_pdo_os], chan->set_count_pdo_bp, *(chan->set_raw_count));
EC_WRITE_BIT(&pd[chan->ena_latch_ext_pos_pdo_os], chan->ena_latch_ext_pos_pdo_bp, *(chan->ena_latch_ext_pos));
EC_WRITE_BIT(&pd[chan->ena_latch_ext_neg_pdo_os], chan->ena_latch_ext_neg_pdo_bp, *(chan->ena_latch_ext_neg));
EC_WRITE_S16(&pd[chan->set_count_val_pdo_os], *(chan->set_raw_count_val));
EC_WRITE_BIT(&pd[chan->dcm_ena_pdo_os], chan->dcm_ena_pdo_bp, *(chan->dcm_enable));
EC_WRITE_BIT(&pd[chan->dcm_reset_pdo_os], chan->dcm_reset_pdo_bp, *(chan->dcm_reset));
EC_WRITE_BIT(&pd[chan->dcm_reduce_torque_pdo_os], chan->dcm_reduce_torque_pdo_bp, *(chan->dcm_reduce_torque));
EC_WRITE_S16(&pd[chan->dcm_velo_pdo_os], (int16_t)raw_val);
}
}
void lcec_el7041_1000_write(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;
double tmpval, tmpdc, raw_val;
int enable_on_edge;
// set outputs
// check for enable edge
enable_on_edge = *(hd->dcm_enable) && !hd->last_dcm_enable;
hd->last_dcm_enable = *(hd->dcm_enable);
// check for autoreset
if (enable_on_edge && hd->internal_fault) {
hd->fault_reset_retry = 1;
hd->fault_reset_state = 1;
hd->fault_reset_cycle = 0;
}
// validate duty cycle limits, both limits must be between
// 0.0 and 1.0 (inclusive) and max must be greater then min
if (*(hd->dcm_max_dc) > 1.0) {
*(hd->dcm_max_dc) = 1.0;
}
if (*(hd->dcm_min_dc) > *(hd->dcm_max_dc)) {
*(hd->dcm_min_dc) = *(hd->dcm_max_dc);
}
if (*(hd->dcm_min_dc) < -1.0) {
*(hd->dcm_min_dc) = -1.0;
}
if (*(hd->dcm_max_dc) < *(hd->dcm_min_dc)) {
*(hd->dcm_max_dc) = *(hd->dcm_min_dc);
}
// do scale calcs only when scale changes
if (*(hd->dcm_scale) != hd->dcm_old_scale) {
// validate the new scale value
if ((*(hd->dcm_scale) < 1e-20) && (*(hd->dcm_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(hd->dcm_scale) = 1.0;
}
// get ready to detect future scale changes
hd->dcm_old_scale = *(hd->dcm_scale);
// we will need the reciprocal
hd->dcm_scale_recip = 1.0 / *(hd->dcm_scale);
}
// get command
tmpval = *(hd->dcm_value);
if (*(hd->dcm_absmode) && (tmpval < 0)) {
tmpval = -tmpval;
}
// convert value command to duty cycle
tmpdc = tmpval * hd->dcm_scale_recip + *(hd->dcm_offset);
if (tmpdc < *(hd->dcm_min_dc)) {
tmpdc = *(hd->dcm_min_dc);
}
if (tmpdc > *(hd->dcm_max_dc)) {
tmpdc = *(hd->dcm_max_dc);
}
// set output values
if (*(hd->dcm_enable) == 0) {
raw_val = 0;
*(hd->dcm_curr_dc) = 0;
} else {
raw_val = (double)0x7fff * tmpdc;
if (raw_val > (double)0x7fff) {
raw_val = (double)0x7fff;
}
if (raw_val < (double)-0x7fff) {
raw_val = (double)-0x7fff;
}
*(hd->dcm_curr_dc) = tmpdc;
}
// update value
*(hd->dcm_raw_val) = (int32_t)raw_val;
// fault reset
if (*(hd->fault_reset)) {
*(hd->dcm_reset) = 1;
}
if (hd->fault_reset_retry > 0) {
if (hd->fault_reset_state) {
*(hd->dcm_reset) = 1;
}
} else {
if (*(hd->dcm_enable)) {
*(hd->dcm_reset) = 0;
}
*(hd->fault_reset) = 0;
}
// set output data
EC_WRITE_BIT(&pd[hd->set_count_pdo_os], hd->set_count_pdo_bp, *(hd->set_raw_count));
EC_WRITE_BIT(&pd[hd->ena_latch_c_pdo_os], hd->ena_latch_c_pdo_bp, *(hd->ena_latch_c));
EC_WRITE_BIT(&pd[hd->ena_latch_ext_pos_pdo_os], hd->ena_latch_ext_pos_pdo_bp, *(hd->ena_latch_ext_pos));
EC_WRITE_BIT(&pd[hd->ena_latch_ext_neg_pdo_os], hd->ena_latch_ext_neg_pdo_bp, *(hd->ena_latch_ext_neg));
EC_WRITE_S16(&pd[hd->set_count_val_pdo_os], *(hd->set_raw_count_val));
EC_WRITE_BIT(&pd[hd->dcm_ena_pdo_os], hd->dcm_ena_pdo_bp, *(hd->dcm_enable));
EC_WRITE_BIT(&pd[hd->dcm_reset_pdo_os], hd->dcm_reset_pdo_bp, *(hd->dcm_reset));
EC_WRITE_BIT(&pd[hd->dcm_reduce_torque_pdo_os], hd->dcm_reduce_torque_pdo_bp, *(hd->dcm_reduce_torque));
EC_WRITE_S16(&pd[hd->dcm_velo_pdo_os], (int16_t)raw_val);
}
