int rtapi_app_main(void)
{
char name[HAL_NAME_LEN + 2];
int n, retval;
/* only one port at the moment */
num_ports = 1;
n = 0; /* port number */
/* STEP 1: initialise the driver */
comp_id = hal_init("hal_skeleton");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: hal_init() failed\n");
return -1;
}
/* STEP 2: allocate shared memory for skeleton data */
port_data_array = hal_malloc(num_ports * sizeof(skeleton_t));
if (port_data_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* STEP 3: export the pin(s) */
rtapi_snprintf(name, HAL_NAME_LEN, "skeleton.%d.pin-%02d-out", n, 1);
retval =
hal_pin_u32_new(name, HAL_IN, &(port_data_array->data_out), comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: port %d var export failed with err=%i\n", n,
retval);
hal_exit(comp_id);
return -1;
}
/* STEP 4: export write function */
rtapi_snprintf(name, HAL_NAME_LEN, "skeleton.%d.write", n);
retval =
hal_export_funct(name, write_port, &(port_data_array[n]), 0, 0,
comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: port %d write funct export failed\n", n);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SKELETON: installed driver for %d ports\n", num_ports);
hal_ready(comp_id);
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);
}
}
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_encoder_pair(int num, encoder_pair_t * 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 pins for the quadrature inputs */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-A", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_A), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-B", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_B), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-A", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_A), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-B", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_B), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for the enable input */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.enable", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->enable), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for output */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.error", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->error), comp_id);
if (retval != 0) {
return retval;
}
/* export pins for config info() */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-ppr", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_ppr), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-ppr", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_ppr), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-teeth", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_teeth), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-teeth", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_teeth), comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
