int rtapi_app_main(void)
{
int n, retval, i;
if(num_chan && names[0]) {
rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n");
return -EINVAL;
}
if(!num_chan && !names[0]) num_chan = default_num_chan;
if(num_chan) {
howmany = num_chan;
} else {
for(i=0; names[i]; i++) {howmany = i+1;}
}
/* test for number of channels */
if ((howmany <= 0) || (howmany > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: invalid number of channels: %d\n", howmany);
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("siggen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SIGGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for siggen data */
siggen_array = hal_malloc(howmany * sizeof(hal_siggen_t));
if (siggen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export variables and functions for each siggen */
i = 0; // for names= items
for (n = 0; n < howmany; n++) {
/* export everything for this loop */
if(num_chan) {
char buf[HAL_NAME_LEN + 1];
rtapi_snprintf(buf, sizeof(buf), "siggen.%d", n);
retval = export_siggen(n, &(siggen_array[n]),buf);
} else {
retval = export_siggen(n, &(siggen_array[n]),names[i++]);
}
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: siggen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SIGGEN: installed %d signal generators\n", howmany);
hal_ready(comp_id);
return 0;
}
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;
}
/**
\brief Export pins for component(s)
\param
num: component number
addr: pointer to array of the num^th FOC channel data
*/
static int exportFOCaxis(int num, FOC_data_t * addr)
{
int retval = 0;
//
// PINS
//
// make available setpoint in hal
if ( (retval = hal_pin_float_newf(HAL_IN, &(addr->setpoint), comp_id, "hal_zed_can.%d.setpoint", num) ) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin setpoint export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
// make available position feedback in hal
if ( (retval = hal_pin_float_newf(HAL_OUT, &(addr->feedback), comp_id, "hal_zed_can.%d.feedback", num) ) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin feedback export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
//
// PARAMS
//
// instantiate a parameter
retval = hal_param_float_newf(HAL_RW, &(addr->dbg), comp_id, "hal_zed_can.%d.debug", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param debug export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
// instantiate parameter
retval = hal_pin_u32_newf(HAL_OUT, &(addr->focstatus), comp_id, "hal_zed_can.%d.status", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin status export failed with err=%d" ,retval);
hal_exit(comp_id);
return -1;
}
// instantiate parameter
retval = hal_pin_u32_newf(HAL_OUT, &(addr->focerror), comp_id, "hal_zed_can.%d.error", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param error export failed with err=%d",retval);
hal_exit(comp_id);
return -1;
}
// completed successfully
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: exportFOCaxis(%d) completed successfully.\n", num);
return 0;
}
int rtapi_app_main(void)
{
int n, retval;
/* test for too many delays asked for */
if ((num_delays <= 0) || (num_delays > MAX_DELAYS)) {
rtapi_print_msg(RTAPI_MSG_ERR, "TIMEDELAY: ERROR: Invalid number of bit delays\n");
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("timedelay");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for delay array */
delay_array = hal_malloc(num_delays * sizeof(bit_delay_t));
if (delay_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export pins/params for all delays */
for (n = 0; n < num_delays; n++) {
/* export all vars */
retval = export_delay(n, &(delay_array[n]));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: group %d export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
/* export update function */
retval = hal_export_funct("process_delays", process_delays, delay_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: process_delays funct export failed\n");
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"TIMEDELAY: installed %d time delays\n", num_delays);
hal_ready(comp_id);
return 0;
}
int
rtapi_app_main(void)
{
int i;
Plc *pComp;
// Connect to the HAL.
component.id = hal_init("boss_plc");
if (component.id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "BOSS_PLC: ERROR: hal_init() failed\n");
return(-1);
}
for(i = 0; i < MAX_DEVICES; i++){
component.plcTable[i] = NULL;
}
if(count > MAX_DEVICES)
count = MAX_DEVICES;
for(i = 0; i < count; i++){
// Allocate memory for device object.
pComp = hal_malloc(sizeof(Plc));
if (pComp == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, "BOSS_PLC: ERROR: hal_malloc() failed\n");
hal_exit(component.id);
return(-1);
}
// Save pointer to device object.
component.plcTable[i] = pComp;
// Initialize device.
if(Plc_Init(pComp)){
hal_exit(component.id);
return(-1);
}
// Export pins, parameters, and functions.
if(Plc_Export(pComp, component.id, i)){
hal_exit(component.id);
return(-1);
}
}
hal_ready(component.id);
return(0);
}
int rtapi_app_main(void)
{
int retval;
void *shm_base;
long skip;
/* connect to the HAL */
comp_id = hal_init("scope_rt");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SCOPE: ERROR: hal_init() failed\n");
return -1;
}
/* connect to scope shared memory block */
skip = (sizeof(scope_shm_control_t) + 3) & ~3;
shm_size = skip + num_samples * sizeof(scope_data_t);
shm_id = rtapi_shmem_new(SCOPE_SHM_KEY, comp_id, shm_size);
if (shm_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SCOPE RT: ERROR: failed to get shared memory (key=0x%x, size=%lu)\n",
SCOPE_SHM_KEY,
shm_size
);
hal_exit(comp_id);
return -1;
}
retval = rtapi_shmem_getptr(shm_id, &shm_base);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SCOPE: ERROR: failed to map shared memory\n");
rtapi_shmem_delete(shm_id, comp_id);
hal_exit(comp_id);
return -1;
}
/* init control structure */
ctrl_rt = &ctrl_struct;
init_rt_control_struct(shm_base);
/* export scope data sampling function */
retval = hal_export_funct("scope.sample", sample, NULL, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SCOPE_RT: ERROR: sample funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_DBG, "SCOPE_RT: installed sample function\n");
hal_ready(comp_id);
return 0;
}
static int return_instance_samples(int n)
{
int i, nr_of_samples = 0;
char character;
// traverse the string to count the number of correct pins
for (i = 0; (character = samples[n][i]); i++) {
switch (character) {
case 'b':
case 'B':
case 'f':
case 'F':
case 's':
case 'S':
case 'u':
case 'U':
nr_of_samples++;
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "invalid character in "
"\"samples=\" string. Needs to be only b,f,s or u\n");
hal_exit(comp_id);
return -1;
}
}
return nr_of_samples;
}
/**
\brief Determine Zynq revision
\details Parse data in /proc/cpuinfo for 'Revision'
\return
-1: unable to parse /proc/cpuinfo
-1: unable to parse a version number
nnnn: the one and only revision */
static int zynq_revision()
{
char *path = "/proc/cpuinfo", *s, line[1024];
int rev = -1;
// parse /proc/cpuinfo for the line: Revision
char *rev_line = "Revision";
FILE *f = fopen(path,"r");
if (!f) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: can't open %s: %d - %s\n",
path, errno, strerror(errno));
hal_exit(comp_id);
return -1;
}
while (fgets(line, sizeof(line), f)) {
if (!strncmp(line, rev_line, strlen(rev_line))) {
s = strchr(line, ':');
if (s && 1 == sscanf(s, ":%d", &rev)) {
fclose(f);
return rev;
}
}
}
fclose(f);
return -1;
}
int rtapi_app_main(void) {
int res = 0;
comp_id = hal_init(name);
if(comp_id < 0) return comp_id;
vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id);
if (vtable_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
name, name, VTVERSION, &vtk, vtable_id );
return -ENOENT;
}
haldata = hal_malloc(sizeof(struct haldata));
if(!haldata) goto error;
if((res = hal_pin_float_new("tripodkins.Bx", HAL_IO, &(haldata->bx), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("tripodkins.Cx", HAL_IO, &(haldata->cx), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("tripodkins.Cy", HAL_IO, &(haldata->cy), comp_id)) < 0) goto error;
Bx = Cx = Cy = 1.0;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void quit_cleanup(void)
{
char *fnct_name = "quit_cleanup";
int counter, ret;
DBG(gbl.init_dbg, "started");
for (counter = 0; counter < gbl.tot_mb_links; counter++) {
if (gbl.mb_links[counter].modbus != NULL) {
modbus_close(gbl.mb_links[counter].modbus);
modbus_free(gbl.mb_links[counter].modbus);
gbl.mb_links[counter].modbus = NULL;
}
}
gbl.tot_mb_links = 0;
if (gbl.mb_tx != NULL) {
free(gbl.mb_tx);
}
gbl.mb_tx = NULL;
gbl.tot_mb_tx = 0;
if (gbl.hal_mod_id >= 0) {
ret = hal_exit(gbl.hal_mod_id);
DBG(gbl.init_dbg, "unloading HAL module [%d] ret[%d]", gbl.hal_mod_id, ret);
}
DBG(gbl.init_dbg, "done OK");
}
int rtapi_app_main(void)
{
int res = 0;
comp_id = hal_init(name);
if (comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if (((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Tool_offset),
comp_id, "%s.Tool-offset", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Y_offset),
comp_id, "%s.Y-offset", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Z_offset),
comp_id, "%s.Z-offset", name)) < 0))
goto error;
vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id);
if (vtable_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
name, name, VTVERSION, &vtk, vtable_id );
return -ENOENT;
}
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void)
{
if (gpio)
munmap((void *) gpio, BCM2835_BLOCK_SIZE);
close(mem_fd);
hal_exit(comp_id);
}
int rtapi_app_main(void) {
int result;
comp_id = hal_init(name);
if(comp_id < 0) return comp_id;
vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id);
if (vtable_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
name, name, VTVERSION, &vtk, vtable_id );
return -ENOENT;
}
haldata = hal_malloc(sizeof(struct haldata));
result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
if(result < 0) goto error;
*(haldata->pivot_length) = 0.666;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return result;
}
void display_help (void)
{
printf("\nClassicLadder v"CL_RELEASE_VER_STRING"\n"CL_RELEASE_DATE_STRING"\n"
"Copyright (C) 2001-2004 Marc Le Douarain\[email protected]\n"
"Adapted to EMC\n"
"\n"
"ClassicLadder comes with NO WARRANTY\n"
"to the extent permitted by law.\n"
"\n"
"You may redistribute copies of ClassicLadder\n"
"under the terms of the GNU Lesser General Public Licence.\n"
"See the file `lesserGPL.txt' for more information.\n");
printf("This version of Classicladder is adapted for use with EMC and HAL\n"
"\nUsage: classicladder [OPTIONS] [PATH]\n"
"eg: loadusr -w classicladder ladtest.clp\n"
"eg: loadusr -w classicladder --nogui ladtest.clp\n"
"eg: loadusr -w classicladder --modmaster ladtest.clp\n"
"\n"
" --nogui do not create a GUI, only load a configuration\n"
" --config=filename initialize modbus master I/O & load config file-( deprecated- use --modmaster)\n"
" --modmaster initialize modbus master I/O ( modbus config is loaded with other objects )\n"
" --modslave initialize modbus slave I/O (TCP only- B and W variables accesable\n"
" --modbus_port=portnumber used for modbus slave using TCP ( ethernet )\n"
" --debug sets the RTAPI debuglevel for printing debug messages\n"
"Please also note that the classicladder realtime module must be loaded first\n"
"eg: loadrt classicladder_rt for default number of ladder objects\n"
);
hal_exit(compId); // add for emc
exit(0);
}
static void init_usr_control_struct(void *shmem)
{
char *cp;
int n, skip;
/* first clear entire user struct to all zeros */
cp = (char *) ctrl_usr;
for (n = 0; n < sizeof(scope_usr_control_t); n++) {
cp[n] = 0;
}
/* save pointer to shared control structure */
ctrl_shm = shmem;
/* round size of shared struct up to a multiple of 4 for alignment */
skip = (sizeof(scope_shm_control_t) + 3) & ~3;
/* the rest of the shared memory area is the data buffer */
ctrl_usr->buffer = (scope_data_t *) (((char *) (shmem)) + skip);
/* is the realtime component loaded already? */
if (ctrl_shm->shm_size == 0) {
/* no, this is an error condition */
rtapi_print_msg(RTAPI_MSG_ERR, "Realtime component not loaded? ctrl_shm->size == 0");
hal_exit(comp_id);
exit(1);
}
/* init any non-zero fields */
/* set all 16 channels to "no source assigned" */
for (n = 0; n < 16; n++) {
ctrl_usr->chan[n].data_source_type = -1;
}
/* done */
}
void
rtapi_app_exit(void)
{
int i, j;
Device *pDevice;
hal_exit(driver.componentId);
for(i = 0; i < MAX_DEVICES; i++){
if((pDevice = driver.deviceTable[i]) != NULL){
// turn off digital outputs
for(j = 0; j < pDevice->numFpga; j++){
pDevice->pCard->fpga[j].digitalIo = MOTENC_DIGITAL_OUT;
}
// set DAC outputs to zero volts
for(j = 0; j < MOTENC_NUM_DAC_CHANNELS; j++){
pDevice->pCard->dac[j] = MOTENC_DAC_COUNT_ZERO;
}
// Unmap card.
iounmap((void *)(pDevice->pCard));
// TODO: Free device object when HAL supports free.
// hal_free(pDevice);
}
}
}
int rtapi_app_main(void) {
int res=0;
comp_id = hal_init("pumakins");
if (comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata) goto error;
if((res = hal_pin_float_new("pumakins.A2", HAL_IO, &(haldata->a2), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("pumakins.A3", HAL_IO, &(haldata->a3), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("pumakins.D3", HAL_IO, &(haldata->d3), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("pumakins.D4", HAL_IO, &(haldata->d4), comp_id)) < 0) goto error;
PUMA_A2 = DEFAULT_PUMA560_A2;
PUMA_A3 = DEFAULT_PUMA560_A3;
PUMA_D3 = DEFAULT_PUMA560_D3;
PUMA_D4 = DEFAULT_PUMA560_D4;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void)
{
int retval;
rtapi_set_msg_handler(old_handler);
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() started.\n");
retval = hal_stop_threads();
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: hal_stop_threads() failed, returned %d\n"), retval);
}
/* free shared memory */
retval = rtapi_shmem_delete(emc_shmem_id, mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: rtapi_shmem_delete() failed, returned %d\n"), retval);
}
/* disconnect from HAL and RTAPI */
retval = hal_exit(mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: hal_exit() failed, returned %d\n"), retval);
}
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() finished.\n");
}
void rtapi_app_exit(void)
{
pins_exit();
if( iomem )
iounmap( iomem, GPIO_MAP_SIZE );
hal_exit(comp_id);
}
void rtapi_app_exit(void)
{
#if (TRACE!=0)
fclose(dptrace);
#endif
hal_exit(comp_id);
}
int
rtapi_app_main(void)
{
int i;
Pid *pComp;
// Check number of channels.
if((num_chan <= 0) || (num_chan > MAX_CHANNELS)){
rtapi_print_msg(RTAPI_MSG_ERR,
"PID: ERROR: invalid num_chan: %d\n", num_chan);
return(-1);
}
// Connect to the HAL.
component.id = hal_init("at_pid");
if(component.id < 0){
rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_init() failed\n");
return(-1);
}
// Allocate memory for pid objects.
component.pidTable = hal_malloc(num_chan * sizeof(*pComp));
if(component.pidTable == NULL){
rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_malloc() failed\n");
hal_exit(component.id);
return(-1);
}
pComp = component.pidTable;
for(i = 0; i < num_chan; i++, pComp++){
// Initialize pid.
if(Pid_Init(pComp)){
hal_exit(component.id);
return(-1);
}
// Export pins, parameters, and functions.
if(Pid_Export(pComp, component.id, i)){
hal_exit(component.id);
return(-1);
}
}
hal_ready(component.id);
return(0);
}
void rtapi_app_exit(void)
{
int n;
for (n = 0; n < num_ports; n++) {
hal_parport_release(&port_data_array[n].portdata);
}
hal_exit(comp_id);
}
int rtapi_app_main(void)
{
int n, numchan, max_depth, retval;
fifo_t tmp_fifo[MAX_SAMPLERS];
/* validate config info */
for ( n = 0 ; n < MAX_SAMPLERS ; n++ ) {
if (( cfg[n] == NULL ) || ( *cfg == '\0' ) || ( depth[n] <= 0 )) {
break;
}
tmp_fifo[n].num_pins = parse_types(&(tmp_fifo[n]), cfg[n]);
if ( tmp_fifo[n].num_pins == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: bad config string '%s'\n", cfg[n]);
return -EINVAL;
}
/* allow one extra "slot" for the sample number */
max_depth = MAX_SHMEM / (sizeof(shmem_data_t) * (tmp_fifo[n].num_pins + 1));
if ( depth[n] > max_depth ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: depth too large, max is %d\n", max_depth);
return -ENOMEM;
}
tmp_fifo[n].depth = depth[n];
}
if ( n == 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: no channels specified\n");
return -EINVAL;
}
numchan = n;
/* clear shmem IDs */
for ( n = 0 ; n < MAX_SAMPLERS ; n++ ) {
shmem_id[n] = -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("sampler");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SAMPLER: ERROR: hal_init() failed\n");
return -EINVAL;
}
/* create the samplers - allocate memory, export pins, etc. */
for (n = 0; n < numchan; n++) {
retval = init_sampler(n, &(tmp_fifo[n]));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: sampler %d init failed\n", n);
hal_exit(comp_id);
return retval;
}
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SAMPLER: installed %d data samplers\n", numchan);
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
/* is sample function linked to a thread? */
if (ctrl_shm->thread_name[0] != '\0') {
/* need to unlink it before we release the scope shared memory */
hal_del_funct_from_thread("scope.sample", ctrl_shm->thread_name);
}
rtapi_shmem_delete(shm_id, comp_id);
hal_exit(comp_id);
}
/**
\brief Parse incoming CAN messages
\params rxframe: incoming message
n: CAN channel
FOC_data_t: FOC data image to fill
\return 0 everything OK
-1 */
int ParseMessage(struct can_frame *rxframe, int n, FOC_data_t *focrxdata)
{
// parse packet type, mask away CAN address
switch( ( rxframe->can_id & 0xFFFFFF) ) {
case 0xFFFF81:
{
// 0x81FFFF81, here it is a periodic! grab first (4 bytes) data (position).
__s32 *pos = NULL;
// FOC position feedback is 65536 postions per turn
// for a direct drive screw with 1mm x turn 65536.0;
// for a direct drive screw with 5mm x turn 65536.0/5.0;
// for 46:22 pulleys and 5mm x turn 65535/(5*46/22)
pos = (__s32*) rxframe->data;
// *(focrxdata->feedback) = (float) *pos / (65536.0/5.0);
*(focrxdata->feedback) = (float) *pos / ( (float)PPR[n] / ( (float)Screw_ratio[n]*((float)Motor_gear[n]/Screw_gear[n])));
}
break;
case 0xFFFF82:
{
// 0x81FFFF82, here it is a status/error packet.
hal_u32_t *stat = NULL, *err = NULL;
stat = (hal_u32_t*) rxframe->data;
err = (hal_u32_t*) (rxframe->data+4);
*(focrxdata->focstatus) = (hal_u32_t) *stat;
*(focrxdata->focerror) = (hal_u32_t) *err;
}
break;
case 0xFFFFFE:
// 0x8xFFFFFE: oh, it's just an ack to some boring command...
break;
case 0xFFFFFF:
// 0x8xFFFFFF: ok, this is a command NACK, something serious has happened,
// like an set-point overrange or such...
// \todo try to mend it.
rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: Error: NACK (0x%X) from CAN%d",rxframe->can_id, n);
// hal_exit(comp_id);
// return (-1);
break;
default:
// aliens incoming!
rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: Error: unexpected message received 0x%X form CAN%d",rxframe->can_id, n);
hal_exit(comp_id);
return (-1);
break;
}
return (0);
}
void
rtapi_app_exit(void)
{
Pid *pComp;
hal_exit(component.id);
if((pComp = component.pidTable) != NULL){
// TODO: Free pid objects when HAL supports free.
// hal_free(pComp);
}
}
void rtapi_app_exit(void)
{
int n;
/* free any shmem blocks */
for ( n = 0 ; n < MAX_SAMPLERS ; n++ ) {
if ( shmem_id[n] > 0 ) {
rtapi_shmem_delete(shmem_id[n], comp_id);
}
}
hal_exit(comp_id);
}
int rtapi_app_main(void)
{
int n, retval;
/* test for number of channels */
if ((num_chan <= 0) || (num_chan > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: invalid num_chan: %d\n", num_chan);
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("siggen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SIGGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for siggen data */
siggen_array = hal_malloc(num_chan * sizeof(hal_siggen_t));
if (siggen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export variables and functions for each siggen */
for (n = 0; n < num_chan; n++) {
/* export everything for this loop */
retval = export_siggen(n, &(siggen_array[n]));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: siggen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SIGGEN: installed %d signal generators\n", num_chan);
hal_ready(comp_id);
return 0;
}
/* signal handler */
static void quit(int sig)
{
if ( ignore_sig ) {
return;
}
if ( shmem_id >= 0 ) {
rtapi_shmem_delete(shmem_id, comp_id);
}
if ( comp_id >= 0 ) {
hal_exit(comp_id);
}
exit(exitval);
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
// detach from all rings, and delete them
int n;
for (n = 0; n < count; n++) {
if (instance[n]) {
hal_delayline_t *hd = instance[n]->scratchpad;
hal_ring_detach(instance[n]);
hal_ring_deletef("%s.samples", hd->name);
}
}
}