// init HAL objects
static int export_halobjs(struct inst_data *ip, int owner_id, const char *name)
{
if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name))
return -1;
if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name))
return -1;
if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name))
return -1;
// unittest observer pins, per instance
if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name))
return -1;
if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name))
return -1;
// export a thread function, passing the pointer to the instance's HAL memory blob
if (hal_export_functf(funct, ip, 0, 0, owner_id, "%s.funct", name))
return -1;
return 0;
}
int rtapi_app_main(void)
{
int res = 0, i;
comp_id = hal_init("genserkins");
if (comp_id < 0)
return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata)
goto error;
for (i = 0; i < GENSER_MAX_JOINTS; i++) {
if ((res =
hal_pin_float_newf(HAL_IO, &(haldata->a[i]), comp_id,
"genserkins.A-%d", i)) < 0)
goto error;
*(haldata->a[i])=0;
if ((res =
hal_pin_float_newf(HAL_IO, &(haldata->alpha[i]), comp_id,
"genserkins.ALPHA-%d", i)) < 0)
goto error;
*(haldata->alpha[i])=0;
if ((res =
hal_pin_float_newf(HAL_IO, &(haldata->d[i]), comp_id,
"genserkins.D-%d", i)) < 0)
goto error;
*(haldata->d[i])=0;
if ((res =
hal_param_s32_newf(HAL_RW, &(haldata->unrotate[i]), comp_id,
"genserkins.unrotate-%d", i)) < 0)
goto error;
haldata->unrotate[i]=0;
}
KINS_PTR = hal_malloc(sizeof(genser_struct));
haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose));
if (KINS_PTR == NULL)
goto error;
if (haldata->pos == NULL)
goto error;
if ((res=
hal_param_s32_newf(HAL_RO, &(KINS_PTR->iterations), comp_id, "genserkins.last-iterations")) < 0)
goto error;
if ((res=
hal_param_s32_newf(HAL_RW, &(KINS_PTR->max_iterations), comp_id, "genserkins.max-iterations")) < 0)
goto error;
KINS_PTR->max_iterations = GENSER_DEFAULT_MAX_ITERATIONS;
A(0) = DEFAULT_A1;
A(1) = DEFAULT_A2;
A(2) = DEFAULT_A3;
A(3) = DEFAULT_A4;
A(4) = DEFAULT_A5;
A(5) = DEFAULT_A6;
ALPHA(0) = DEFAULT_ALPHA1;
ALPHA(1) = DEFAULT_ALPHA2;
ALPHA(2) = DEFAULT_ALPHA3;
ALPHA(3) = DEFAULT_ALPHA4;
ALPHA(4) = DEFAULT_ALPHA5;
ALPHA(5) = DEFAULT_ALPHA6;
D(0) = DEFAULT_D1;
D(1) = DEFAULT_D2;
D(2) = DEFAULT_D3;
D(3) = DEFAULT_D4;
D(4) = DEFAULT_D5;
D(5) = DEFAULT_D6;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
static int init_sampler(int num, fifo_t *tmp_fifo)
{
int size, retval, n, usefp;
void *shmem_ptr;
sampler_t *str;
pin_data_t *pptr;
fifo_t *fifo;
char buf[HAL_NAME_LEN + 2];
/* alloc shmem for base sampler data and user specified pins */
size = sizeof(sampler_t) + tmp_fifo->num_pins * sizeof(pin_data_t);
str = hal_malloc(size);
if (str == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't allocate HAL shared memory\n");
return -ENOMEM;
}
/* export "standard" pins and params */
retval = hal_pin_bit_newf(HAL_OUT, &(str->full), comp_id,
"sampler.%d.full", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'full' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
"sampler.%d.enable", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'enable' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
"sampler.%d.curr-depth", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: 'curr_depth' pin export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->overruns), comp_id,
"sampler.%d.overruns", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'overruns' parameter export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->sample_num), comp_id,
"sampler.%d.sample-num", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'sample-num' parameter export failed\n");
return -EIO;
}
/* init the standard pins and params */
*(str->full) = 0;
*(str->enable) = 1;
*(str->curr_depth) = 0;
str->overruns = 0;
str->sample_num = 0;
/* HAL pins are right after the sampler_t struct in HAL shmem */
pptr = (pin_data_t *)(str+1);
usefp = 0;
/* export user specified pins (the ones that sample data) */
for ( n = 0 ; n < tmp_fifo->num_pins ; n++ ) {
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d.pin.%d", num, n);
retval = hal_pin_new(buf, tmp_fifo->type[n], HAL_IN, (void **)pptr, comp_id );
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: pin '%s' export failed\n", buf);
return -EIO;
}
/* init the pin value */
switch ( tmp_fifo->type[n] ) {
case HAL_FLOAT:
*(pptr->hfloat) = 0.0;
usefp = 1;
break;
case HAL_BIT:
*(pptr->hbit) = 0;
break;
case HAL_U32:
*(pptr->hu32) = 0;
break;
case HAL_S32:
*(pptr->hs32) = 0;
break;
default:
break;
}
pptr++;
}
/* export update function */
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d", num);
retval = hal_export_funct(buf, sample, str, usefp, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: function export failed\n");
return retval;
}
/* alloc shmem for user/RT comms (fifo) */
size = sizeof(fifo_t) + (tmp_fifo->num_pins + 1) * tmp_fifo->depth * sizeof(shmem_data_t);
shmem_id[num] = rtapi_shmem_new(SAMPLER_SHMEM_KEY+num, comp_id, size);
if ( shmem_id[num] < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: couldn't allocate user/RT shared memory\n");
return -ENOMEM;
}
retval = rtapi_shmem_getptr(shmem_id[num], &shmem_ptr);
if ( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't map user/RT shared memory\n");
return -ENOMEM;
}
fifo = shmem_ptr;
str->fifo = fifo;
/* copy data from temp_fifo */
*fifo = *tmp_fifo;
/* init fields */
fifo->in = 0;
fifo->out = 0;
fifo->last_sample = 0;
fifo->last_sample--;
/* mark it inited for user program */
fifo->magic = FIFO_MAGIC_NUM;
return 0;
}
int main(int argc, char **argv)
{
int retval = 0;
modbus_t *mb_ctx;
haldata_t *haldata;
slavedata_t slavedata;
int slave;
int hal_comp_id;
struct timespec loop_timespec, remaining;
int baud, bits, stopbits, verbose;
char *device, *endarg;
char parity;
int opt;
int argindex, argvalue;
done = 0;
// assume that nothing is specified on the command line
baud = 38400;
bits = 8;
stopbits = 1;
verbose = 0;
device = "/dev/ttyS0";
parity = 'O';
/* slave / register info */
slave = 1;
slavedata.read_reg_start = START_REGISTER_R;
slavedata.read_reg_count = NUM_REGISTERS_R;
slavedata.write_reg_start = START_REGISTER_W;
slavedata.write_reg_count = NUM_REGISTERS_R;
// process command line options
while ((opt=getopt_long(argc, argv, option_string, long_options, NULL)) != -1) {
switch(opt) {
case 'b': // serial data bits, probably should be 8 (and defaults to 8)
argindex=match_string(optarg, bitstrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid number of bits: %s\n", optarg);
retval = -1;
goto out_noclose;
}
bits = atoi(bitstrings[argindex]);
break;
case 'd': // device name, default /dev/ttyS0
// could check the device name here, but we'll leave it to the library open
if (strlen(optarg) > FILENAME_MAX) {
printf("gs2_vfd: ERROR: device node name is too long: %s\n", optarg);
retval = -1;
goto out_noclose;
}
device = strdup(optarg);
break;
case 'g':
case 'v':
verbose = 1;
break;
case 'n': // module base name
if (strlen(optarg) > HAL_NAME_LEN-20) {
printf("gs2_vfd: ERROR: HAL module name too long: %s\n", optarg);
retval = -1;
goto out_noclose;
}
modname = strdup(optarg);
break;
case 'p': // parity, should be a string like "even", "odd", or "none"
argindex=match_string(optarg, paritystrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid parity: %s\n", optarg);
retval = -1;
goto out_noclose;
}
parity = paritychars[argindex];
break;
case 'r': // Baud rate, 38400 default
argindex=match_string(optarg, ratestrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid baud rate: %s\n", optarg);
retval = -1;
goto out_noclose;
}
baud = atoi(ratestrings[argindex]);
break;
case 's': // stop bits, defaults to 1
argindex=match_string(optarg, stopstrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid number of stop bits: %s\n", optarg);
retval = -1;
goto out_noclose;
}
stopbits = atoi(stopstrings[argindex]);
break;
case 't': // target number (MODBUS ID), default 1
argvalue = strtol(optarg, &endarg, 10);
if ((*endarg != '\0') || (argvalue < 1) || (argvalue > 254)) {
printf("gs2_vfd: ERROR: invalid slave number: %s\n", optarg);
retval = -1;
goto out_noclose;
}
slave = argvalue;
break;
case 'h':
default:
usage(argc, argv);
exit(0);
break;
}
}
printf("%s: device='%s', baud=%d, parity='%c', bits=%d, stopbits=%d, address=%d, verbose=%d\n",
modname, device, baud, parity, bits, stopbits, slave, verbose);
/* point TERM and INT signals at our quit function */
/* if a signal is received between here and the main loop, it should prevent
some initialization from happening */
signal(SIGINT, quit);
signal(SIGTERM, quit);
/* Assume 38.4k O-8-1 serial settings, device 1 */
mb_ctx = modbus_new_rtu(device, baud, parity, bits, stopbits);
if (mb_ctx == NULL) {
printf("%s: ERROR: couldn't open modbus serial device: %s\n", modname, modbus_strerror(errno));
goto out_noclose;
}
/* the open has got to work, or we're out of business */
if (((retval = modbus_connect(mb_ctx))!=0) || done) {
printf("%s: ERROR: couldn't open serial device: %s\n", modname, modbus_strerror(errno));
goto out_noclose;
}
modbus_set_debug(mb_ctx, verbose);
modbus_set_slave(mb_ctx, slave);
/* create HAL component */
hal_comp_id = hal_init(modname);
if ((hal_comp_id < 0) || done) {
printf("%s: ERROR: hal_init failed\n", modname);
retval = hal_comp_id;
goto out_close;
}
/* grab some shmem to store the HAL data in */
haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
if ((haldata == 0) || done) {
printf("%s: ERROR: unable to allocate shared memory\n", modname);
retval = -1;
goto out_close;
}
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat1), hal_comp_id, "%s.status-1", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat2), hal_comp_id, "%s.status-2", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_cmd), hal_comp_id, "%s.frequency-command", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_out), hal_comp_id, "%s.frequency-out", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->curr_out), hal_comp_id, "%s.output-current", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->DCBusV), hal_comp_id, "%s.DC-bus-volts", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->outV), hal_comp_id, "%s.output-voltage", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->RPM), hal_comp_id, "%s.motor-RPM", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->scale_freq), hal_comp_id, "%s.scale-frequency", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->power_factor), hal_comp_id, "%s.power-factor", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->load_pct), hal_comp_id, "%s.load-percentage", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->FW_Rev), hal_comp_id, "%s.firmware-revision", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->errorcount), hal_comp_id, "%s.error-count", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->looptime), hal_comp_id, "%s.loop-time", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->retval), hal_comp_id, "%s.retval", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->at_speed), hal_comp_id, "%s.at-speed", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->is_stopped), hal_comp_id, "%s.is-stopped", modname); // JET
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_IN, &(haldata->speed_command), hal_comp_id, "%s.speed-command", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_on), hal_comp_id, "%s.spindle-on", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_fwd), hal_comp_id, "%s.spindle-fwd", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_rev), hal_comp_id, "%s.spindle-rev", modname); //JET
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->err_reset), hal_comp_id, "%s.err-reset", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->speed_tolerance), hal_comp_id, "%s.tolerance", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->motor_hz), hal_comp_id, "%s.nameplate-HZ", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->motor_RPM), hal_comp_id, "%s.nameplate-RPM", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->ack_delay), hal_comp_id, "%s.ack-delay", modname);
if (retval!=0) goto out_closeHAL;
/* make default data match what we expect to use */
*(haldata->stat1) = 0;
*(haldata->stat2) = 0;
*(haldata->freq_cmd) = 0;
*(haldata->freq_out) = 0;
*(haldata->curr_out) = 0;
*(haldata->DCBusV) = 0;
*(haldata->outV) = 0;
*(haldata->RPM) = 0;
*(haldata->scale_freq) = 0;
*(haldata->power_factor) = 0;
*(haldata->load_pct) = 0;
*(haldata->FW_Rev) = 0;
haldata->errorcount = 0;
haldata->looptime = 0.1;
haldata->motor_RPM = 1730;
haldata->motor_hz = 60;
haldata->speed_tolerance = 0.01;
haldata->ack_delay = 2;
*(haldata->err_reset) = 0;
*(haldata->spindle_on) = 0;
*(haldata->spindle_fwd) = 1;
*(haldata->spindle_rev) = 0;
haldata->old_run = -1; // make sure the initial value gets output
haldata->old_dir = -1;
haldata->old_err_reset = -1;
hal_ready(hal_comp_id);
/* here's the meat of the program. loop until done (which may be never) */
while (done==0) {
read_data(mb_ctx, &slavedata, haldata);
write_data(mb_ctx, &slavedata, haldata);
/* don't want to scan too fast, and shouldn't delay more than a few seconds */
if (haldata->looptime < 0.001) haldata->looptime = 0.001;
if (haldata->looptime > 2.0) haldata->looptime = 2.0;
loop_timespec.tv_sec = (time_t)(haldata->looptime);
loop_timespec.tv_nsec = (long)((haldata->looptime - loop_timespec.tv_sec) * 1000000000l);
nanosleep(&loop_timespec, &remaining);
}
retval = 0; /* if we get here, then everything is fine, so just clean up and exit */
out_closeHAL:
hal_exit(hal_comp_id);
out_close:
modbus_close(mb_ctx);
modbus_free(mb_ctx);
out_noclose:
return retval;
}
int lcec_stmds5k_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
lcec_master_t *master = slave->master;
lcec_stmds5k_data_t *hal_data;
int err;
// initialize callbacks
slave->proc_read = lcec_stmds5k_read;
slave->proc_write = lcec_stmds5k_write;
// alloc hal memory
if ((hal_data = hal_malloc(sizeof(lcec_stmds5k_data_t))) == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name);
return -EIO;
}
memset(hal_data, 0, sizeof(lcec_stmds5k_data_t));
slave->hal_data = hal_data;
// read sdos
// B18 : torque reference
if ((hal_data->sdo_torque_reference = lcec_read_sdo(slave, 0x2212, 0x00, 4)) == NULL) {
return -EIO;
}
hal_data->torque_reference = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_torque_reference)) * STMDS5K_TORQUE_REF_DIV;
if (hal_data->torque_reference > 1e-20 || hal_data->torque_reference < -1e-20) {
hal_data->torque_reference_rcpt = 1.0 / hal_data->torque_reference;
} else {
hal_data->torque_reference_rcpt = 0.0;
}
// C01 : max rpm
if ((hal_data->sdo_speed_max_rpm = lcec_read_sdo(slave, 0x2401, 0x00, 4)) == NULL) {
return -EIO;
}
hal_data->speed_max_rpm = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm));
// D02 : setpoint max rpm
if ((hal_data->sdo_speed_max_rpm_sp = lcec_read_sdo(slave, 0x2602, 0x00, 2)) == NULL) {
return -EIO;
}
hal_data->speed_max_rpm_sp = (double)EC_READ_S16(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm_sp));
if (hal_data->speed_max_rpm_sp > 1e-20 || hal_data->speed_max_rpm_sp < -1e-20) {
hal_data->speed_max_rpm_sp_rcpt = 1.0 / hal_data->speed_max_rpm_sp;
} else {
hal_data->speed_max_rpm_sp_rcpt = 0.0;
}
// initialize POD entries
// E200 : device state byte
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x28c8, 0x00, &hal_data->dev_state_pdo_os, NULL);
// E100 : speed motor (x 0.1% relative to C01)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2864, 0x00, &hal_data->speed_mot_pdo_os, NULL);
// E02 : torque motor filterd (x 0,1 Nm)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2802, 0x00, &hal_data->torque_mot_pdo_os, NULL);
// D200 : speed state word
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26c8, 0x00, &hal_data->speed_state_pdo_os, NULL);
// E09 : rotor position
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2809, 0x00, &hal_data->pos_mot_pdo_os, NULL);
// A180 : Device Control Byte
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x20b4, 0x00, &hal_data->dev_ctrl_pdo_os, NULL);
// D230 : speed setpoint (x 0.1 % relative to D02, -200.0% .. 200.0%)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26e6, 0x00, &hal_data->speed_sp_rel_pdo_os, NULL);
// C230 : maximum torque (x 1%, 0% .. 200%)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x24e6, 0x00, &hal_data->torque_max_pdo_os, NULL);
// export pins
if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->vel_cmd), comp_id, "%s.%s.%s.srv-vel-cmd", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb), comp_id, "%s.%s.%s.srv-vel-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb_rpm), comp_id, "%s.%s.%s.srv-vel-fb-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_rpm), comp_id, "%s.%s.%s.srv-vel-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(hal_data->enc_raw), comp_id, "%s.%s.%s.srv-enc-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enc-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_hi), comp_id, "%s.%s.%s.srv-pos-raw-hi", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-hi failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_lo), comp_id, "%s.%s.%s.srv-pos-raw-lo", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-lo failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->pos_fb), comp_id, "%s.%s.%s.srv-pos-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb), comp_id, "%s.%s.%s.srv-torque-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_abs), comp_id, "%s.%s.%s.srv-torque-fb-abs", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-abs failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_pct), comp_id, "%s.%s.%s.srv-torque-fb-pct", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-pct failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->torque_lim), comp_id, "%s.%s.%s.srv-torque-lim", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-lim failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->stopped), comp_id, "%s.%s.%s.srv-stopped", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-stopped failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->at_speed), comp_id, "%s.%s.%s.srv-at-speed", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-at-speed failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->overload), comp_id, "%s.%s.%s.srv-overload", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-overload failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->ready), comp_id, "%s.%s.%s.srv-ready", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->error), comp_id, "%s.%s.%s.srv-error", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-error failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->toggle), comp_id, "%s.%s.%s.srv-toggle", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->loc_ena), comp_id, "%s.%s.%s.srv-loc-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-loc-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->enable), comp_id, "%s.%s.%s.srv-enable", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->err_reset), comp_id, "%s.%s.%s.srv-err-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-err-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->fast_ramp), comp_id, "%s.%s.%s.srv-fast-ramp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-fast-ramp failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->brake), comp_id, "%s.%s.%s.srv-brake", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-brake failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IO, &(hal_data->index_ena), comp_id, "%s.%s.%s.srv-index-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-index-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->pos_reset), comp_id, "%s.%s.%s.srv-pos-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_neg), comp_id, "%s.%s.%s.srv-on-home-neg", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_pos), comp_id, "%s.%s.%s.srv-on-home-pos", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
// export parameters
if ((err = hal_param_float_newf(HAL_RW, &(hal_data->pos_scale), comp_id, "%s.%s.%s.srv-pos-scale", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->torque_reference), comp_id, "%s.%s.%s.srv-torque-ref", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-ref failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm), comp_id, "%s.%s.%s.srv-max-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm_sp), comp_id, "%s.%s.%s.srv-max-rpm-sp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm-sp failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_s32_newf(HAL_RW, &(hal_data->home_raw), comp_id, "%s.%s.%s.srv-home-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-home-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
// set default pin values
*(hal_data->vel_cmd) = 0.0;
*(hal_data->vel_fb) = 0.0;
*(hal_data->vel_fb_rpm) = 1.0;
*(hal_data->vel_rpm) = 0.0;
*(hal_data->pos_raw_hi) = 0;
*(hal_data->pos_raw_lo) = 0;
*(hal_data->pos_fb) = 0.0;
*(hal_data->torque_fb) = 0.0;
*(hal_data->torque_fb_abs) = 0.0;
*(hal_data->torque_fb_pct) = 0.0;
*(hal_data->torque_lim) = 1.0;
*(hal_data->stopped) = 0;
*(hal_data->at_speed) = 0;
*(hal_data->overload) = 0;
*(hal_data->ready) = 0;
*(hal_data->error) = 0;
*(hal_data->toggle) = 0;
*(hal_data->loc_ena) = 0;
*(hal_data->enable) = 0;
*(hal_data->err_reset) = 0;
*(hal_data->fast_ramp) = 0;
*(hal_data->brake) = 0;
*(hal_data->index_ena) = 0;
*(hal_data->pos_reset) = 0;
*(hal_data->enc_raw) = 0;
*(hal_data->on_home_neg) = 0;
*(hal_data->on_home_pos) = 0;
// initialize variables
hal_data->pos_scale = 1.0;
hal_data->do_init = 1;
hal_data->pos_cnt = 0;
hal_data->index_cnt = 0;
hal_data->last_pos_cnt = 0;
hal_data->pos_scale_old = hal_data->pos_scale + 1.0;
hal_data->pos_scale_rcpt = 1.0;
hal_data->pos_scale_cnt = 1.0;
hal_data->last_index_ena = 0;
hal_data->index_ref = 0;
hal_data->home_raw = 0;
return 0;
}
// init HAL objects
static int export_halobjs(struct inst_data *ip, int owner_id, const char *name)
{
if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name))
return -1;
if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name))
return -1;
if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name))
return -1;
// unittest observer pins, per instance
if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name))
return -1;
if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name))
return -1;
// exporting '<instname>.funct' as an extended thread function
// see lutn.c for a discussion of advantages
hal_export_xfunct_args_t xfunct_args = {
.type = FS_XTHREADFUNC,
.funct.x = funct,
.arg = ip, // the instance's HAL memory blob
.uses_fp = 1,
.reentrant = 0,
.owner_id = owner_id
};
return hal_export_xfunctf(&xfunct_args, "%s.funct", name);
}
// constructor - init all HAL pins, params, funct etc here
static int instantiate(const int argc, const char**argv)
{
// argv[0]: component name
const char *name = argv[1]; // instance name
struct inst_data *ip;
// allocate a named instance, and some HAL memory for the instance data
int inst_id = hal_inst_create(name, comp_id,
sizeof(struct inst_data),
(void **)&ip);
if (inst_id < 0)
return -1;
// here ip is guaranteed to point to a blob of HAL memory
// of size sizeof(struct inst_data).
HALINFO("inst=%s argc=%d\n", name, argc);
HALINFO("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALINFO("module parms: answer=%d text='%s'", answer, text);
// example - parse newinst arguments getopt-style:
// straight from: http://www.informit.com/articles/article.aspx?p=175771&seqNum=3
int do_all, do_help, do_verbose; /* flag variables */
char *myfile;
struct option longopts[] = {
{ "all", no_argument, & do_all, 1 },
{ "file", required_argument, NULL, 'f' },
{ "help", no_argument, & do_help, 1 },
{ "verbose", no_argument, & do_verbose, 1 },
{ 0, 0, 0, 0 }
};
int c;
while ((c = getopt_long(argc, argv, ":f:", longopts, NULL)) != -1) {
switch (c) {
case 'f':
myfile = optarg;
break;
case 0: // getopt_long() set a variable, just keep going
break;
case ':': // missing option argument
HALERR("%s: option `-%c' requires an argument\n",
argv[1], optopt);
break;
case '?':
default: // invalid option
HALERR("%s: option `-%c' is invalid, ignored",
argv[1], optopt);
break;
}
}
HALINFO("do_all=%d do_help=%d do_verbose=%d myfile=%s",
do_all, do_help, do_verbose, myfile);
// these pins/params/functs will be owned by the instance,
// and can be separately exited via 'halcmd delinst <instancename>'
int retval = export_halobjs(ip, inst_id, name);
// unittest: echo instance parameters into observer pins
if (!retval) {
*(ip->repeat_value) = repeat;
*(ip->prefix_len) = strlen(prefix);
}
return retval;
}
// custom destructor
// pins, params, and functs are automatically deallocated by hal_exit(comp_id)
// regardless if a destructor is used or not (see below), so usually it is not
// needed
//
// however, some objects like vtables, rings, threads, signals are not owned by
// a component, hence cannot be automatically exited by hal_exit() even if desired
//
// interaction with such objects may require a custom destructor like below for
// cleanup actions
// NB: if a customer destructor is used, it is called
// - after the instance's functs have been removed from their respective threads
// (so a thread funct call cannot interact with the destructor any more)
// - any pins and params of this instance are still intact when the destructor is
// called, and they are automatically destroyed by the HAL library once the
// destructor returns
static int delete(const char *name, void *inst, const int inst_size)
{
HALERR("inst=%s size=%d %p\n", name, inst_size, inst);
HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALERR("module parms: answer=%d text='%s'", answer, text);
return 0;
}
int rtapi_app_main(void)
{
HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALERR("module parms: answer=%d text='%s'", answer, text);
// to use default destructor, use NULL instead of delete
comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, delete, compname);
if (comp_id < 0)
return comp_id;
#if 0
// this is how an 'instance' would have been done in the legacy way
struct inst_data *ip = hal_malloc(sizeof(struct inst_data));
// these pins/params/functs will be owned by the component
// NB: this 'instance' cannot be exited, and no new one created on the fly
if (export_halobjs(ip, comp_id, "foo"))
return -1;
#endif
// unittest only, see nosetests/unittest_instbindings.py and
// nosetests/unittest_icomp.py
// purpose: echo module params into observer pins
// (cant set pins to strings, so just echo the string length)
if ((cd = export_observer_pins(comp_id, compname)) == NULL)
return -1;
*(cd->answer_value) = answer;
*(cd->text_len) = strlen(text);
hal_ready(comp_id);
return 0;
}
int hm2_pwmgen_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x0003)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->pwmgen.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;
}
if (hm2->config.num_pwmgens > md->instances) {
HM2_ERR(
"config.num_pwmgens=%d, but only %d are available, not loading driver\n",
hm2->config.num_pwmgens,
md->instances
);
return -EINVAL;
}
if (hm2->config.num_pwmgens == 0) {
return 0;
}
//
// looks good, start initializing
//
if (hm2->config.num_pwmgens == -1) {
hm2->pwmgen.num_instances = md->instances;
} else {
hm2->pwmgen.num_instances = hm2->config.num_pwmgens;
}
// allocate the module-global HAL shared memory
hm2->pwmgen.hal = (hm2_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_pwmgen_module_global_t));
if (hm2->pwmgen.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(hm2->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t));
if (hm2->pwmgen.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->pwmgen.clock_frequency = md->clock_freq;
hm2->pwmgen.version = md->version;
hm2->pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride);
hm2->pwmgen.pwm_mode_addr = md->base_address + (1 * md->register_stride);
hm2->pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (2 * md->register_stride);
hm2->pwmgen.pdmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride);
hm2->pwmgen.enable_addr = md->base_address + (4 * md->register_stride);
r = hm2_register_tram_write_region(hm2, hm2->pwmgen.pwm_value_addr, (hm2->pwmgen.num_instances * sizeof(rtapi_u32)), &hm2->pwmgen.pwm_value_reg);
if (r < 0) {
HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r);
goto fail0;
}
hm2->pwmgen.pwm_mode_reg = (rtapi_u32 *)rtapi_kmalloc(hm2->pwmgen.num_instances * sizeof(rtapi_u32), RTAPI_GFP_KERNEL);
if (hm2->pwmgen.pwm_mode_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
// export to HAL
// FIXME: r hides the r in enclosing function, and it returns the wrong thing
{
int i;
int r;
char name[HAL_NAME_LEN + 1];
// these hal parameters affect all pwmgen instances
r = hal_param_u32_newf(
HAL_RW,
&(hm2->pwmgen.hal->param.pwm_frequency),
hm2->llio->comp_id,
"%s.pwmgen.pwm_frequency",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pwmgen.pwm_frequency param, aborting\n");
goto fail1;
}
hm2->pwmgen.hal->param.pwm_frequency = 20000;
hm2->pwmgen.written_pwm_frequency = 0;
r = hal_param_u32_newf(
HAL_RW,
&(hm2->pwmgen.hal->param.pdm_frequency),
hm2->llio->comp_id,
"%s.pwmgen.pdm_frequency",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pwmgen.pdm_frequency param, aborting\n");
goto fail1;
}
hm2->pwmgen.hal->param.pdm_frequency = 20000;
hm2->pwmgen.written_pdm_frequency = 0;
for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.value", hm2->llio->name, i);
r = hal_pin_float_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.value), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.enable", hm2->llio->name, i);
r = hal_pin_bit_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.scale", hm2->llio->name, i);
r = hal_param_float_new(name, HAL_RW, &(hm2->pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding param '%s', aborting\n", name);
goto fail1;
}
r = hal_param_s32_newf(
HAL_RW,
&(hm2->pwmgen.instance[i].hal.param.output_type),
hm2->llio->comp_id,
"%s.pwmgen.%02d.output-type",
hm2->llio->name,
i
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
goto fail1;
}
// init hal objects
*(hm2->pwmgen.instance[i].hal.pin.enable) = 0;
*(hm2->pwmgen.instance[i].hal.pin.value) = 0.0;
hm2->pwmgen.instance[i].hal.param.scale = 1.0;
hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;
hm2->pwmgen.instance[i].written_output_type = -666; // force an update at the start
hm2->pwmgen.instance[i].written_enable = -666; // force an update at the start
}
}
return hm2->pwmgen.num_instances;
fail1:
rtapi_kfree(hm2->pwmgen.pwm_mode_reg);
fail0:
hm2->pwmgen.num_instances = 0;
return r;
}
static int hal_export_xfunctfv(const hal_export_xfunct_args_t *xf, const char *fmt, va_list ap)
{
int *prev, next, cmp, sz;
hal_funct_t *nf, *fptr;
char name[HAL_NAME_LEN + 1];
CHECK_HALDATA();
CHECK_LOCK(HAL_LOCK_LOAD);
sz = rtapi_vsnprintf(name, sizeof(name), fmt, ap);
if(sz == -1 || sz > HAL_NAME_LEN) {
HALERR("length %d invalid for name starting '%s'", sz, name);
return -ENOMEM;
}
HALDBG("exporting function '%s' type %d", name, xf->type);
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before accessing shared data */
rtapi_mutex_get(&(hal_data->mutex));
comp = halpr_find_owning_comp(xf->owner_id);
if (comp == 0) {
/* bad comp_id */
HALERR("funct '%s': owning component %d not found",
name, xf->owner_id);
return -EINVAL;
}
if (comp->type == TYPE_USER) {
/* not a realtime component */
HALERR("funct '%s': component %s/%d is not realtime (%d)",
name, comp->name, comp->comp_id, comp->type);
return -EINVAL;
}
bool legacy = (halpr_find_inst_by_id(xf->owner_id) == NULL);
// instances may export functs post hal_ready
if (legacy && (comp->state > COMP_INITIALIZING)) {
HALERR("funct '%s': called after hal_ready", name);
return -EINVAL;
}
/* allocate a new function structure */
nf = alloc_funct_struct();
if (nf == 0)
NOMEM("function '%s'", name);
/* initialize the structure */
nf->uses_fp = xf->uses_fp;
nf->owner_id = xf->owner_id;
nf->reentrant = xf->reentrant;
nf->users = 0;
nf->handle = rtapi_next_handle();
nf->arg = xf->arg;
nf->type = xf->type;
nf->funct.l = xf->funct.l; // a bit of a cheat really
rtapi_snprintf(nf->name, sizeof(nf->name), "%s", name);
/* search list for 'name' and insert new structure */
prev = &(hal_data->funct_list_ptr);
next = *prev;
while (1) {
if (next == 0) {
/* reached end of list, insert here */
nf->next_ptr = next;
*prev = SHMOFF(nf);
/* break out of loop and init the new function */
break;
}
fptr = SHMPTR(next);
cmp = strcmp(fptr->name, nf->name);
if (cmp > 0) {
/* found the right place for it, insert here */
nf->next_ptr = next;
*prev = SHMOFF(nf);
/* break out of loop and init the new function */
break;
}
if (cmp == 0) {
/* name already in list, can't insert */
free_funct_struct(nf);
HALERR("duplicate function '%s'", name);
return -EINVAL;
}
/* didn't find it yet, look at next one */
prev = &(fptr->next_ptr);
next = *prev;
}
// at this point we have a new function and can
// yield the mutex by scope exit
}
/* init time logging variables */
nf->runtime = 0;
nf->maxtime = 0;
nf->maxtime_increased = 0;
/* at this point we have a new function and can yield the mutex */
rtapi_mutex_give(&(hal_data->mutex));
switch (xf->type) {
case FS_LEGACY_THREADFUNC:
case FS_XTHREADFUNC:
/* create a pin with the function's runtime in it */
if (hal_pin_s32_newf(HAL_OUT, &(nf->runtime), xf->owner_id, "%s.time",name)) {
HALERR("failed to create pin '%s.time'", name);
return -EINVAL;
}
*(nf->runtime) = 0;
/* note that failure to successfully create the following params
does not cause the "export_funct()" call to fail - they are
for debugging and testing use only */
/* create a parameter with the function's maximum runtime in it */
nf->maxtime = 0;
hal_param_s32_newf(HAL_RW, &(nf->maxtime), xf->owner_id, "%s.tmax", name);
/* create a parameter with the function's maximum runtime in it */
nf->maxtime_increased = 0;
hal_param_bit_newf(HAL_RO, &(nf->maxtime_increased), xf->owner_id,
"%s.tmax-inc", name);
break;
case FS_USERLAND: // no timing pins/params
;
}
return 0;
}
