static int export_dac(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 voltage received by the board() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-value", num);
retval = hal_pin_float_new(buf, HAL_IN, &addr->dac_value[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for offset */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-offset", num);
retval =
hal_param_float_new(buf, HAL_RW, &addr->dac_offset[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for gain */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-gain", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->dac_gain[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
/***********************************************************************
* 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;
}
static int export_dio_pins(int io_points)
{
int i, msg, retval=0;
/* 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 */
if (io_points == 0) return 0;
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
for (i = 0; i < io_points; i++) {
/* point, direction, driver */
retval |= export_pin(i, vti_driver->dir_bits[i / 4], vti_driver);
}
/* restore saved message level */ rtapi_set_msg_level(msg);
return retval;
}
int _ulapi_init(const char *modname) {
if (ulapi_load(&rtapi_switch) < 0) {
return -ENOSYS;
}
rtapi_print_msg(RTAPI_MSG_DBG,
"_ulapi_init(): ulapi %s %s loaded\n",
flavor->name, rtapi_switch->git_version);
// switch logging level to what was set in global via msgd:
rtapi_set_msg_level(global_data->user_msg_level);
// and return what was intended to start with
return rtapi_init(modname);
}
int rtapi_app_main(void)
{
int retval;
dlist_init_entry(&head);
if ((comp_id = hal_xinit(TYPE_RT, 0, 0,
instantiate_encoder_pair,
delete_encoder_pair,
compname)) < 0)
return comp_id;
hal_export_xfunct_args_t u = {
.type = FS_XTHREADFUNC,
.funct.x = update,
.arg = &head,
.uses_fp = 1,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&u, "%s.update",
prefix)) < 0)
return retval;
hal_export_xfunct_args_t mp = {
.type = FS_XTHREADFUNC,
.funct.x = sample,
.arg = &head,
.uses_fp = 0,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&mp, "%s.sample",
prefix)) < 0)
return retval;
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}
static int instantiate_encoder_pair(const int argc, const char**argv)
{
encoder_pair_t *p;
int retval;
int msg;
const char* name;
if(argc >= 2)
name = argv[1];
else
HALFAIL_RC(EINVAL, "ERROR: insufficient args in argv");
/* 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. */
msg = rtapi_get_msg_level();
#ifndef VERBOSE_SETUP
rtapi_set_msg_level(RTAPI_MSG_WARN);
#endif
if ((retval = hal_inst_create(name, comp_id, sizeof(encoder_pair_t), (void **)&p)) < 0)
return retval;
p->inst_id = retval;
if ((retval = export_encoder_pair(name, p->inst_id, p)) != 0)
HALFAIL_RC(retval, "%s: ERROR: export(%s) failed", compname, name);
// append to instance list
dlist_init_entry(&p->list);
dlist_add_after(&p->list, &head);
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_counter(int num, counter_t * addr)
{
int retval, msg;
/* 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 */
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseA), comp_id,
"encoder.%d.phase-A", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseB), comp_id,
"encoder.%d.phase-B", num);
if (retval != 0) {
return retval;
}
/* export pin for the index input */
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseZ), comp_id,
"encoder.%d.phase-Z", num);
if (retval != 0) {
return retval;
}
/* export pin for the index enable input */
retval = hal_pin_bit_newf(HAL_IO, &(addr->index_ena), comp_id,
"encoder.%d.index-enable", num);
if (retval != 0) {
return retval;
}
/* export pin for the reset input */
retval = hal_pin_bit_newf(HAL_IN, &(addr->reset), comp_id,
"encoder.%d.reset", num);
if (retval != 0) {
return retval;
}
/* export pins for position latching */
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_in), comp_id,
"encoder.%d.latch-input", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_rising), comp_id,
"encoder.%d.latch-rising", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_falling), comp_id,
"encoder.%d.latch-falling", num);
if (retval != 0) {
return retval;
}
/* export parameter for raw counts */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->raw_counts), comp_id,
"encoder.%d.rawcounts", num);
if (retval != 0) {
return retval;
}
/* export pin for counts captured by capture() */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->count), comp_id,
"encoder.%d.counts", num);
if (retval != 0) {
return retval;
}
/* export pin for counts latched by capture() */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->count_latch), comp_id,
"encoder.%d.counts-latched", num);
if (retval != 0) {
return retval;
}
/* export pin for minimum speed estimated by capture() */
retval = hal_pin_float_newf(HAL_IN, &(addr->min_speed), comp_id,
"encoder.%d.min-speed-estimate", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos), comp_id,
"encoder.%d.position", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled and interpolated position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_interp), comp_id,
"encoder.%d.position-interpolated", num);
if (retval != 0) {
return retval;
}
/* export pin for latched position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_latch), comp_id,
"encoder.%d.position-latched", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled velocity captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->vel), comp_id,
"encoder.%d.velocity", num);
if (retval != 0) {
return retval;
}
/* export pin for scaling */
retval = hal_pin_float_newf(HAL_IO, &(addr->pos_scale), comp_id,
"encoder.%d.position-scale", num);
if (retval != 0) {
return retval;
}
/* export pin for x4 mode */
retval = hal_pin_bit_newf(HAL_IO, &(addr->x4_mode), comp_id,
"encoder.%d.x4-mode", num);
if (retval != 0) {
return retval;
}
/* export pin for counter mode */
retval = hal_pin_bit_newf(HAL_IO, &(addr->counter_mode), comp_id,
"encoder.%d.counter-mode", num);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int ulapi_load(rtapi_switch_t **ulapi_switch)
{
int retval;
const char *errmsg;
rtapi_get_handle_t rtapi_get_handle;
char ulapi_lib_fname[PATH_MAX];
char *instance = getenv("INSTANCE");
char *debug_env = getenv("ULAPI_DEBUG");
int size = 0;
int globalkey;
// set the rtapi_instance global for this hal library instance
if (instance != NULL)
rtapi_instance = atoi(instance);
if (debug_env)
ulapi_debug = atoi(debug_env);
rtapi_set_msg_level(ulapi_debug);
// tag message origin field
rtapi_set_logtag("ulapi");
// first thing is to attach the global segment, based on
// the RTAPI instance id. This will contain the flavor
// this ULAPI HAL instance is to run with.
// Also, it's the prerequisite for common error message
// handling through the message ringbuffer; unless then
// error messages will go to stderr.
// the global segment is attached once here per ULAPI instance;
// it's address is passed to the ulapi-<flavor>.so module once loaded.
// init the common shared memory driver APU
shm_common_init();
globalkey = OS_KEY(GLOBAL_KEY, rtapi_instance);
retval = shm_common_new(globalkey, &size,
rtapi_instance, (void **) &global_data, 0);
if (retval == -ENOENT) {
// the global_data segment does not exist. Happens if the realtime
// script was not started
rtapi_print_msg(RTAPI_MSG_ERR,
"ULAPI:%d ERROR: realtime not started\n",
rtapi_instance);
return retval;
}
if (retval < 0) {
// some other error attaching global
rtapi_print_msg(RTAPI_MSG_ERR,
"ULAPI:%d ERROR: shm_common_new() failed key=0x%x %s\n",
rtapi_instance, globalkey, strerror(-retval));
return retval;
}
if (size != sizeof(global_data_t)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ULAPI:%d ERROR: global segment size mismatch,"
" expected: %zd, actual:%d\n",
rtapi_instance, sizeof(global_data_t), size);
return -EINVAL;
}
if (global_data->magic != GLOBAL_READY) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ULAPI:%d ERROR: global segment invalid magic:"
" expected: 0x%x, actual: 0x%x\n",
rtapi_instance, GLOBAL_READY,
global_data->magic);
return -EINVAL;
}
// global data set up ok
// obtain handle on flavor descriptor as detected by rtapi_msgd
flavor = flavor_byid(global_data->rtapi_thread_flavor);
if (flavor == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d BUG - invalid flavor id: %d\n",
rtapi_instance, global_data->rtapi_thread_flavor);
return -EINVAL;
}
snprintf(ulapi_lib_fname,PATH_MAX,"%s/%s-%s%s",
EMC2_RTLIB_DIR, ulapi_lib, flavor->name, flavor->so_ext);
// dynload the proper ulapi.so:
if ((ulapi_so = dlopen(ulapi_lib_fname, RTLD_GLOBAL|RTLD_NOW)) == NULL) {
errmsg = dlerror();
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d FATAL - dlopen(%s) failed: %s\n",
rtapi_instance, ulapi_lib_fname,
errmsg ? errmsg : "NULL");
return -ENOENT;
}
// resolve rtapi_switch getter function
dlerror();
if ((rtapi_get_handle = (rtapi_get_handle_t)
dlsym(ulapi_so, "rtapi_get_handle")) == NULL) {
errmsg = dlerror();
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d FATAL - resolving %s: cant"
" dlsym(rtapi_get_handle): %s\n",
rtapi_instance, ulapi_lib, errmsg ? errmsg : "NULL");
return -ENOENT;
}
assert(rtapi_get_handle != NULL);
// this redirects calls to rtapi through the just-loaded ulapi
*ulapi_switch = rtapi_get_handle();
// from here on it is safe to call all RTAPI functions (i.e. including those
// which go through rtapi_switch)
// resolve main function
dlerror();
if ((ulapi_main_ref = (ulapi_main_t)
dlsym(ulapi_so, "ulapi_main")) == NULL) {
errmsg = dlerror();
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d FATAL - resolving %s: "
"cant dlsym(ulapi_main): %s\n",
rtapi_instance, ulapi_lib, errmsg ? errmsg : "NULL");
return -ENOENT;
}
// resolve exit function
dlerror();
if ((ulapi_exit_ref = (ulapi_exit_t)
dlsym(ulapi_so, "ulapi_exit")) == NULL) {
errmsg = dlerror();
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB: FATAL - resolving %s:"
" cant dlsym(ulapi_exit): %s\n",
ulapi_lib, errmsg ? errmsg : "NULL");
return -ENOENT;
}
assert(ulapi_main_ref != NULL);
assert(ulapi_exit_ref != NULL);
// call the ulapi init method, passing in the global segment
if ((retval = ulapi_main_ref(rtapi_instance,
flavor->id, global_data)) < 0) {
// check shmdrv, permissions
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_LIB: FATAL - cannot attach to instance %d"
" - realtime not started?\n",
rtapi_instance);
return -ENOENT;
}
// pretty bad - we loaded the wrong ulapi.so
if (flavor->id != rtapi_switch->thread_flavor_id) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_LIB: BUG: thread flavors disagree:"
" hal_lib.c=%d rtapi=%d\n",
flavor->id, rtapi_switch->thread_flavor_id);
}
// sanity check - may be harmless
if (strcmp(GIT_VERSION, rtapi_switch->git_version)) {
rtapi_print_msg(RTAPI_MSG_WARN,
"HAL_LIB: UP API warning - git versions disagree:"
" hal_lib.c=%s %s=%s\n",
GIT_VERSION, ulapi_lib, rtapi_switch->git_version);
}
// declare victory
return 0;
}
static int export_joint(int num, joint_hal_t * addr)
{
int retval, msg;
/* 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 joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "axis.%d.joint-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "axis.%d.joint-pos-fb", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "axis.%d.motor-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "axis.%d.motor-offset", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "axis.%d.motor-pos-fb", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "axis.%d.pos-lim-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "axis.%d.neg-lim-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "axis.%d.home-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "axis.%d.index-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "axis.%d.amp-enable-out", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "axis.%d.amp-fault-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "axis.%d.jog-counts", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "axis.%d.jog-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "axis.%d.jog-scale", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_vel_mode), mot_comp_id, "axis.%d.jog-vel-mode", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "axis.%d.homing", num);
if (retval != 0) {
return retval;
}
/* export joint parameters */ //FIXME-AJ: changing these to joints will break configs.
retval =
hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd),
mot_comp_id, "axis.%d.coarse-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "axis.%d.joint-vel-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "axis.%d.backlash-corr", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "axis.%d.backlash-filt", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "axis.%d.backlash-vel", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "axis.%d.f-error", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "axis.%d.f-error-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "axis.%d.free-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "axis.%d.free-vel-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "axis.%d.free-tp-enable", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "axis.%d.kb-jog-active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "axis.%d.wheel-jog-active", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "axis.%d.active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "axis.%d.in-position", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "axis.%d.error", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "axis.%d.pos-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "axis.%d.neg-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "axis.%d.homed", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "axis.%d.f-errored", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "axis.%d.faulted", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "axis.%d.home-state", num);
if (retval != 0) {
return retval;
}
if(num >=3 && num <= 5) {
// for rotaries only...
retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "axis.%d.unlock", num);
if (retval != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "axis.%d.is-unlocked", num);
if (retval != 0) return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
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_sim_enc(sim_enc_t * addr, char *prefix)
{
int retval, msg;
/* 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 pulses per rev */
retval = hal_pin_u32_newf(HAL_IO, &(addr->ppr), comp_id,
"%s.ppr", prefix);
if (retval != 0) {
return retval;
}
/* export param variable for scaling */
retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id,
"%s.scale", prefix);
if (retval != 0) {
return retval;
}
/* export pin for speed command */
retval = hal_pin_float_newf(HAL_IN, &(addr->speed), comp_id,
"%s.speed", prefix);
if (retval != 0) {
return retval;
}
/* export pins for output phases */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseA), comp_id,
"%s.phase-A", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseB), comp_id,
"%s.phase-B", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseZ), comp_id,
"%s.phase-Z", prefix);
if (retval != 0) {
return retval;
}
/* export pin for rawcounts */
retval = hal_pin_s32_newf(HAL_IN, &(addr->rawcounts), comp_id,
"%s.rawcounts", prefix);
if (retval != 0) {
return retval;
}
/* init parameters */
*(addr->ppr) = 100;
*(addr->scale) = 1.0;
/* init internal vars */
addr->old_scale = 0.0;
addr->scale_mult = 1.0;
/* init the state variables */
addr->accum = 0;
addr->addval = 0;
addr->state = 0;
addr->cycle = 0;
/* init the outputs */
*(addr->phaseA) = 0;
*(addr->phaseB) = 0;
*(addr->phaseZ) = 0;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
int main(int argc, char **argv)
{
int c, fd;
int keep_going, retval, errorcount;
int filemode = 0;
char *filename = NULL;
FILE *srcfile = NULL;
char raw_buf[MAX_CMD_LEN+1];
int linenumber = 1;
char *cf=NULL, *cw=NULL, *cl=NULL;
if (argc < 2) {
/* no args specified, print help */
print_help_general(0);
exit(0);
}
/* set default level of output - 'quiet' */
rtapi_set_msg_level(RTAPI_MSG_ERR);
/* set default for other options */
keep_going = 0;
/* start parsing the command line, options first */
while(1) {
c = getopt(argc, argv, "+RCfi:kqQsvVhe");
if(c == -1) break;
switch(c) {
case 'R':
/* force an unlock of the HAL mutex - to be used after a segfault in a hal program */
if (release_HAL_mutex() < 0) {
printf("HALCMD: Release Mutex failed!\n");
return 1;
}
return 0;
break;
case 'h':
/* -h = help */
if (argc > optind) { /* there are more arguments */
do_help_cmd(argv[optind]);
} else {
print_help_general(1);
}
return 0;
break;
case 'k':
/* -k = keep going */
keep_going = 1;
break;
case 'q':
/* -q = quiet (default) */
rtapi_set_msg_level(RTAPI_MSG_ERR);
break;
case 'Q':
/* -Q = very quiet */
rtapi_set_msg_level(RTAPI_MSG_NONE);
break;
case 's':
/* script friendly mode */
scriptmode = 1;
break;
case 'v':
/* -v = verbose */
rtapi_set_msg_level(RTAPI_MSG_INFO);
break;
case 'V':
/* -V = very verbose */
rtapi_set_msg_level(RTAPI_MSG_ALL);
break;
case 'e':
echo_mode = 1;
break;
case 'f':
filemode = 1;
break;
case 'C':
cl = getenv("COMP_LINE");
cw = getenv("COMP_POINT");
if (!cl || !cw) exit(0);
c = atoi(cw)-strlen(argv[0])-1;
if (c<0) c=0;
cl += strlen(argv[0])+1;
if (c>0 && cl[c]=='\0') c--; // if at end of line, back up one char
cf=&cl[c];
{
int n;
for (n=c; n>0; n--, cf--) {
if (isspace(*cf) || *cf == '=' || *cf == '<' || *cf == '>') {
cf++;
break;
}
}
halcmd_startup(1);
propose_completion(cl, cf, n);
}
if (comp_id >= 0) halcmd_shutdown();
exit(0);
break;
#ifndef NO_INI
case 'i':
/* -i = allow reading 'setp' values from an ini file */
if (halcmd_inifile == NULL) {
/* it's the first -i (ignore repeats) */
/* there is a following arg, and it's not an option */
filename = optarg;
halcmd_inifile = fopen(filename, "r");
if (halcmd_inifile == NULL) {
fprintf(stderr,
"Could not open ini file '%s'\n",
filename);
exit(-1);
}
/* make sure file is closed on exec() */
fd = fileno(halcmd_inifile);
fcntl(fd, F_SETFD, FD_CLOEXEC);
}
break;
#endif /* NO_INI */
case '?':
/* option not in getopt() list
getopt already printed an error message */
exit(-1);
break;
default:
/* option in getopt list but not in switch statement - bug */
printf("Unimplemented option '-%c'\n", c);
exit(-1);
break;
}
}
if(filemode) {
/* it's the first -f (ignore repeats) */
if (argc > optind) {
/* there is a following arg, and it's not an option */
filename = argv[optind++];
srcfile = fopen(filename, "r");
halcmd_set_filename(filename);
if (srcfile == NULL) {
fprintf(stderr,
"Could not open command file '%s'\n",
filename);
exit(-1);
}
/* make sure file is closed on exec() */
fd = fileno(srcfile);
fcntl(fd, F_SETFD, FD_CLOEXEC);
} else {
halcmd_set_filename("<stdin>");
/* no filename followed -f option, use stdin */
srcfile = stdin;
prompt_mode = 1;
}
}
if ( halcmd_startup(0) != 0 ) return 1;
errorcount = 0;
/* HAL init is OK, let's process the command(s) */
if (srcfile == NULL) {
#ifndef NO_INI
if(halcmd_inifile) {
fprintf(stderr, "-i may only be used together with -f\n");
errorcount++;
}
#endif
if(errorcount == 0 && argc > optind) {
halcmd_set_filename("<commandline>");
halcmd_set_linenumber(0);
retval = halcmd_parse_cmd(&argv[optind]);
if (retval != 0) {
errorcount++;
}
}
} else {
/* read command line(s) from 'srcfile' */
while (get_input(srcfile, raw_buf, MAX_CMD_LEN)) {
char *tokens[MAX_TOK+1];
halcmd_set_linenumber(linenumber++);
/* remove comments, do var substitution, and tokenise */
retval = halcmd_preprocess_line(raw_buf, tokens);
if(echo_mode) {
halcmd_echo("%s\n", raw_buf);
}
if (retval == 0) {
/* the "quit" command is not handled by parse_line() */
if ( ( strcasecmp(tokens[0],"quit") == 0 ) ||
( strcasecmp(tokens[0],"exit") == 0 ) ) {
break;
}
/* process command */
retval = halcmd_parse_cmd(tokens);
}
/* did a signal happen while we were busy? */
if ( halcmd_done ) {
/* treat it as an error */
errorcount++;
/* exit from loop */
break;
}
if ( retval != 0 ) {
errorcount++;
}
if (( errorcount > 0 ) && ( keep_going == 0 )) {
/* exit from loop */
break;
}
}
}
/* all done */
halcmd_shutdown();
if ( errorcount > 0 ) {
return 1;
} else {
return 0;
}
}
static int export_port(int portnum, parport_t * port)
{
int retval, msg;
/* 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);
retval = 0;
/* declare input pins (status port) */
retval += export_input_pin(portnum, 15, port->status_in, 0);
retval += export_input_pin(portnum, 13, port->status_in, 1);
retval += export_input_pin(portnum, 12, port->status_in, 2);
retval += export_input_pin(portnum, 10, port->status_in, 3);
retval += export_input_pin(portnum, 11, port->status_in, 4);
if (port->data_dir != 0) {
/* declare input pins (data port) */
retval += export_input_pin(portnum, 2, port->data_in, 0);
retval += export_input_pin(portnum, 3, port->data_in, 1);
retval += export_input_pin(portnum, 4, port->data_in, 2);
retval += export_input_pin(portnum, 5, port->data_in, 3);
retval += export_input_pin(portnum, 6, port->data_in, 4);
retval += export_input_pin(portnum, 7, port->data_in, 5);
retval += export_input_pin(portnum, 8, port->data_in, 6);
retval += export_input_pin(portnum, 9, port->data_in, 7);
} else {
/* declare output pins (data port) */
retval += export_output_pin(portnum, 2,
port->data_out, port->data_inv, port->data_reset, 0);
retval += export_output_pin(portnum, 3,
port->data_out, port->data_inv, port->data_reset, 1);
retval += export_output_pin(portnum, 4,
port->data_out, port->data_inv, port->data_reset, 2);
retval += export_output_pin(portnum, 5,
port->data_out, port->data_inv, port->data_reset, 3);
retval += export_output_pin(portnum, 6,
port->data_out, port->data_inv, port->data_reset, 4);
retval += export_output_pin(portnum, 7,
port->data_out, port->data_inv, port->data_reset, 5);
retval += export_output_pin(portnum, 8,
port->data_out, port->data_inv, port->data_reset, 6);
retval += export_output_pin(portnum, 9,
port->data_out, port->data_inv, port->data_reset, 7);
retval += hal_param_u32_newf(HAL_RW, &port->reset_time, comp_id,
"parport.%d.reset-time", portnum);
retval += hal_param_u32_newf(HAL_RW, &port->debug1, comp_id,
"parport.%d.debug1", portnum);
retval += hal_param_u32_newf(HAL_RW, &port->debug2, comp_id,
"parport.%d.debug2", portnum);
port->write_time = 0;
}
if(port->use_control_in == 0) {
/* declare output variables (control port) */
retval += export_output_pin(portnum, 1,
port->control_out, port->control_inv, port->control_reset, 0);
retval += export_output_pin(portnum, 14,
port->control_out, port->control_inv, port->control_reset, 1);
retval += export_output_pin(portnum, 16,
port->control_out, port->control_inv, port->control_reset, 2);
retval += export_output_pin(portnum, 17,
port->control_out, port->control_inv, port->control_reset, 3);
} else {
/* declare input variables (control port) */
retval += export_input_pin(portnum, 1, port->control_in, 0);
retval += export_input_pin(portnum, 14, port->control_in, 1);
retval += export_input_pin(portnum, 16, port->control_in, 2);
retval += export_input_pin(portnum, 17, port->control_in, 3);
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return retval;
}
static int export_pwmgen(int num, pwmgen_t * addr, int output_type)
{
int retval, msg;
/* 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 paramameters */
retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id,
"pwmgen.%d.scale", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->offset), comp_id,
"pwmgen.%d.offset", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IO, &(addr->dither_pwm), comp_id,
"pwmgen.%d.dither-pwm", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->pwm_freq), comp_id,
"pwmgen.%d.pwm-freq", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->min_dc), comp_id,
"pwmgen.%d.min-dc", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->max_dc), comp_id,
"pwmgen.%d.max-dc", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->curr_dc), comp_id,
"pwmgen.%d.curr-dc", num);
if (retval != 0) {
return retval;
}
/* export pins */
retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id,
"pwmgen.%d.enable", num);
if (retval != 0) {
return retval;
}
*(addr->enable) = 0;
retval = hal_pin_float_newf(HAL_IN, &(addr->value), comp_id,
"pwmgen.%d.value", num);
if (retval != 0) {
return retval;
}
*(addr->value) = 0.0;
if (output_type == 2) {
/* export UP/DOWN pins */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[UP_PIN]), comp_id,
"pwmgen.%d.up", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[UP_PIN]) = 0;
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DOWN_PIN]), comp_id,
"pwmgen.%d.down", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[DOWN_PIN]) = 0;
} else {
/* export PWM pin */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[PWM_PIN]), comp_id,
"pwmgen.%d.pwm", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[PWM_PIN]) = 0;
if ( output_type == 1 ) {
/* export DIR pin */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DIR_PIN]), comp_id,
"pwmgen.%d.dir", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[DIR_PIN]) = 0;
}
}
/* set default pin values */
*(addr->scale) = 1.0;
*(addr->offset) = 0.0;
*(addr->dither_pwm) = 0;
*(addr->pwm_freq) = 0;
*(addr->min_dc) = 0.0;
*(addr->max_dc) = 1.0;
*(addr->curr_dc) = 0.0;
/* init other fields */
addr->period = 50000;
addr->high_time = 0;
addr->period_timer = 0;
addr->high_timer = 0;
addr->curr_output = 0;
addr->output_type = output_type;
addr->pwm_mode = PWM_DISABLED;
addr->direction = 0;
addr->old_scale = *(addr->scale) + 1.0;
addr->old_pwm_freq = -1;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
void process_options (int argc, char *argv[])
{
int error = 0;
for (;;)
{
int option_index = 0;
static const char *short_options = "c:";
static const struct option long_options[] = {
{"nogui", no_argument, 0, 'n'},
{"config", required_argument, 0, 'c'},
{"modmaster",no_argument,0,'m'},
{"modslave",no_argument,0,'s'},
{"debug",no_argument,0,'d'},
{"modbus_port", required_argument, 0, 'p'},
{"newpath", required_argument, 0, 'f'},
{0, 0, 0, 0},
};
int c = getopt_long(argc, argv, short_options,
long_options, &option_index);
if (c == EOF)
break;
switch (c)
{
case 'n':
nogui = 1;
break;
case 'c':
read_config (optarg);
modmaster=1;
break;
case 'm':
modmaster=1;
break;
case 's':
modslave=1;
break;
case 'd':
rtapi_set_msg_level(RTAPI_MSG_ALL);
break;
case 'p':
ModbusServerPort = atoi( optarg );
break;
case 'f':
NewPath = ( optarg );
pathswitch=1;
break;
case '?':
error = 1;
break;
}
}
if (error)
display_help ();
if ((argc - optind) != 0)
VerifyDirectorySelected (argv[optind]);
}
/**
\brief main realtime task */
int rtapi_app_main(void)
{
// zynq and FPGA code revision
int rev, zrev, n;
// save messaging level
static int msg_level;
int retval = 0;
int fdmisc;
// save message level on entering
msg_level = rtapi_get_msg_level();
/* setup messaging level in:
RTAPI_MSG_NONE, RTAPI_MSG_ERR, RTAPI_MSG_WARN,
RTAPI_MSG_INFO, RTAPI_MSG_DBG, RTAPI_MSG_ALL */
rtapi_set_msg_level(RTAPI_MSG_ALL);
// check Zynq revision
if ((zrev = zynq_revision()) < 0) {
// unable to determine zynq revision
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: unable to determine zynq revision");
hal_exit(comp_id);
return -1;
}
// notify zynq revision
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zynq Revision %d \n", zrev);
// check Zedboard FPGA hardware revision
rev = zb_revision();
// do revision specific configuration
switch (rev) {
case 01:
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zedboard FPGA Revision 01\n");
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: FPGA revision %d not (yet) supported\n", rev);
return -1;
break;
}
// open and map miscellaneous peripheral for PMOD IO access
fdmisc = open("/dev/mem", O_RDWR);
map_MiscAddress = (unsigned int) mmap(NULL, 100, PROT_READ | PROT_WRITE, MAP_SHARED, fdmisc, (off_t)MISC_ADDR );
if (-1 == map_MiscAddress ) {
fprintf(stderr, "MMap failed to map Miscellaneus peripheral\n");
return 0;
}
printf("Map Misc peripheral: virtual 0x%x real 0x%x \n", map_MiscAddress, MISC_ADDR);
// parse module parameters in order to find the number
// of configured FOC channels and their CAN address/configuration
for(n = 0; n < MAX_FOC_CHAN && FOC_axis[n] != -1 ; n++) {
// check for a valid CAN address
if( (FOC_axis[n] <= 0) || ( FOC_axis[n] > 15) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: bad CAN address '%i', axis %i", FOC_axis[n], n);
hal_exit(comp_id);
return -1;
}
// check the control mode configuration
if( INVALID == parse_ctrl_type(ctrl_type[n])) {
rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: ERROR: bad control type '%s' for axis %i ('c','j','v','i','t')",
ctrl_type[n], n);
hal_exit(comp_id);
return -1;
}
// found a correctly configured channel
num_chan++;
// notify
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC axis %d with CAN address %d.",n, FOC_axis[n] );
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Motor gear %d, Screw gear %d, Screw ratio %d Encoder PPR %d.", Screw_gear[n], Motor_gear[n], Screw_ratio[n], PPR[n]);
}
if( (0 == num_chan) || (8 < num_chan) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: channels configured incorrectly.");
hal_exit(comp_id);
return -1;
}
// allocate shared memory for FOC_data of each axis
FOC_data_array = hal_malloc(num_chan * sizeof(FOC_data_t));
if ( 0 == FOC_data_array ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/** \note CAN8/2FOC connection is 1 axis for each CAN channel */
// configure CAN communication
for(n = 0; n < num_chan ; n++) {
if ( 0 != setup_CAN(n) ) {
hal_exit(comp_id);
return -1;
}
}
// try to init the component
comp_id = hal_init("hal_zed_can");
if( comp_id < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_init() failed\n");
hal_exit(comp_id);
return -1;
}
/* Export the variables/parameters for each FOC axis */
for (n = 0; n < num_chan; n++) {
// export pins/params for the n^th component
retval = exportFOCaxis(n, &(FOC_data_array[n]) );
if( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: exportFOCaxis() failed");
hal_exit(comp_id);
return -1;
}
}
// export the read_feedback and send_setpoint functions as hal_zed_can.send_setpoint and hal_zed_can.read_feedback in hal
retval = hal_export_funct("hal_zed_can.send_setpoint", send_setpoint, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: write funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_zed_can.read_feedback", read_feedback, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: read funct export failed\n");
hal_exit(comp_id);
return -1;
}
// activate periodic communication on all axis
setup_2FOC_periodic();
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC periodic data exchange active.");
// all operations succeded
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: driver installed successfully.\n");
hal_ready(comp_id);
// return to previous mesaging level
rtapi_set_msg_level(msg_level);
return 0;
}
int main( int argc, char *argv[] )
{
int used=0, NumRung;
static int old_level ;
bindtextdomain("linuxcnc", EMC2_PO_DIR);
setlocale(LC_MESSAGES,"");
setlocale(LC_CTYPE,"");
textdomain("linuxcnc");
old_level = rtapi_get_msg_level();
compId=hal_init("classicladder"); //emc
if (compId<0) return -1; //emc
signal(SIGTERM,do_exit); //emc
InitModbusMasterBeforeReadConf( );
if (ClassicLadder_AllocAll())
{
char ProjectLoadedOk=TRUE;
process_options (argc, argv);
if (nogui==TRUE)
{
rtapi_print(_("INFO CLASSICLADDER- No ladder GUI requested-Realtime runs till HAL closes.\n"));
ClassicLadder_InitAllDatas( );
ProjectLoadedOk = LoadProjectFiles( InfosGene->CurrentProjectFileName );
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
InfosGene->LadderState = STATE_RUN;
ClassicLadder_FreeAll(TRUE);
hal_ready(compId);
hal_exit(compId);
return 0;
} else {
for(NumRung=0;NumRung<NBR_RUNGS;NumRung++) { if ( RungArray[NumRung].Used ) used++; }
if((used==0) || ( (argc - optind) != 0) )
{
ClassicLadder_InitAllDatas( );
ProjectLoadedOk = LoadProjectFiles( InfosGene->CurrentProjectFileName );
InitGtkWindows( argc, argv );
UpdateAllGtkWindows();
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
UpdateWindowTitleWithProjectName( );
MessageInStatusBar( ProjectLoadedOk?_("Project loaded and running"):_("Project failed to load..."));
if (!ProjectLoadedOk)
{
ClassicLadder_InitAllDatas( );
if (modmaster) { PrepareModbusMaster( ); }
}
}else{
InitGtkWindows( argc, argv );
UpdateAllGtkWindows();
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
UpdateWindowTitleWithProjectName( );
MessageInStatusBar(_("GUI reloaded with existing ladder program"));
if (modmaster) { PrepareModbusMaster( ); }
}
if (modslave) { InitSocketServer( 0/*UseUdpMode*/, ModbusServerPort/*PortNbr*/); }
InfosGene->LadderState = STATE_RUN;
hal_ready(compId);
gtk_main();
rtapi_print(_("INFO CLASSICLADDER- Ladder GUI closed. Realtime runs till HAL closes\n"));
ClassicLadder_FreeAll(TRUE);
hal_exit(compId);
return 0;
}
}
rtapi_print(_("ERROR CLASSICLADDER- Ladder memory allocation error\n"));
ClassicLadder_FreeAll(TRUE);
rtapi_set_msg_level(old_level);
hal_exit(compId);
return 0;
}
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;
}
