svd-ps_vfd.c
1 /* 2 svd-ps_vfd.c 3 4 This is a userspace program that interfaces the Soyan Power SVD-PS VFDs 5 to the LinuxCNC HAL. 6 7 Copyright (C) 2020 Tinic Uro 8 Copyright (C) 2017 Sebastian Kuzminsky 9 10 This program is free software; you can redistribute it and/or 11 modify it under the terms of the GNU Lesser General Public 12 License as published by the Free Software Foundation, version 2. 13 14 This program is distributed in the hope that it will be useful, 15 but WITHOUT ANY WARRANTY; without even the implied warranty of 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 General Public License for more details. 18 19 You should have received a copy of the GNU Lesser General Public 20 License along with this program; if not, write to the Free Software 21 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301-1307 USA. 22 */ 23 24 #include <errno.h> 25 #include <getopt.h> 26 #include <math.h> 27 #include <signal.h> 28 #include <stdio.h> 29 #include <stdlib.h> 30 #include <string.h> 31 #include <time.h> 32 #include <unistd.h> 33 34 #include <modbus.h> 35 36 #include "hal.h" 37 #include "rtapi.h" 38 39 40 // If a modbus transaction fails, retry this many times before giving up. 41 #define NUM_MODBUS_RETRIES 5 42 43 44 typedef struct { 45 hal_float_t *period; 46 47 hal_float_t *speed_cmd; 48 hal_float_t *freq_cmd; 49 hal_bit_t *at_speed; 50 51 hal_bit_t *spindle_on; 52 53 hal_u32_t *modbus_errors; 54 } haldata_t; 55 56 haldata_t *haldata; 57 58 int hal_comp_id; 59 60 61 typedef struct { 62 int address; // 0-0xffff for a register, -1 for end-of-list 63 const char *name; 64 65 // Multiply the uint16 value read by this multiplier to get the human- 66 // readable value. 67 float multiplier; 68 69 hal_float_t *hal_pin; 70 } modbus_register_t; 71 72 int num_modbus_registers = 0; 73 modbus_register_t *modbus_register; 74 75 76 static int done; 77 char *modname = "svd-ps_vfd"; 78 79 float motor_max_speed = 0.0; 80 float max_freq = 0.0; 81 float min_freq = 0.0; 82 83 int baud; 84 85 static struct option long_options[] = { 86 {"device", 1, 0, 'd'}, 87 {"rate", 1, 0, 'r'}, 88 {"bits", 1, 0, 'b'}, 89 {"parity", 1, 0, 'p'}, 90 {"stopbits", 1, 0, 's'}, 91 {"target", 1, 0, 't'}, 92 {"verbose", 0, 0, 'v'}, 93 {"help", 0, 0, 'h'}, 94 {"motor-max-speed", 1, 0, 'S'}, 95 {"max-frequency", 1, 0, 'F'}, 96 {"min-frequency", 1, 0, 'f'}, 97 {0,0,0,0} 98 }; 99 100 static char *option_string = "d:r:b:p:s:t:vhS:F:f:"; 101 102 static char *bitstrings[] = {"5", "6", "7", "8", NULL}; 103 104 static char *paritystrings[] = {"even", "odd", "none", NULL}; 105 static char paritychars[] = {'E', 'O', 'N'}; 106 107 static char *ratestrings[] = {"1200", "2400", "4800", "9600", "19200", "38400", NULL}; 108 static char *stopstrings[] = {"1", "2", NULL}; 109 110 111 static void quit(int sig) { 112 done = 1; 113 } 114 115 116 int match_string(char *string, char **matches) { 117 int len, which, match; 118 which=0; 119 match=-1; 120 if ((matches==NULL) || (string==NULL)) return -1; 121 len = strlen(string); 122 while (matches[which] != NULL) { 123 if ((!strncmp(string, matches[which], len)) && (len <= strlen(matches[which]))) { 124 if (match>=0) return -1; // multiple matches 125 match=which; 126 } 127 ++which; 128 } 129 return match; 130 } 131 132 133 void usage(int argc, char **argv) { 134 printf("Usage: %s [ARGUMENTS]\n", argv[0]); 135 printf( 136 "\n" 137 "This program interfaces the Soyan Power SVD-PS VFD to the LinuxCNC HAL.\n" 138 "\n" 139 "Required arguments:\n" 140 " -S, --motor-max-speed SPEED\n" 141 " The motor's max speed in RPM.\n" 142 " -F, --max-frequency F\n" 143 " This is the maximum output frequency of the VFD in Hz.\n" 144 " -f, --min-frequency F\n" 145 " This is the minimum output frequency of the VFD in Hz.\n" 146 "\n" 147 "Optional arguments:\n" 148 " -d, --device PATH (default /dev/ttyS0)\n" 149 " Set the name of the serial device to use.\n" 150 " -r, --rate RATE (default 9600)\n" 151 " Set baud rate to RATE. It is an error if the rate is not one of\n" 152 " the following: 1200, 2400, 4800, 9600, 19200, 38400\n" 153 " -b, --bits BITS (default 8)\n" 154 " Set number of data bits to BITS, must be between 5 and 8 inclusive.\n" 155 " -p, --parity PARITY (default none)\n" 156 " Set serial parity to one of 'even', 'odd', or 'none'.\n" 157 " -s, --stopbits {1,2} (default 2)\n" 158 " Set serial stop bits to 1 or 2.\n" 159 " -t, --target TARGET (default 1)\n" 160 " Set Modbus target (slave) number. This must match the device\n" 161 " number you set on the SVD-PS VFD.\n" 162 " -v, --verbose\n" 163 " Turn on verbose mode.\n" 164 " -h, --help\n" 165 " Show this help.\n" 166 ); 167 } 168 169 170 void svd_modbus_sleep(void) { 171 float seconds_per_bit = 1.0 / (float)baud; 172 useconds_t useconds_per_bit = seconds_per_bit * 1000 * 1000; 173 useconds_t delay = useconds_per_bit * 8 * 5; 174 usleep(delay); 175 } 176 177 178 int set_motor_on_forward(modbus_t *mb) { 179 int r; 180 uint16_t addr = 0x2000; 181 uint16_t val = 1; 182 183 for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) { 184 svd_modbus_sleep(); 185 r = modbus_write_register(mb, addr, val); 186 if (r == 1) { 187 return 0; 188 } 189 fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno)); 190 *haldata->modbus_errors = *haldata->modbus_errors + 1; 191 } 192 return -1; 193 } 194 195 int set_motor_off(modbus_t *mb) { 196 int r; 197 uint16_t addr = 0x2000; 198 uint16_t val = 5; 199 200 for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) { 201 svd_modbus_sleep(); 202 r = modbus_write_register(mb, addr, val); 203 if (r == 1) { 204 return 0; 205 } 206 fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno)); 207 *haldata->modbus_errors = *haldata->modbus_errors + 1; 208 } 209 return -1; 210 } 211 212 213 int set_motor_frequency(modbus_t *mb, float freq) { 214 int r; 215 uint16_t addr = 0x1000; 216 uint16_t val; // 100 * the frequency percentage 217 val = (freq / max_freq) * 10000; 218 219 for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) { 220 svd_modbus_sleep(); 221 r = modbus_write_register(mb, addr, val); 222 if (r == 1) { 223 return 0; 224 } 225 fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno)); 226 *haldata->modbus_errors = *haldata->modbus_errors + 1; 227 } 228 return -1; 229 } 230 231 232 int read_modbus_register(modbus_t *mb, modbus_register_t *reg) { 233 uint16_t data; 234 int r; 235 236 for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) { 237 svd_modbus_sleep(); 238 r = modbus_read_registers(mb, reg->address, 1, &data); 239 if (r == 1) { 240 *reg->hal_pin = data * reg->multiplier; 241 return 0; 242 } 243 fprintf(stderr, "%s: error reading %s (register 0x%04x): %s\n", __func__, reg->name, reg->address, modbus_strerror(errno)); 244 *haldata->modbus_errors = *haldata->modbus_errors + 1; 245 } 246 return -1; 247 } 248 249 250 void read_modbus_registers(modbus_t *mb) { 251 for (int i = 0; i < num_modbus_registers; i ++) { 252 (void)read_modbus_register(mb, &modbus_register[i]); 253 } 254 } 255 256 257 modbus_register_t *add_modbus_register(modbus_t *mb, int address, const char *pin_name, float multiplier) { 258 int r; 259 modbus_register_t *reg; 260 261 reg = &modbus_register[num_modbus_registers]; 262 263 r = hal_pin_float_newf(HAL_OUT, ®->hal_pin, hal_comp_id, "%s.%s", modname, pin_name); 264 if (r != 0) { 265 return NULL; 266 } 267 268 reg->address = address; 269 reg->name = pin_name; 270 reg->multiplier = multiplier; 271 272 read_modbus_register(mb, reg); 273 274 num_modbus_registers ++; 275 276 return reg; 277 } 278 279 280 int main(int argc, char **argv) { 281 char *device; 282 int bits; 283 char parity; 284 int stopbits; 285 int verbose; 286 287 int retval = 0; 288 modbus_t *mb; 289 int slave; 290 struct timespec period_timespec; 291 char *endarg; 292 int opt; 293 int argindex, argvalue; 294 295 modbus_register_t *speed_fb_reg; 296 297 done = 0; 298 299 // assume that nothing is specified on the command line 300 device = "/dev/ttyS0"; 301 baud = 9600; 302 bits = 8; 303 parity = 'N'; 304 stopbits = 2; 305 306 verbose = 0; 307 308 /* slave / register info */ 309 slave = 1; 310 311 // process command line options 312 while ((opt = getopt_long(argc, argv, option_string, long_options, NULL)) != -1) { 313 switch (opt) { 314 case 'd': // device name, default /dev/ttyS0 315 // could check the device name here, but we'll leave it to the library open 316 if (strlen(optarg) > FILENAME_MAX) { 317 printf("ERROR: device node name is too long: %s\n", optarg); 318 retval = -1; 319 goto out_noclose; 320 } 321 device = strdup(optarg); 322 break; 323 324 case 'r': // Baud rate, 9600 default 325 argindex=match_string(optarg, ratestrings); 326 if (argindex<0) { 327 printf("ERROR: invalid baud rate: %s\n", optarg); 328 retval = -1; 329 goto out_noclose; 330 } 331 baud = atoi(ratestrings[argindex]); 332 break; 333 334 case 'b': // serial data bits, probably should be 8 (and defaults to 8) 335 argindex=match_string(optarg, bitstrings); 336 if (argindex<0) { 337 printf("ERROR: invalid number of bits: %s\n", optarg); 338 retval = -1; 339 goto out_noclose; 340 } 341 bits = atoi(bitstrings[argindex]); 342 break; 343 344 case 'p': // parity, should be a string like "even", "odd", or "none" 345 argindex=match_string(optarg, paritystrings); 346 if (argindex<0) { 347 printf("ERROR: invalid parity: %s\n", optarg); 348 retval = -1; 349 goto out_noclose; 350 } 351 parity = paritychars[argindex]; 352 break; 353 354 case 's': // stop bits, defaults to 2 355 argindex=match_string(optarg, stopstrings); 356 if (argindex<0) { 357 printf("ERROR: invalid number of stop bits: %s\n", optarg); 358 retval = -1; 359 goto out_noclose; 360 } 361 stopbits = atoi(stopstrings[argindex]); 362 break; 363 364 case 't': // target number (MODBUS ID), default 1 365 argvalue = strtol(optarg, &endarg, 10); 366 if ((*endarg != '\0') || (argvalue < 1) || (argvalue > 254)) { 367 printf("ERROR: invalid slave number: %s\n", optarg); 368 retval = -1; 369 goto out_noclose; 370 } 371 slave = argvalue; 372 break; 373 374 case 'v': 375 verbose = 1; 376 break; 377 378 case 'h': 379 usage(argc, argv); 380 exit(0); 381 break; 382 383 case 'S': 384 motor_max_speed = strtof(optarg, &endarg); 385 if ((*endarg != '\0') || (motor_max_speed == 0.0)) { 386 printf("%s: ERROR: invalid motor max speed: %s\n", modname, optarg); 387 exit(1); 388 } 389 break; 390 391 case 'F': 392 max_freq = strtof(optarg, &endarg); 393 if ((*endarg != '\0') || (max_freq == 0.0)) { 394 printf("%s: ERROR: invalid max freq: %s\n", modname, optarg); 395 exit(1); 396 } 397 break; 398 399 case 'f': 400 min_freq = strtof(optarg, &endarg); 401 if ((*endarg != '\0') || (min_freq == 0.0)) { 402 printf("%s: ERROR: invalid min freq: %s\n", modname, optarg); 403 exit(1); 404 } 405 break; 406 407 default: 408 usage(argc, argv); 409 exit(1); 410 break; 411 } 412 } 413 414 if (motor_max_speed == 0.0) { 415 printf("%s: must specify --motor-max-speed\n", modname); 416 exit(1); 417 } 418 419 if (max_freq == 0.0) { 420 printf("%s: must specify --max-frequency\n", modname); 421 exit(1); 422 } 423 424 if (min_freq == 0.0) { 425 printf("%s: must specify --min-frequency\n", modname); 426 exit(1); 427 } 428 429 if (min_freq > max_freq) { 430 printf("%s: min frequency (%f) must be less than max frequency (%f)\n", modname, min_freq, max_freq); 431 exit(1); 432 } 433 434 { 435 struct sigaction sa; 436 int r; 437 438 sa.sa_handler = quit; 439 sigemptyset(&sa.sa_mask); 440 441 // libmodbus sometimes segfaults if its system calls get 442 // interrupted by signals. This works around it by automatically 443 // restarting the system call instead of having it return EINTR. 444 sa.sa_flags = SA_RESTART; 445 446 r = sigaction(SIGINT, &sa, NULL); 447 if (r != 0) { 448 printf("failed to set SIGINT handler: %s\n", strerror(errno)); 449 exit(1); 450 } 451 r = sigaction(SIGTERM, &sa, NULL); 452 if (r != 0) { 453 printf("failed to set SIGTERM handler: %s\n", strerror(errno)); 454 exit(1); 455 } 456 } 457 458 printf("%s: device='%s', baud=%d, parity='%c', bits=%d, stopbits=%d, address=%d\n", 459 modname, device, baud, parity, bits, stopbits, slave); 460 461 mb = modbus_new_rtu(device, baud, parity, bits, stopbits); 462 if (mb == NULL) { 463 printf("%s: ERROR: couldn't open modbus serial device: %s\n", modname, modbus_strerror(errno)); 464 goto out_noclose; 465 } 466 467 { 468 struct timeval t; 469 470 // Set the response timeout. 471 t.tv_sec = 0; 472 t.tv_usec = 30 * 1000; 473 #if (LIBMODBUS_VERSION_CHECK(3, 1, 2)) 474 modbus_set_response_timeout(mb, t.tv_sec, t.tv_usec); 475 #else 476 modbus_set_response_timeout(mb, &t); 477 #endif 478 479 // Disable the byte timeout so it just waits for the complete 480 // response timeout instead. 481 #if (LIBMODBUS_VERSION_CHECK(3, 1, 2)) 482 t.tv_sec = 0; 483 t.tv_usec = 0; 484 modbus_set_byte_timeout(mb, t.tv_sec, t.tv_usec); 485 #else 486 t.tv_sec = -1; 487 modbus_set_byte_timeout(mb, &t); 488 #endif 489 } 490 491 retval = modbus_connect(mb); 492 if (retval != 0) { 493 printf("%s: ERROR: couldn't open serial device: %s\n", modname, modbus_strerror(errno)); 494 goto out_noclose; 495 } 496 497 modbus_set_debug(mb, verbose); 498 499 modbus_set_slave(mb, slave); 500 501 /* create HAL component */ 502 hal_comp_id = hal_init(modname); 503 if (hal_comp_id < 0) { 504 printf("%s: ERROR: hal_init failed\n", modname); 505 retval = hal_comp_id; 506 goto out_close; 507 } 508 509 haldata = (haldata_t *)hal_malloc(sizeof(haldata_t)); 510 if (haldata == NULL) { 511 printf("%s: ERROR: unable to allocate shared memory\n", modname); 512 retval = -1; 513 goto out_closeHAL; 514 } 515 516 retval = hal_pin_float_newf(HAL_IN, &(haldata->period), hal_comp_id, "%s.period-seconds", modname); 517 if (retval != 0) goto out_closeHAL; 518 519 retval = hal_pin_float_newf(HAL_IN, &(haldata->speed_cmd), hal_comp_id, "%s.speed-cmd", modname); 520 if (retval != 0) goto out_closeHAL; 521 522 retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_cmd), hal_comp_id, "%s.freq-cmd", modname); 523 if (retval != 0) goto out_closeHAL; 524 525 retval = hal_pin_bit_newf(HAL_OUT, &(haldata->at_speed), hal_comp_id, "%s.at-speed", modname); 526 if (retval != 0) goto out_closeHAL; 527 528 retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_on), hal_comp_id, "%s.spindle-on", modname); 529 if (retval != 0) goto out_closeHAL; 530 531 retval = hal_pin_u32_newf(HAL_OUT, &(haldata->modbus_errors), hal_comp_id, "%s.modbus-errors", modname); 532 if (retval != 0) goto out_closeHAL; 533 534 *haldata->period = 0.1; 535 536 *haldata->freq_cmd = 0.0; 537 *haldata->at_speed = 0; 538 539 *haldata->modbus_errors = 0; 540 541 modbus_register = (modbus_register_t *)hal_malloc(20 * sizeof(modbus_register_t)); 542 if (modbus_register == NULL) { 543 printf("%s: ERROR: unable to allocate memory\n", modname); 544 retval = -1; 545 goto out_closeHAL; 546 } 547 if (add_modbus_register(mb, 0x1001, "freq-fb", 0.01) == NULL) { 548 goto out_closeHAL; 549 } 550 if (add_modbus_register(mb, 0x1002, "dc-bus-voltage", 0.1) == NULL) { 551 goto out_closeHAL; 552 } 553 if (add_modbus_register(mb, 0x1003, "output-voltage", 1.0) == NULL) { 554 goto out_closeHAL; 555 } 556 if (add_modbus_register(mb, 0x1004, "output-current", 1.0) == NULL) { 557 goto out_closeHAL; 558 } 559 speed_fb_reg = add_modbus_register(mb, 0x1005, "speed-fb", 1.0); 560 if (speed_fb_reg == NULL) { 561 goto out_closeHAL; 562 } 563 if (add_modbus_register(mb, 0x1006, "output-power", 1.0) == NULL) { 564 goto out_closeHAL; 565 } 566 if (add_modbus_register(mb, 0x1009, "input-terminal", 1) == NULL) { 567 goto out_closeHAL; 568 } 569 if (add_modbus_register(mb, 0x100a, "AI1", 0.001) == NULL) { 570 goto out_closeHAL; 571 } 572 if (add_modbus_register(mb, 0x100b, "AI2", 0.001) == NULL) { 573 goto out_closeHAL; 574 } 575 576 // Activate HAL component 577 hal_ready(hal_comp_id); 578 579 while (done == 0) { 580 if (*haldata->period < 0.001) *haldata->period = 0.001; 581 if (*haldata->period > 2.0) *haldata->period = 2.0; 582 period_timespec.tv_sec = (time_t)(*haldata->period); 583 period_timespec.tv_nsec = (long)((*haldata->period - period_timespec.tv_sec) * 1000000000l); 584 nanosleep(&period_timespec, NULL); 585 586 read_modbus_registers(mb); 587 588 if (*haldata->spindle_on) { 589 set_motor_on_forward(mb); 590 *haldata->freq_cmd = (*haldata->speed_cmd / motor_max_speed) * max_freq; 591 set_motor_frequency(mb, *haldata->freq_cmd); 592 593 if ((fabs(*haldata->speed_cmd - *speed_fb_reg->hal_pin) / *haldata->speed_cmd) < 0.02) { 594 *haldata->at_speed = 1; 595 } else { 596 *haldata->at_speed = 0; 597 } 598 } else { 599 set_motor_off(mb); 600 *haldata->at_speed = 0; 601 *haldata->freq_cmd = 0.0; 602 } 603 604 } 605 606 usleep(10 * 1000); 607 set_motor_off(mb); 608 609 retval = 0; /* if we get here, then everything is fine, so just clean up and exit */ 610 out_closeHAL: 611 hal_exit(hal_comp_id); 612 out_close: 613 modbus_close(mb); 614 modbus_free(mb); 615 out_noclose: 616 return retval; 617 }