int rtapi_app_main(void)
{
int n, retval;
retval = setup_user_step_type();
if(retval < 0) {
return retval;
}
for (n = 0; n < MAX_CHAN && step_type[n] != -1 ; n++) {
if ((step_type[n] > MAX_STEP_TYPE) || (step_type[n] < 0)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: bad stepping type '%i', axis %i\n",
step_type[n], n);
return -1;
}
if(parse_ctrl_type(ctrl_type[n]) == INVALID) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: bad control type '%s' for axis %i (must be 'p' or 'v')\n",
ctrl_type[n], n);
return -1;
}
num_chan++;
}
if (num_chan == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: no channels configured\n");
return -1;
}
/* periodns will be set to the proper value when 'make_pulses()' runs for
the first time. We load a default value here to avoid glitches at
startup, but all these 'constants' are recomputed inside
'update_freq()' using the real period. */
old_periodns = periodns = 50000;
old_dtns = 1000000;
/* precompute some constants */
periodfp = periodns * 0.000000001;
freqscale = (1L << PICKOFF) * periodfp;
accelscale = freqscale * periodfp;
dt = old_dtns * 0.000000001;
recip_dt = 1.0 / dt;
/* have good config info, connect to the HAL */
comp_id = hal_init("stepgen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for counter data */
stepgen_array = hal_malloc(num_chan * sizeof(stepgen_t));
if (stepgen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export all the variables for each pulse generator */
for (n = 0; n < num_chan; n++) {
/* export all vars */
retval = export_stepgen(n, &(stepgen_array[n]),
step_type[n], (parse_ctrl_type(ctrl_type[n]) == POSITION));
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: stepgen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
/* export functions */
retval = hal_export_funct("stepgen.make-pulses", make_pulses,
stepgen_array, 0, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: makepulses funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("stepgen.update-freq", update_freq,
stepgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: freq update funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("stepgen.capture-position", update_pos,
stepgen_array, 1, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STEPGEN: ERROR: pos update funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"STEPGEN: installed %d step pulse generators\n", num_chan);
hal_ready(comp_id);
return 0;
}
/**
\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;
}
