// // Copyright (C) 2009 scotta at CNCZone.com, alan_3301 at CNCZone.com // 2014 Benjamin Brockhaus // 2015 Sebastian Kuzminsky // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA // #ifndef ULAPI #error This is intended as a userspace component only. #endif #include #include #include #include #include #include #include #include #include #include "rtapi.h" #include "hal.h" #include "hy_comm.h" #define HYCOMM_MIN_OK 10 // bits in Status Data #define STATUS_SetF 0x00 #define STATUS_OutF 0x01 #define STATUS_OutA 0x02 #define STATUS_RoTT 0x03 #define STATUS_DCV 0x04 #define STATUS_ACV 0x05 #define STATUS_Cont 0x06 #define STATUS_Tmp 0x07 // control commands CNTR #define CONTROL_Run_Fwd 0x01 #define CONTROL_Run_Rev 0x11 #define CONTROL_Stop 0x08 // control responses CNST #define CONTROL_Run 0x01 #define CONTROL_Jog 0x02 #define CONTROL_Command_rf 0x04 #define CONTROL_Running 0x08 #define CONTROL_Jogging 0x10 #define CONTROL_Running_rf 0x20 #define CONTROL_Bracking 0x40 #define CONTROL_Track_Start 0x80 int hycomm_ok; // set hycomm-ok bit if last HYCOMM_OK transactions went well int debug; int slave = 1; /* HAL data struct */ typedef struct { // The HAL comp name will be set to , and all pin and parameter // names will begin with . hal_bit_t *enable; // bit to enable this component hal_float_t *Set_F; // frequency command hal_float_t *Out_F; // actual output frequency hal_float_t *Out_A; // actual output amps hal_float_t *RoTT; // actual motor rmp (based on VFD parameters) hal_float_t *DCV; // DC Volts (to be confirmed) hal_float_t *ACV; // AC Volts (to be confirmed) hal_float_t *Cont; hal_float_t *Tmp; // Temperature (to be confirmed) hal_bit_t *spindle_forward; // spindle forward input hal_bit_t *spindle_reverse; // spindle reverse input hal_bit_t *spindle_on; // spinlde on input hal_float_t *CNTR; // stores the status of the control request hal_float_t *CNST; // stores the response of the control request hal_bit_t *CNST_Run; // CNST Run bit hal_bit_t *CNST_Jog; // CNST Jog bit hal_bit_t *CNST_Command_rf; // CNST Run reverse / forward bit hal_bit_t *CNST_Running; // CNST Running bit hal_bit_t *CNST_Jogging; // CNST Jogging bit hal_bit_t *CNST_Running_rf; // CNST Jog reverse / forward bit hal_bit_t *CNST_Bracking; // CNST bracking bit hal_bit_t *CNST_Track_Start; // CNST track start bit hal_float_t *speed_command; // spindle speed command from EMC hal_float_t *freq_cmd; // calculated frequency command hal_float_t *max_freq; // PD005 Max Operating Frequency hal_float_t *base_freq; // PD004 Base Frequency hal_float_t *freq_lower_limit; // PD011 Frequency Lower Limit hal_float_t *rated_motor_voltage; // PD141 Rated Motor Voltage - as per motor name plate hal_float_t *rated_motor_current; // PD142 Rated Motor Current - as per motor name plate hal_u32_t *motor_poles; // PD143 Number of motor poles - from motor name plate hal_float_t *rated_motor_rev; // max motor speed (at max_freq). PD144 gets set to value corresponding to RPM at 50Hz hal_bit_t *hycomm_ok; // the last HYCOMM_OK transactions returned successfully hal_float_t *max_rpm; // calculated based on VFD max frequency setup parameter hal_float_t *spindle_speed_fb; // (out) reports current spindle speed hal_bit_t *spindle_at_speed; // (out) True when spindle is on and at commanded speed hal_float_t *spindle_at_speed_tolerance; // (in) hal_float_t retval; hal_s32_t errorcount; hal_float_t looptime; //hal_float_t motor_nameplate_hz; // speeds are scaled in Hz, not RPM //hal_float_t motor_nameplate_RPM; // nameplate RPM at default Hz //hal_float_t rpm_limit; // do-not-exceed output frequency //hal_bit_t *acc_dec_pattern; // if set: choose ramp times as defined in F500+F501 // if zero (default): choose ramp times as defined in ACC and DEC //hal_float_t upper_limit_hz; // VFD setup parameter - maximum output frequency in HZ //hal_bit_t old_run; // so we can detect changes in the run state //hal_bit_t old_dir; //hal_bit_t old_err_reset; //hal_u32_t old_cmd1_reg; // copy of last write to FA00 //hal_u32_t failed_reg; // remember register for failed hycomm transaction - aids debugging } haldata_t; static int done; static void quit_signal_handler(int sig) { done = 1; } char *modname = "hy_vfd"; static struct option long_options[] = { {"bits", 1, 0, 'b'}, {"device", 1, 0, 'd'}, {"debug", 0, 0, 'g'}, {"help", 0, 0, 'h'}, {"name", 1, 0, 'n'}, {"parity", 1, 0, 'p'}, {"rate", 1, 0, 'r'}, {"stopbits", 1, 0, 's'}, {"target", 1, 0, 't'}, {"max-frequency", 1, 0, 'F'}, {"min-frequency", 1, 0, 'f'}, {"base-frequency", 1, 0, 'B'}, {"motor-voltage", 1, 0, 'V'}, {"motor-current", 1, 0, 'I'}, {"motor-speed", 1, 0, 'S'}, {"motor-poles", 1, 0, 'P'}, {"register", 1, 0, 'x'}, {"regdump", 0, 0, 'y'}, {0,0,0,0} }; static char *option_string = "b:d:ghn:p:r:s:t:F:f:B:V:I:S:P:"; static char *bitstrings[] = {"5", "6", "7", "8", NULL}; static char *paritystrings[] = {"even", "odd", "none", NULL}; static char *ratestrings[] = {"110", "300", "600", "1200", "2400", "4800", "9600", "19200", "38400", "57600", "115200", NULL}; static char *stopstrings[] = {"1", "2", NULL}; int match_string(char *string, char **matches) { int len, which, match; which=0; match=-1; if ((matches==NULL) || (string==NULL)) return -1; len = strlen(string); while (matches[which] != NULL) { if ((!strncmp(string, matches[which], len)) && (len <= strlen(matches[which]))) { if (match>=0) return -1; // multiple matches match=which; } ++which; } return match; } void usage(int argc, char **argv) { printf("Usage: %s [options]\n", argv[0]); printf( "This is a userspace HAL program, typically loaded using the halcmd \"loadusr\" command:\n" " loadusr hy_vfd\n" "There are several command-line options. Options that have a set list of possible values may\n" " be set by using any number of characters that are unique. For example, --rate 5 will use\n" " a baud rate of 57600, since no other available baud rates start with \"5\"\n" "-b or --bits (default 8)\n" " Set number of data bits to , where n must be from 5 to 8 inclusive\n" "-d or --device (default /dev/ttyS0)\n" " Set the name of the serial device node to use\n" "-g or --debug\n" " Turn on debugging messages. Debug mode will cause all hycomm messages\n" " to be printed in hex on the terminal.\n" "-n or --name (default hy_vfd)\n" " Set the name of the HAL module. The HAL comp name will be set to , and all pin\n" " and parameter names will begin with .\n" "-p or --parity {even,odd,none} (default even)\n" " Set serial parity to even, odd, or none.\n" "-r or --rate (default 19200)\n" " Set baud rate to . It is an error if the rate is not one of the following:\n" " 110, 300, 600, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200\n" "-s or --stopbits {1,2} (default 1)\n" " Set serial stop bits to 1 or 2\n" "-t or --target (default 1)\n" " Set HYCOMM target (slave) number. This must match the device number you set on the Huanyang VFD.\n" "-F or --max-frequency \n" " Set VFD max frequency to Hz. This will be read from the VFD\n" " register 5 if not supplied on the command line.\n" "-f or --min-frequency \n" " Set VFD min frequency to Hz. This will be read from the VFD\n" " register 11 if not supplied on the command line.\n" "-B or --base-frequency \n" " Set VFD base frequency to Hz. This will be read from the VFD\n" " register 11 if not supplied on the command line.\n" "-V or --motor-voltage \n" " Set VFD max output voltage to (Volts). This will be read from the\n" " VFD register 141 if not supplied on the command line.\n" "-I or --motor-current \n" " Set VFD max output current to (Amps). This will be read from the\n" " VFD register 142 if not supplied on the command line.\n" "-S or --motor-speed \n" " Set the motor's max speed to (RPM). This will be computed from the\n" " 50 Hz RPM value read from the VFD register P144 if not supplied on the\n" " command line.\n" "-P or --motor-poles \n" " Set VFD register PD143 (Number of Motor Poles) to . This value\n" " comes from the motor's name plate.\n" "-x or --register PDnnn=mmm\n" " Set the specified register to the specified value at startup\n" " Generally a one-time setting. Up to ten registers at a time can be set by repeating the command\n" "-y or --regdump\n" " Print the values of all registers when the VFD is enabled\n" ); } int write_data(hycomm_param_t *hc_param, hycomm_data_t *hc_data, haldata_t *haldata) { int retval; int CNTR, old_CNTR; int CNST; hal_float_t hz_per_rpm; int freq_comp; int freq, old_freq; // calculate and set frequency register, limit the frequency (upper and lower to VFD set parameters hc_data->function = WRITE_FREQ_DATA; hc_data->parameter = 0x00; if ((*(haldata->spindle_forward) && !*(haldata->spindle_reverse)) && *(haldata->spindle_on)) { freq_comp = 1; } else if ((*(haldata->spindle_reverse) && !*(haldata->spindle_forward)) && *(haldata->spindle_on)) { freq_comp = -1; } else { freq_comp = 0; } hz_per_rpm = *haldata->max_freq / *(haldata->rated_motor_rev); freq = abs((int)((*(haldata->speed_command)+freq_comp)*hz_per_rpm*100)); // limit the frequency to the max and min as setup in the VFD if (freq > *(haldata->max_freq)*100) freq = *(haldata->max_freq)*100; if (freq < *(haldata->freq_lower_limit)*100) freq = *(haldata->freq_lower_limit)*100; old_freq = *(haldata->freq_cmd); if (freq != old_freq) { // commanded frequency has changed hc_data->data = freq; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->freq_cmd) = freq; } // update the control register hc_data->function = WRITE_CONTROL_DATA; hc_data->parameter = 0x00; if ((*(haldata->spindle_forward) && !*(haldata->spindle_reverse)) && *(haldata->spindle_on)) { CNTR = CONTROL_Run_Fwd; } else if ((*(haldata->spindle_reverse) && !*(haldata->spindle_forward)) && (*haldata->spindle_on)) { CNTR = CONTROL_Run_Rev; } else { CNTR = CONTROL_Stop; } old_CNTR = *(haldata->CNTR); hc_data->data = old_CNTR; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; if (CNTR != old_CNTR) { // CNTR register needs to be updated hc_data->data = CNTR; *(haldata->CNTR) = CNTR; } CNST = hc_data->ret_data; *(haldata->CNST) = CNST; if ((CNST & CONTROL_Run) != 0) { *(haldata->CNST_Run) = TRUE; } else { *(haldata->CNST_Run) = FALSE; } if ((CNST & CONTROL_Jog) != 0) { *(haldata->CNST_Jog) = TRUE; } else { *(haldata->CNST_Jog) = FALSE; } if ((CNST & CONTROL_Command_rf) != 0) { *(haldata->CNST_Command_rf) = TRUE; } else { *(haldata->CNST_Command_rf) = FALSE; } if ((CNST & CONTROL_Running) != 0) { *(haldata->CNST_Running) = TRUE; } else { *(haldata->CNST_Running) = FALSE; } if ((CNST & CONTROL_Jogging) != 0) { *(haldata->CNST_Jogging) = TRUE; } else { *(haldata->CNST_Jogging) = FALSE; } if ((CNST & CONTROL_Running_rf) != 0) { *(haldata->CNST_Running_rf) = TRUE; } else { *(haldata->CNST_Running_rf) = FALSE; } if ((CNST & CONTROL_Bracking) != 0) { *(haldata->CNST_Bracking) = TRUE; } else { *(haldata->CNST_Bracking) = FALSE; } if ((CNST & CONTROL_Track_Start) != 0) { *(haldata->CNST_Track_Start) = TRUE; } else { *(haldata->CNST_Track_Start) = FALSE; } retval = 0; haldata->retval = retval; return retval; failed: if (hc_param->debug) { printf("write_data: FAILED\n"); } haldata->retval = retval; haldata->errorcount++; retval = -1; return retval; } int read_setup(hycomm_param_t *hc_param, hycomm_data_t *hc_data, haldata_t *haldata) { int retval; float rpm_at_50hz; /* can't do anything with a null HAL data block */ if (haldata == NULL) return -1; /* but we can signal an error if the other params are null */ if (hc_param==NULL) { haldata->errorcount++; return -1; } if (debug & 0x2) { int reg; printf("Current Register values list\n"); for (reg = 1; reg < 184; reg++){ hc_data->function = FUNCTION_READ; hc_data->data = 0x0000; hc_data->parameter = reg; if ((retval = hy_comm(hc_param, hc_data)) != 0){ printf("PD%03d = INVALID\n", reg); } else { printf("PD%03d = %d\n", reg, hc_data->ret_data); } } } if (hc_param->debug) { printf("read_setup: reading setup parameters:\n"); } hc_data->function = FUNCTION_READ; hc_data->data = 0x0000; hc_data->parameter = 5; // PD005 Max Operating Frequency if (*haldata->max_freq != 0) { // user passed in motor max freq, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->max_freq * 100); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->max_freq) = hc_data->ret_data * 0.01; hc_data->parameter = 4; // PD004 Base Frequency if (*haldata->base_freq != 0) { // user passed in base freq, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->base_freq * 100); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->base_freq) = hc_data->ret_data * 0.01; hc_data->parameter = 11; // PD011 Frequency Lower Limit if (*haldata->freq_lower_limit != 0) { // user passed in motor min freq, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->freq_lower_limit * 100); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->freq_lower_limit) = hc_data->ret_data * 0.01; hc_data->parameter = 141; // PD141 Rated Motor Voltage if (*haldata->rated_motor_voltage != 0) { // user passed in motor max voltage, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->rated_motor_voltage * 10); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->rated_motor_voltage) = hc_data->ret_data * 0.1; hc_data->parameter = 142; // PD142 Rated Motor Current if (*haldata->rated_motor_current != 0) { // user passed in motor max current, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->rated_motor_current * 10); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->rated_motor_current) = hc_data->ret_data * 0.1; hc_data->parameter = 143; // PD143 Number of Motor Poles if (*haldata->motor_poles != 0) { // user passed in motor poles, send to VFD hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)(*haldata->motor_poles); if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *haldata->motor_poles = hc_data->ret_data; hc_data->parameter = 144; // PD144 Rated Motor Rev (at 50 Hz) if (*haldata->rated_motor_rev != 0) { // user passed in motor max speed // we know motor max freq // write the VFD's P144 with "motor speed at 50 Hz" rpm_at_50hz = (*haldata->rated_motor_rev / *haldata->max_freq) * 50.0; hc_data->function = FUNCTION_WRITE; hc_data->data = (uint16_t)rpm_at_50hz; if ((retval = hy_comm(hc_param, hc_data)) != 0) { goto failed; } hc_data->function = FUNCTION_READ; } if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; rpm_at_50hz = hc_data->ret_data; *(haldata->rated_motor_rev) = (rpm_at_50hz / 50.0) * *haldata->max_freq; // Handle explicitly set registers for (int i = 0;hc_param->extra_reg[i] != 0; i++){ hc_data->parameter = hc_param->extra_reg[i]; hc_data->function = FUNCTION_WRITE; hc_data->data = hc_param->extra_val[i]; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; hc_data->function = FUNCTION_READ; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; if (hc_param->debug) printf ("Register %i successfully set to %i\n", hc_param->extra_reg[i], hc_param->extra_val[i]); } if (hc_param->debug) { printf("read_setup: read setup parameters - OK.\n"); } fflush(NULL); retval = 0; haldata->retval = retval; return retval; failed: if (hc_param->debug) { printf("read_setup: FAILED\n"); } haldata->retval = retval; haldata->errorcount++; retval = -1; return retval; } int read_data(hycomm_param_t *hc_param, hycomm_data_t *hc_data, haldata_t *haldata) { int retval; // Read the Status Data registers hc_data->function = READ_CONTROL_STATUS; hc_data->parameter = 0x00; hc_data->data = STATUS_SetF; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->Set_F) = hc_data->ret_data * 0.01; if (hc_param->debug) { printf("Set_F = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_OutF; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->Out_F) = hc_data->ret_data * 0.01; if (hc_param->debug) { printf("Out_F = [%.2X]", hc_data->ret_data); printf("\n"); } if ( *haldata->spindle_on && (fabs(*haldata->speed_command) > 0.0) && (*haldata->Set_F > 0.0) ) { *haldata->spindle_speed_fb = (*haldata->Out_F / *haldata->max_freq) * *haldata->rated_motor_rev; if (fabs(1 - (*haldata->spindle_speed_fb / fabs(*haldata->speed_command))) < *haldata->spindle_at_speed_tolerance) { *haldata->spindle_at_speed = 1; } else { *haldata->spindle_at_speed = 0; } } else { *haldata->spindle_speed_fb = 0.0; *haldata->spindle_at_speed = 0; } hc_data->data = STATUS_OutA; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->Out_A) = hc_data->ret_data * 0.1; if (hc_param->debug) { printf("Out_A = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_RoTT; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->RoTT) = hc_data->ret_data; if (hc_param->debug) { printf("RoTT = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_DCV; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->DCV) = hc_data->ret_data; if (hc_param->debug) { printf("DCV = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_ACV; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->ACV) = hc_data->ret_data; if (hc_param->debug) { printf("ACV = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_Cont; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->Cont) = hc_data->ret_data; if (hc_param->debug) { printf("Cont = [%.2X]", hc_data->ret_data); printf("\n"); } hc_data->data = STATUS_Tmp; if ((retval = hy_comm(hc_param, hc_data)) != 0) goto failed; *(haldata->Tmp) = hc_data->ret_data; if (hc_param->debug) { printf("Tmp = [%.2X]", hc_data->ret_data); printf("\n"); } retval = 0; haldata->retval = retval; return retval; failed: if (hc_param->debug) { printf("read_data: FAILED\n"); } haldata->retval = retval; haldata->errorcount++; retval = -1; return retval; } int main(int argc, char **argv) { int retval; hycomm_param_t hc_param; hycomm_data_t hc_data; haldata_t *haldata; int hal_comp_id; struct timespec loop_timespec, remaining; int baud, bits, stopbits; char *device, *parity, *endarg; int opt; int argindex, argvalue; int extra_reg[10], extra_val[10]; int extra_index = -1; float max_freq = 0; float base_freq = 0; float min_freq = 0; float motor_v = 0; float motor_i = 0; float motor_speed = 0; hal_u32_t motor_poles = 0; done = 0; // assume that nothing is specified on the command line baud = 19200; bits = 8; stopbits = 1; debug = 0; device = "/dev/ttyS0"; parity = "even"; slave = 1; // process command line options while ((opt=getopt_long(argc, argv, option_string, long_options, NULL)) != -1) { switch(opt) { case 'x': // not in list, is it an explicit PD number? if (++extra_index < 10){ retval = sscanf(optarg, "PD%d=%d", &extra_reg[extra_index],&extra_val[extra_index]); if (retval <0) { printf("Too many direct register writes, max is 10"); goto out_noclose; } if (debug & 1) printf("will set register %i to %i\n", extra_reg[extra_index],extra_val[extra_index]); } break; case 'b': // serial data bits, probably should be 8 (and defaults to 8) argindex=match_string(optarg, bitstrings); if (argindex<0) { printf("hy_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("hy_vfd: ERROR: device node name is too long: %s\n", optarg); retval = -1; goto out_noclose; } device = strdup(optarg); break; case 'g': debug |= 1; break; case 'y': debug |= 2; break; case 'n': // module base name if (strlen(optarg) > HAL_NAME_LEN-20) { printf("hy_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("hy_vfd: ERROR: invalid parity: %s\n", optarg); retval = -1; goto out_noclose; } parity = paritystrings[argindex]; break; case 'r': // Baud rate, 19200 default argindex=match_string(optarg, ratestrings); if (argindex<0) { printf("hy_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("hy_vfd: ERROR: invalid number of stop bits: %s\n", optarg); retval = -1; goto out_noclose; } stopbits = atoi(stopstrings[argindex]); break; case 't': // target number (HYCOMM ID), default 1 argvalue = strtol(optarg, &endarg, 10); if ((*endarg != '\0') || (argvalue < 1) || (argvalue > 254)) { printf("hy_vfd: ERROR: invalid slave number: %s\n", optarg); retval = -1; goto out_noclose; } slave = argvalue; break; case 'F': max_freq = strtof(optarg, &endarg); if ((*endarg != '\0') || (max_freq < 0.0) || (max_freq > UINT16_MAX)) { printf("hy_vfd: ERROR: invalid max frequency: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'B': base_freq = strtof(optarg, &endarg); if ((*endarg != '\0') || (base_freq < 0.0) || (base_freq > 400.0)) { printf("hy_vfd: ERROR: invalid base frequency: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'f': min_freq = strtof(optarg, &endarg); if ((*endarg != '\0') || (min_freq < 0.0) || (min_freq > UINT16_MAX)) { printf("hy_vfd: ERROR: invalid min frequency: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'V': motor_v = strtof(optarg, &endarg); if ((*endarg != '\0') || (motor_v < 0.0) || (motor_v > UINT16_MAX)) { printf("hy_vfd: ERROR: invalid motor max voltage: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'I': motor_i = strtof(optarg, &endarg); if ((*endarg != '\0') || (motor_i < 0.0) || (motor_i > UINT16_MAX)) { printf("hy_vfd: ERROR: invalid motor max current: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'S': motor_speed = strtof(optarg, &endarg); if ((*endarg != '\0') || (motor_speed < 0.0) || (motor_speed > UINT16_MAX)) { printf("hy_vfd: ERROR: invalid motor max speed: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'P': motor_poles = strtof(optarg, &endarg); if (*endarg != '\0') { printf("hy_vfd: ERROR: invalid motor poles: %s\n", optarg); retval = -1; goto out_noclose; } break; case 'h': default: usage(argc, argv); exit(0); break; } } if (debug & 1) { printf("%s: device='%s', baud=%d, bits=%d, parity='%s', stopbits=%d, address=%d, debug=%d, PID=%d\n", modname,device, baud, bits, parity, stopbits, slave, debug, getpid()); } /* SIGTERM and SIGINT trigger a clean shutdown */ signal(SIGINT, quit_signal_handler); signal(SIGTERM, quit_signal_handler); /* Assume 19.2k E-8-1 serial settings, device 1 */ hycomm_init(&hc_param, device, baud, parity, bits, stopbits); hc_param.debug = debug & 1; hc_param.print_errors = 1; /* the open has got to work, or we're out of business */ if (((retval = hycomm_connect(&hc_param))!=0) || done) { printf("%s: ERROR: couldn't open serial device\n", modname); goto out_noclose; } /* 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_bit_newf(HAL_IN, &(haldata->enable), hal_comp_id, "%s.enable", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->Set_F), hal_comp_id, "%s.SetF", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->Out_F), hal_comp_id, "%s.OutF", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->Out_A), hal_comp_id, "%s.OutA", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->RoTT), hal_comp_id, "%s.Rott", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->DCV), hal_comp_id, "%s.DCV", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->ACV), hal_comp_id, "%s.ACV", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->Cont), hal_comp_id, "%s.Cont", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->Tmp), hal_comp_id, "%s.Tmp", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_forward), hal_comp_id, "%s.spindle-forward", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_reverse), hal_comp_id, "%s.spindle-reverse", 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_float_newf(HAL_OUT, &(haldata->CNTR), hal_comp_id, "%s.CNTR", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->CNST), hal_comp_id, "%s.CNST", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Run), hal_comp_id, "%s.CNST-run", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Jog), hal_comp_id, "%s.CNST-jog", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Command_rf), hal_comp_id, "%s.CNST-command-rf", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Running), hal_comp_id, "%s.CNST-running", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Jogging), hal_comp_id, "%s.CNST-jogging", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Running_rf), hal_comp_id, "%s.CNST-running-rf", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Bracking), hal_comp_id, "%s.CNST-bracking", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->CNST_Track_Start), hal_comp_id, "%s.CNST-track-start", modname); 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_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->max_freq), hal_comp_id, "%s.max-freq", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->base_freq), hal_comp_id, "%s.base-freq", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_lower_limit), hal_comp_id, "%s.freq-lower-limit", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->rated_motor_voltage), hal_comp_id, "%s.rated-motor-voltage", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->rated_motor_current), hal_comp_id, "%s.rated-motor-current", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->rated_motor_rev), hal_comp_id, "%s.rated-motor-rev", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_u32_newf(HAL_OUT, &(haldata->motor_poles), hal_comp_id, "%s.motor-poles", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->hycomm_ok), hal_comp_id, "%s.hycomm-ok", 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->retval), hal_comp_id, "%s.retval", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_OUT, &(haldata->spindle_speed_fb), hal_comp_id, "%s.spindle-speed-fb", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_bit_newf(HAL_OUT, &(haldata->spindle_at_speed), hal_comp_id, "%s.spindle-at-speed", modname); if (retval!=0) goto out_closeHAL; retval = hal_pin_float_newf(HAL_IN, &(haldata->spindle_at_speed_tolerance), hal_comp_id, "%s.spindle-at-speed-tolerance", modname); if (retval!=0) goto out_closeHAL; /* make default data match what we expect to use */ *(haldata->enable) = 0; *(haldata->Set_F) = 0; *(haldata->Out_F) = 0; *(haldata->Out_A) = 0; *(haldata->RoTT) = 0; *(haldata->DCV) = 0; *(haldata->ACV) = 0; *(haldata->Cont) = 0; *(haldata->Tmp) = 0; *(haldata->spindle_forward) = 0; *(haldata->spindle_reverse) = 0; *(haldata->spindle_on) = 0; *(haldata->freq_cmd) = 0; *(haldata->CNTR) = 0; *(haldata->CNST) = 0; *(haldata->max_freq) = max_freq; *(haldata->base_freq) = base_freq; *(haldata->freq_lower_limit) = min_freq; *(haldata->rated_motor_voltage) = motor_v; *(haldata->rated_motor_current) = motor_i; *(haldata->rated_motor_rev) = motor_speed; *(haldata->motor_poles) = motor_poles; *(haldata->hycomm_ok) = 0; *haldata->spindle_speed_fb = 0.0; *haldata->spindle_at_speed = 0; *haldata->spindle_at_speed_tolerance = 0.02; hc_data.slave = slave; haldata->errorcount = 0; haldata->looptime = 0.1; //haldata->motor_nameplate_hz = 50; // folks in The Colonies typically would use 60Hz and 1730 rpm //haldata->motor_nameplate_RPM = 1410; //haldata->rpm_limit = MAX_RPM; //haldata->acc_dec_pattern = 0; //haldata->old_run = -1; // make sure the initial value gets output //haldata->old_dir = -1; //haldata->old_err_reset = -1; //haldata->failed_reg = 0; hal_ready(hal_comp_id); hc_data.slave = slave; // wait until EMC and AXIS is ready, ie enable bit is set while (!*(haldata->enable)){ // do nothing until enabled usleep(10*1000); if (done) { // killed by signal exit(0); } } // read the VFD setup parameters for (;extra_index >= 0; extra_index--){ hc_param.extra_reg[extra_index] = extra_reg[extra_index]; hc_param.extra_val[extra_index] = extra_val[extra_index]; } do { retval = read_setup(&hc_param, &hc_data, haldata); if (retval != 0) { fprintf(stderr, "error reading setup from VFD, retrying\n"); usleep(100 * 1000); } if (done) { // killed by signal exit(0); } } while (retval != 0); // here's the meat of the program. loop until done (which may be never) while (done==0) { if (*(haldata->enable)) { // Read inputs if (read_data(&hc_param, &hc_data, haldata) < 0) { hycomm_ok = 0; } else { hycomm_ok++; } // Set outputs if (write_data(&hc_param, &hc_data, haldata) < 0) { hycomm_ok = 0; } else { hycomm_ok++; } } if (hycomm_ok > HYCOMM_MIN_OK) { *(haldata->hycomm_ok) = 1; } else { *(haldata->hycomm_ok) = 0; } // 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 > 1.0) haldata->looptime = 1.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: hycomm_close(&hc_param); out_noclose: return retval; }