/***********************************************************************
* LOCAL FUNCTION DEFINITIONS *
* these are functions used for exporting various HAL pins/prams *
************************************************************************/
static int export_counter(int num, vti_struct * addr)
{
int retval, msg;
char buf[HAL_NAME_LEN + 2];
/* This function exports a lot of stuff, which results in a lot of
logging if msg_level is at INFO or ALL. So we save the current value
of msg_level and restore it later. If you actually need to log this
function's actions, change the second line below */
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &addr->count[num], comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position", num);
retval = hal_pin_float_new(buf, HAL_OUT, &addr->pos[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->pos_scale[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
int export_encoder(hal_pru_generic_t *hpg, int i)
{
char name[HAL_NAME_LEN + 1];
int r, j;
// HAL values common to all channels in this instance
// ...nothing to do here...
// HAL values for individual channels
for (j=0; j < hpg->encoder.instance[i].num_channels; j++) {
// Export HAL Pins
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawcounts", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawcounts), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawlatch", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawlatch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count-latched", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position-latched", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.velocity", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.velocity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.reset", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.reset), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IO, &(hpg->encoder.instance[i].chan[j].hal.pin.index_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-polarity", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_polarity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.quadrature-error", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
// Export HAL Parameters
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.scale", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.scale), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.counter-mode", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.counter_mode), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.filter", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.filter), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.vel-timeout", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.vel_timeout), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
//
// init the hal objects that need it
//
hpg->encoder.instance[i].chan[j].hal.param.scale = 1.0;
hpg->encoder.instance[i].chan[j].hal.param.index_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.counter_mode = 0;
// hpg->encoder.instance[i].chan[j].hal.param.filter = 1;
hpg->encoder.instance[i].chan[j].hal.param.vel_timeout = 0.5;
*hpg->encoder.instance[i].chan[j].hal.pin.rawcounts = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.rawlatch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count_latch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.position = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.position_latch = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.velocity = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error = 0;
hpg->encoder.instance[i].chan[j].zero_offset = 0;
hpg->encoder.instance[i].chan[j].prev_reg_count = 0;
hpg->encoder.instance[i].chan[j].state = HM2_ENCODER_STOPPED;
}
return 0;
}
MarkHal::MarkHal()
{
int retval;
/* STEP 1: initialise the hal component */
comp_id= hal_init("mark");
if (comp_id< 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_init() failed\n");;
return ;
}
/* STEP 2: allocate shared memory for hal pins data */
halpins= (MarkHalPins *) hal_malloc(sizeof(MarkHalPins));
if ( halpins== 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return;
}
/* STEP 3: export pins*/
retval = hal_pin_s32_new("mark.cvCmd", HAL_IO, &halpins->cvCmd, comp_id);
if (retval != 0)
return;
*halpins->cvCmd=0;
retval = hal_pin_u32_new("mark.ready-index", HAL_IN, &halpins->readyIndex, comp_id);
if (retval != 0)
return ;
*halpins->readyIndex=0;
retval = hal_pin_u32_new("mark.show", HAL_IO, &halpins->showWin, comp_id);
if (retval != 0)
return ;
*halpins->showWin=0;
retval = hal_pin_bit_new("mark.clear-result", HAL_IN, &halpins->clearResult, comp_id);
if (retval != 0)
return ;
*halpins->clearResult=0;
retval = hal_pin_u32_new("mark.posCmd", HAL_IO, &halpins->posCmd, comp_id);
if (retval != 0)
return;
*halpins->posCmd=0;
retval = hal_pin_float_new("mark.pos-x", HAL_OUT, &halpins->posAxis[0], comp_id);
if (retval != 0)
return ;
retval = hal_pin_float_new("mark.pos-y", HAL_OUT, &halpins->posAxis[1], comp_id);
if (retval != 0)
return ;
retval = hal_pin_float_new("mark.pos-z", HAL_OUT, &halpins->posAxis[2], comp_id);
if (retval != 0)
return ;
retval = hal_pin_float_new("mark.pos-a", HAL_OUT, &halpins->posAxis[3], comp_id);
if (retval != 0)
return ;
retval = hal_pin_float_new("mark.pos-b", HAL_OUT, &halpins->posAxis[4], comp_id);
if (retval != 0)
return ;
retval = hal_pin_float_new("mark.pos-c", HAL_OUT, &halpins->posAxis[5], comp_id);
if (retval != 0)
return ;
retval = hal_pin_u32_new("mark.reachCmd", HAL_IN, &halpins->reachCmd, comp_id);
if (retval != 0)
return;
*halpins->reachCmd=0;
retval = hal_pin_bit_new("mark.posValid", HAL_OUT, &halpins->posValid, comp_id);
if (retval != 0)
return ;
*halpins->posValid=1;
retval = hal_pin_bit_new("mark.watchPosValid", HAL_OUT, &halpins->watchPosValid, comp_id);
if (retval != 0)
return ;
*halpins->watchPosValid=1;
retval = hal_pin_bit_new("mark.watchHoleValid", HAL_OUT, &halpins->watchHoleValid, comp_id);
if (retval != 0)
return ;
*halpins->watchHoleValid=1;
retval = hal_pin_bit_new("mark.glueHoleValid", HAL_OUT, &halpins->glueHoleValid, comp_id);
if (retval != 0)
return ;
*halpins->glueHoleValid=1;
retval = hal_pin_bit_new("mark.start", HAL_IN, &halpins->start, comp_id);
if (retval != 0)
return ;
*halpins->start=0;
retval = hal_pin_bit_new("mark.paused", HAL_IN, &halpins->paused, comp_id);
if (retval != 0)
return ;
*halpins->paused=0;
retval = hal_pin_bit_new("mark.stop", HAL_IN, &halpins->stop, comp_id);
if (retval != 0)
return ;
*halpins->stop=0;
retval = hal_pin_s32_new("mark.state", HAL_OUT, &halpins->state, comp_id);
if (retval != 0)
return ;
*halpins->state=MARK_WAITING;
retval = hal_pin_u32_new("mark.fps", HAL_OUT, &halpins->fps, comp_id);
if (retval != 0)
return ;
const char* ioName[io_num] = {"mark.setGlue","mark.pickupDiamond","mark.dropDiamond",
"mark.lightControl","mark.glueUpDown"};
for(int i=0; i<5; i++)
{
retval = hal_pin_bit_new(ioName[i], HAL_IN, &halpins->ioPin[i], comp_id);
if (retval != 0)
return ;
*halpins->ioPin[i]=0;
}
hal_ready(comp_id);
//run config file
const char* halcmd="halcmd -i /home/u/cnc/configs/ppmc/ppmc.ini -f /home/u/cnc/configs/ppmc/watchDiamond.hal";
retval=system(halcmd);
if (retval != 0) {
printf("ERROR: %s fail\n", halcmd);
exit(-1);
}
}
/* init_hal_io() exports HAL pins and parameters making data from
the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
int n, retval;
joint_hal_t *joint_data;
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n");
/* allocate shared memory for machine data */
emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
if (emcmot_hal_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: emcmot_hal_data malloc failed\n"));
return -1;
}
/* export machine wide hal pins */
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error;
*(emcmot_hal_data->spindle_inhibit) = 0;
// spindle orient pins
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error;
*(emcmot_hal_data->spindle_orient_angle) = 0.0;
*(emcmot_hal_data->spindle_orient_mode) = 0;
*(emcmot_hal_data->spindle_orient) = 0;
// if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error;
*emcmot_hal_data->spindle_is_atspeed = 1;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error;
*(emcmot_hal_data->adaptive_feed) = 1.0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error;
*(emcmot_hal_data->feed_hold) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error;
*(emcmot_hal_data->feed_inhibit) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error;
/* export motion-synched digital output pins */
/* export motion digital input pins */
for (n = 0; n < num_dio; n++) {
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error;
}
/* export motion analog input pins */
for (n = 0; n < num_aio; n++) {
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error;
}
/* export machine wide hal parameters */
retval =
hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line),
mot_comp_id);
if (retval != 0) {
return retval;
}
/* export debug parameters */
/* these can be used to view any internal variable, simply change a line
in control.c:output_to_hal() and recompile */
retval =
hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
// FIXME - debug only, remove later
// export HAL parameters for some trajectory planner internal variables
// so they can be scoped
retval =
hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc),
mot_comp_id);
if (retval != 0) {
return retval;
}
for ( n = 0 ; n < 4 ; n++ ) {
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n);
if (retval != 0) {
return retval;
}
}
// end of exporting trajectory planner internals
// export timing related HAL parameters so they can be scoped
retval =
hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
if (retval != 0) {
return retval;
}
#ifdef HAVE_CPU_KHZ
retval =
hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id);
if (retval != 0) {
return retval;
}
#endif
retval =
hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id);
if (retval != 0) {
return retval;
}
/* initialize machine wide pins and parameters */
*(emcmot_hal_data->probe_input) = 0;
/* default value of enable is TRUE, so simple machines
can leave it disconnected */
*(emcmot_hal_data->enable) = 1;
/* motion synched dio, init to not enabled */
for (n = 0; n < num_dio; n++) {
*(emcmot_hal_data->synch_do[n]) = 0;
*(emcmot_hal_data->synch_di[n]) = 0;
}
for (n = 0; n < num_aio; n++) {
*(emcmot_hal_data->analog_output[n]) = 0.0;
*(emcmot_hal_data->analog_input[n]) = 0.0;
}
/*! \todo FIXME - these don't really need initialized, since they are written
with data from the emcmotStatus struct */
*(emcmot_hal_data->motion_enabled) = 0;
*(emcmot_hal_data->in_position) = 0;
*(emcmot_hal_data->coord_mode) = 0;
*(emcmot_hal_data->teleop_mode) = 0;
*(emcmot_hal_data->coord_error) = 0;
*(emcmot_hal_data->on_soft_limit) = 0;
/* init debug parameters */
emcmot_hal_data->debug_bit_0 = 0;
emcmot_hal_data->debug_bit_1 = 0;
emcmot_hal_data->debug_float_0 = 0.0;
emcmot_hal_data->debug_float_1 = 0.0;
emcmot_hal_data->debug_float_2 = 0.0;
emcmot_hal_data->debug_float_3 = 0.0;
emcmot_hal_data->overruns = 0;
emcmot_hal_data->last_period = 0;
/* export joint pins and parameters */
for (n = 0; n < num_joints; n++) {
/* point to axis data */
joint_data = &(emcmot_hal_data->joint[n]);
/* export all vars */
retval = export_joint(n, joint_data);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: joint %d pin/param export failed\n"), n);
return -1;
}
/* init axis pins and parameters */
/* FIXME - struct members are in a state of flux - make sure to
update this - most won't need initing anyway */
*(joint_data->amp_enable) = 0;
*(joint_data->home_state) = 0;
/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
because it is always supported. */
}
/* Done! */
rtapi_print_msg(RTAPI_MSG_INFO,
"MOTION: init_hal_io() complete, %d axes.\n", n);
return 0;
error:
return retval;
}
int hm2_resolver_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int i, r = 0;
int resolvers_per_instance;
//
// some standard sanity checks
//
if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x001F)) {
HM2_ERR("inconsistent resolver Module Descriptor!\n");
return -EINVAL;
}
if (hm2->resolver.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
resolvers_per_instance = hm2_resolver_get_param(2); // just returns 6 at the moment
if (hm2->config.num_resolvers > (md->instances * resolvers_per_instance)) {
HM2_ERR(
"config.num_resolvers=%d, but only %d are available, not loading driver\n",
hm2->config.num_resolvers,
md->instances * resolvers_per_instance);
return -EINVAL;
}
if (hm2->config.num_resolvers == 0) {
return 0;
}
//
// looks good, start initializing
//
/*At the moment it is not clear if there will ever be more than one resolver
instance. If there were to be more than one then they would need to have
a different base-address, and this code would need to be re-enterable.
A bridge to cross when we come to it */
if (hm2->config.num_resolvers == -1) {
hm2->resolver.num_resolvers = md->instances * resolvers_per_instance;
hm2->resolver.num_instances = md->instances;
} else {
hm2->resolver.num_resolvers = hm2->config.num_resolvers;
hm2->resolver.num_instances = md->instances;
}
hm2->resolver.hal = (hm2_resolver_global_t *)hal_malloc(
sizeof(hm2_resolver_global_t));
if (hm2->resolver.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->resolver.instance = (hm2_resolver_instance_t *)hal_malloc(
hm2->resolver.num_resolvers * sizeof(hm2_resolver_instance_t));
if (hm2->resolver.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->resolver.num_instances ; i++ ){
hm2->resolver.stride = md->register_stride;
hm2->resolver.clock_frequency = md->clock_freq;
hm2->resolver.version = md->version;
hm2->resolver.command_addr = md->base_address + (0 * md->register_stride);
hm2->resolver.data_addr = md->base_address + (1 * md->register_stride);
hm2->resolver.status_addr = md->base_address + (2 * md->register_stride);
hm2->resolver.velocity_addr = md->base_address + (3 * md->register_stride);
hm2->resolver.position_addr = md->base_address + (4 * md->register_stride);
// If there were multiple resolver function instances, this would need
// to be the number of resolvers for that particular instance
r = hm2_register_tram_read_region(hm2, hm2->resolver.status_addr,
sizeof(u32),
&hm2->resolver.status_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.position_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
&hm2->resolver.position_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.velocity_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
(u32**)&hm2->resolver.velocity_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for Resolver "
"register (%d)\n", i);
goto fail1;
}
}
// export the resolvers to HAL
{
int i;
int ret;
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.resolver.excitation-khz",
hm2->llio->name);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.hal->pin.excitation_khz),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.position",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.position),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.angle",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.angle),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.velocity),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.count",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.count),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.rawcounts",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.rawcounts),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.reset",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IN,
&(hm2->resolver.instance[i].hal.pin.reset),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-enable",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.index_enable),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.error",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.error),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity-scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.vel_scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-divisor",
hm2->llio->name, i);
ret= hal_pin_u32_new(name, HAL_RW,
&(hm2->resolver.instance[i].hal.pin.index_div),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
//
// init the hal objects that need it
// the things not initialized here will be set by hm2_resolver_tram_init()
//
*hm2->resolver.instance[i].hal.pin.reset = 0;
*hm2->resolver.instance[i].hal.pin.scale = 1.0;
*hm2->resolver.instance[i].hal.pin.vel_scale = 1.0;
*hm2->resolver.instance[i].hal.pin.index_div = 1;
*hm2->resolver.hal->pin.excitation_khz = -1; // don't-write
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
}
}
return hm2->resolver.num_instances;
fail1:
// This is where we would kfree anything kmalloced.
fail0:
hm2->resolver.num_instances = 0;
return r;
}
static int export_freqgen(int num, freqgen_t * addr, int step_type)
{
int n, retval, msg;
char buf[HAL_NAME_LEN + 2];
/* This function exports a lot of stuff, which results in a lot of
logging if msg_level is at INFO or ALL. So we save the current value
of msg_level and restore it later. If you actually need to log this
function's actions, change the second line below */
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export param variable for raw counts */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.rawcounts", num);
retval = hal_param_s32_new(buf, HAL_RO, &(addr->rawcount), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->count), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-fb", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->pos), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for position scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->pos_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for frequency command */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->vel), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for frequency scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->vel_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max frequency */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxfreq", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxfreq), comp_id);
if (retval != 0) {
return retval;
}
/* export param for scaled velocity (frequency in Hz) */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.frequency", num);
retval = hal_param_float_new(buf, HAL_RO, &(addr->freq), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max accel/decel */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxaccel", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxaccel), comp_id);
if (retval != 0) {
return retval;
}
/* set default parameter values */
addr->pos_scale = 1.0;
addr->vel_scale = 1.0;
/* set maxfreq very high - let update_freq() fix it */
addr->maxfreq = 1e15;
addr->maxaccel = 0.0;
addr->wd.st0.step_type = step_type;
/* init the step generator core to zero output */
addr->accum = 0;
addr->addval = 0;
addr->newaddval = 0;
addr->deltalim = 0;
addr->rawcount = 0;
addr->printed_error = 0;
if (step_type == 0) {
/* setup for stepping type 0 - step/dir */
addr->wd.st0.need_step = 0;
addr->wd.st0.setup_timer = 0;
addr->wd.st0.hold_timer = 0;
addr->wd.st0.space_timer = 0;
addr->wd.st0.len_timer = 0;
/* export parameters for step/dir pulse timing */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirsetup", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_setup), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirhold", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_hold), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.steplen", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_len), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.stepspace", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_space),
comp_id);
if (retval != 0) {
return retval;
}
/* init the parameters */
addr->wd.st0.dir_setup = 1;
addr->wd.st0.dir_hold = 1;
addr->wd.st0.step_len = 1;
addr->wd.st0.step_space = 1;
/* export pins for step and direction */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.step", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[STEP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[STEP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dir", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DIR_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DIR_PIN]) = 0;
} else if (step_type == 1) {
/* setup for stepping type 1 - pseudo-PWM */
/* export pins for up and down */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.up", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[UP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[UP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.down", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DOWN_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DOWN_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.count", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[COUNT_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[COUNT_PIN]) = 0;
} else {
/* setup for stepping types 2 and higher */
addr->wd.st2.state = 0;
addr->wd.st2.cycle_max = cycle_len_lut[step_type - 2] - 1;
addr->wd.st2.num_phases = num_phases_lut[step_type - 2];
addr->wd.st2.lut = &(master_lut[step_type - 2][0]);
/* export pins for output phases */
for (n = 0; n < addr->wd.st2.num_phases; n++) {
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.phase-%c", num,
n + 'A');
retval = hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[n]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[n]) = 0;
}
}
/* set initial pin values */
*(addr->count) = 0;
*(addr->pos) = 0.0;
*(addr->vel) = 0.0;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr)
{
int retval, i, w;
char buf[HAL_NAME_LEN+1], base[HAL_NAME_LEN+1];
rtapi_snprintf(base, sizeof(base), "wsum.%d", num);
/* export pin for offset (input) */
rtapi_snprintf(buf, sizeof(buf), "%s.offset", base);
retval = hal_pin_s32_new(buf, HAL_IO, &(addr->offset), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
return retval;
}
/* export pin for output sum */
rtapi_snprintf(buf, sizeof(buf), "%s.sum", base);
retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->sum), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
/* export pin for update hold */
rtapi_snprintf(buf, sizeof(buf), "%s.hold", base);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->hold), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
addr->bits = bitaddr;
addr->num_bits = num_bits;
/* export the input bits and weight parameters, and set the default weights */
w = 1;
for (i=0;i<num_bits;i++) {
rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.in", base, i);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->bits[i].bit), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
return retval;
}
rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.weight", base, i);
retval = hal_pin_s32_new(buf, HAL_IO, &(addr->bits[i].weight), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
return retval;
}
*(addr->bits[i].weight) = w;
w <<= 1;
}
/* set initial parameter and pin values */
*(addr->offset) = 0;
*(addr->sum) = 0;
return 0;
}
