Merge pull request #677 from tinic/master

Add support for Soyan Power SVD-P(S) VFDs

docs/man/.gitignore

@@ -7,6 +7,7 @@ cat3/
 cat9/
 
 man1/hy_gt_vfd.1
+man1/svd-ps_vfd.1
 man1/linuxcnc.1
 man1/halstreamer.1
 man1/thermistor.1

docs/src/man/man1/svd-pd_vfd.1.txt

@@ -0,0 +1,174 @@
+= svd-ps_vfd(1)
+
+
+== NAME
+
+svd-ps_vfd - HAL userspace component for SVD-P(S) VFDs
+
+
+== SYNOPSIS
+
+*svd-ps_vfd* [_OPTIONS_]
+
+== DESCRIPTION
+
+The svd-ps_vfd component interfaces a Soyan Power SVD-P(S) VFD to the
+LinuxCNC HAL.  The VFD is connected via RS-485 to the LinuxCNC computer.
+
+The SVD-P(S) VFDs are also sold under the LAPOND brand.
+
+== Hardware setup
+
+The SVD-P(S) VFDs do not come with a Modbus daughterboard by default,
+it needs to be purchased separately.
+
+== Firmware setup
+
+The sad-ps_vfd component uses standard Modbus protocol communication
+which requires that one parameter be changed from the default settings:
+
+PD-05 = 1 (Standard Modbus protocol)
+
+The following settings have been tested successfully and are the
+default per Soyans documentation:
+
+PD-00 = 6005 (9600 baud)
+
+PD-01 = 0 (8N2)
+
+PD-02 = 1 (Slave address)
+
+PD-03 = 2 (Response delay)
+
+PD-04 = 0 (Communication timeout)
+
+PD-06 = 0 (Current resolution)
+
+== OPTIONS
+
+*-b*, *--bits* _N_::
+
+      (default 8) For Modbus communication.  Set number of data bits
+      to _N_.  _N_ must be between 5 and 8 inclusive.
+
+*-p*, *--parity* [Even,Odd,None]
+
+      (default None) For Modbus communication.  Set serial parity to Even,
+      Odd, or None.
+
+*-r*, *--rate* _N_::
+
+      (default 9600) For Modbus communication.  Set baud rate to _N_.
+      It is an error if the rate is not one of the following: 1200,
+      2400, 4800, 9600, 19200, 38400
+
+*-s*, *--stopbits* [1,2]::
+
+      (default 2) For Modbus communication.  Set serial stop bits to 1
+      or 2.
+
+*-t*, *--target* _N_::
+
+      (default 1) For Modbus communication.  Set Modbus target (slave)
+      number.  This must match the device number you set on the Huanyang
+      GT VFD.
+
+*-d*, *--device* _PATH_::
+
+      (default /dev/ttyS0) For Modbus communication.  Set the name of
+      the serial device node to use.
+
+*-v*, *--verbose*::
+
+      Turn on verbose mode.
+
+*-S*, *--motor-max-speed* _RPM_::
+
+    The motor's max speed in RPM.
+
+*-F*, *--max-frequency* _HZ_::
+
+    This is the maximum output frequency of the VFD in Hz. 
+
+*-f*, *--min-frequency* _HZ_::
+
+    This is the minimum output frequency of the VFD in Hz. 
+
+== PINS
+
+
+*svd-ps_vfd.period* (float, in)::
+
+      The period for the driver's update cycle, in seconds. This is
+      how frequently the driver will wake up, check its HAL pins, and
+      communicate with the VFD.  Must be between 0.001 and 2.000 seconds.
+      Default: 0.1 seconds.
+
+*svd-ps_vfd.speed-cmd* (float, in)::
+
+      The requested motor speed, in RPM.
+
+*svd-ps_vfd.speed-fb* (float, out)::
+
+      The motor's current speed, in RPM, reported by the VFD.
+
+*svd-ps_vfd.at-speed* (bit, out)::
+
+      True when the drive is on and at the commanded speed (within 2%),
+      False otherwise.
+
+*svd-ps_vfd.freq-cmd* (float, out)::
+
+      The requested output frequency, in Hz.  This is set from the
+      .speed-cmd value, and is just shown for debugging purposes.
+
+*svd-ps_vfd.freq-fb* (float, out)::
+
+      The current output frequency of the VFD, in Hz.  This is reported
+      from the VFD to the driver.
+
+*svd-ps_vfd.spindle-on* (bit, in)::
+
+      Set this pin True to command the spindle on, at the speed requested
+      on the .speed-cmd pin.  Set this pin False to command the spindle
+      off.
+
+*svd-ps_vfd.output-voltage* (float, out)::
+
+      The voltage that the VFD is current providing to the motor,
+      in Volts.
+
+*svd-ps_vfd.output-current* (float, out)::
+
+      The current that the motor is currently drawing from the VFD,
+      in Amperes.
+
+*hsvd-ps_vfd.output-power* (float, out)::
+
+      The power that the motor is currently drawing from the VFD,
+      in Watts.
+
+*svd-ps_vfd.dc-bus-voltage* (float, out)::
+
+      The current voltage of the VFD's internal DC power supply, in Volts.
+
+*svd-ps_vfd.modbus-errors* (u32, out)::
+
+      A count of the number of modbus communication errors between the
+      driver and the VFD.  The driver is resilient against communication
+      errors, but a large or growing number here indicates a problem
+      that should be investigated.
+
+*svd-ps_vfd.input-terminal* (float, out)::
+
+        The VFD's input terminal register.
+
+*svd-ps_vfd.AI1* (float, out)::
+
+        The VFD's AI1 register.
+
+*svd-ps_vfd.AI2* (float, out)::
+
+        The VFD's AI2 register.
+
+== ISSUES

src/hal/user_comps/Submakefile

@@ -44,6 +44,19 @@ $(call TOOBJSDEPS, $(HY_GT_SRCS)) : EXTRAFLAGS += $(HY_GT_CCFLAGS)
 USERSRCS += $(HY_GT_SRCS)
 TARGETS += ../bin/hy_gt_vfd
 
+SVD_PS_SRCS := hal/user_comps/svd-ps_vfd.c
+SVD_PS_LDFLAGS := $(LIBMODBUS_LIBS)
+SVD_PS_CCFLAGS := $(LIBMODBUS_CFLAGS)
+
+$(call TOOBJSDEPS, $(SVD_PS_SRCS)) : EXTRAFLAGS += $(SVD_PS_CCFLAGS)
+
+../bin/svd-ps_vfd: $(call TOOBJS, $(SVD_PS_SRCS)) ../lib/liblinuxcnchal.so.0
+	$(ECHO) Linking $(notdir $@)
+	$(Q)$(CC) -o $@ $^ $(LDFLAGS) $(SVD_PS_LDFLAGS)
+
+USERSRCS += $(SVD_PS_SRCS)
+TARGETS += ../bin/svd-ps_vfd
+
 endif # HAVE_LIBMODBUS
 
 

src/hal/user_comps/svd-ps_vfd.c

@@ -0,0 +1,617 @@
+/*
+    svd-ps_vfd.c
+
+    This is a userspace program that interfaces the Soyan Power SVD-PS VFDs
+    to the LinuxCNC HAL.
+
+    Copyright (C) 2020 Tinic Uro
+    Copyright (C) 2017 Sebastian Kuzminsky
+
+    This program is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation, version 2.
+
+    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 Lesser General Public
+    License along with this program; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301-1307 USA.
+*/
+
+#include <errno.h>
+#include <getopt.h>
+#include <math.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <modbus.h>
+
+#include "hal.h"
+#include "rtapi.h"
+
+
+// If a modbus transaction fails, retry this many times before giving up.
+#define NUM_MODBUS_RETRIES 5
+
+
+typedef struct {
+    hal_float_t *period;
+
+    hal_float_t *speed_cmd;
+    hal_float_t *freq_cmd;
+    hal_bit_t *at_speed;
+
+    hal_bit_t	*spindle_on;
+
+    hal_u32_t *modbus_errors;
+} haldata_t;
+
+haldata_t *haldata;
+
+int hal_comp_id;
+
+
+typedef struct {
+    int address;  // 0-0xffff for a register, -1 for end-of-list
+    const char *name;
+
+    // Multiply the uint16 value read by this multiplier to get the human-
+    // readable value.
+    float multiplier;
+
+    hal_float_t *hal_pin;
+} modbus_register_t;
+
+int num_modbus_registers = 0;
+modbus_register_t *modbus_register;
+
+
+static int done;
+char *modname = "svd-ps_vfd";
+
+float motor_max_speed = 0.0;
+float max_freq = 0.0;
+float min_freq = 0.0;
+
+int baud;
+
+static struct option long_options[] = {
+    {"device", 1, 0, 'd'},
+    {"rate", 1, 0, 'r'},
+    {"bits", 1, 0, 'b'},
+    {"parity", 1, 0, 'p'},
+    {"stopbits", 1, 0, 's'},
+    {"target", 1, 0, 't'},
+    {"verbose", 0, 0, 'v'},
+    {"help", 0, 0, 'h'},
+    {"motor-max-speed", 1, 0, 'S'},
+    {"max-frequency", 1, 0, 'F'},
+    {"min-frequency", 1, 0, 'f'},
+    {0,0,0,0}
+};
+
+static char *option_string = "d:r:b:p:s:t:vhS:F:f:";
+
+static char *bitstrings[] = {"5", "6", "7", "8", NULL};
+
+static char *paritystrings[] = {"even", "odd", "none", NULL};
+static char paritychars[] = {'E', 'O', 'N'};
+
+static char *ratestrings[] = {"1200", "2400", "4800", "9600", "19200", "38400", NULL};
+static char *stopstrings[] = {"1", "2", NULL};
+
+
+static void quit(int sig) {
+    done = 1;
+}
+
+
+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 [ARGUMENTS]\n", argv[0]);
+    printf(
+        "\n"
+        "This program interfaces the Soyan Power SVD-PS VFD to the LinuxCNC HAL.\n"
+        "\n"
+        "Required arguments:\n"
+        "    -S, --motor-max-speed SPEED\n"
+        "        The motor's max speed in RPM.\n"
+        "    -F, --max-frequency F\n"
+        "        This is the maximum output frequency of the VFD in Hz.\n"
+        "    -f, --min-frequency F\n"
+        "        This is the minimum output frequency of the VFD in Hz.\n"
+        "\n"
+        "Optional arguments:\n"
+        "    -d, --device PATH (default /dev/ttyS0)\n"
+        "        Set the name of the serial device to use.\n"
+        "    -r, --rate RATE (default 9600)\n"
+        "        Set baud rate to RATE.  It is an error if the rate is not one of\n"
+        "        the following: 1200, 2400, 4800, 9600, 19200, 38400\n"
+        "    -b, --bits BITS (default 8)\n"
+        "        Set number of data bits to BITS, must be between 5 and 8 inclusive.\n"
+        "    -p, --parity PARITY (default none)\n"
+        "        Set serial parity to one of 'even', 'odd', or 'none'.\n"
+        "    -s, --stopbits {1,2} (default 2)\n"
+        "        Set serial stop bits to 1 or 2.\n"
+        "    -t, --target TARGET (default 1)\n"
+        "        Set Modbus target (slave) number.  This must match the device\n"
+        "        number you set on the SVD-PS VFD.\n"
+        "    -v, --verbose\n"
+        "        Turn on verbose mode.\n"
+        "    -h, --help\n"
+        "        Show this help.\n"
+    );
+}
+
+
+void svd_modbus_sleep(void) {
+    float seconds_per_bit = 1.0 / (float)baud;
+    useconds_t useconds_per_bit = seconds_per_bit * 1000 * 1000;
+    useconds_t delay = useconds_per_bit * 8 * 5;
+    usleep(delay);
+}
+
+
+int set_motor_on_forward(modbus_t *mb) {
+    int r;
+    uint16_t addr = 0x2000;
+    uint16_t val = 1;
+
+    for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) {
+        svd_modbus_sleep();
+        r = modbus_write_register(mb, addr, val);
+        if (r == 1) {
+            return 0;
+        }
+        fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno));
+        *haldata->modbus_errors = *haldata->modbus_errors + 1;
+    }
+    return -1;
+}
+
+int set_motor_off(modbus_t *mb) {
+    int r;
+    uint16_t addr = 0x2000;
+    uint16_t val = 5;
+
+    for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) {
+        svd_modbus_sleep();
+        r = modbus_write_register(mb, addr, val);
+        if (r == 1) {
+            return 0;
+        }
+        fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno));
+        *haldata->modbus_errors = *haldata->modbus_errors + 1;
+    }
+    return -1;
+}
+
+
+int set_motor_frequency(modbus_t *mb, float freq) {
+    int r;
+    uint16_t addr = 0x1000;
+    uint16_t val;  // 100 * the frequency percentage
+    val = (freq / max_freq) * 10000;
+
+    for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) {
+        svd_modbus_sleep();
+        r = modbus_write_register(mb, addr, val);
+        if (r == 1) {
+            return 0;
+        }
+        fprintf(stderr, "%s: error writing %u to register 0x%04x: %s\n", __func__, val, addr, modbus_strerror(errno));
+        *haldata->modbus_errors = *haldata->modbus_errors + 1;
+    }
+    return -1;
+}
+
+
+int read_modbus_register(modbus_t *mb, modbus_register_t *reg) {
+    uint16_t data;
+    int r;
+
+    for (int retries = 0; retries <= NUM_MODBUS_RETRIES; retries ++) {
+        svd_modbus_sleep();
+        r = modbus_read_registers(mb, reg->address, 1, &data);
+        if (r == 1) {
+            *reg->hal_pin = data * reg->multiplier;
+            return 0;
+        }
+        fprintf(stderr, "%s: error reading %s (register 0x%04x): %s\n", __func__, reg->name, reg->address, modbus_strerror(errno));
+        *haldata->modbus_errors = *haldata->modbus_errors + 1;
+    }
+    return -1;
+}
+
+
+void read_modbus_registers(modbus_t *mb) {
+    for (int i = 0; i < num_modbus_registers; i ++) {
+        (void)read_modbus_register(mb, &modbus_register[i]);
+    }
+}
+
+
+modbus_register_t *add_modbus_register(modbus_t *mb, int address, const char *pin_name, float multiplier) {
+    int r;
+    modbus_register_t *reg;
+
+    reg = &modbus_register[num_modbus_registers];
+
+    r = hal_pin_float_newf(HAL_OUT, &reg->hal_pin, hal_comp_id, "%s.%s", modname, pin_name);
+    if (r != 0) {
+        return NULL;
+    }
+
+    reg->address = address;
+    reg->name = pin_name;
+    reg->multiplier = multiplier;
+
+    read_modbus_register(mb, reg);
+
+    num_modbus_registers ++;
+
+    return reg;
+}
+
+
+int main(int argc, char **argv) {
+    char *device;
+    int bits;
+    char parity;
+    int stopbits;
+    int verbose;
+
+    int retval = 0;
+    modbus_t *mb;
+    int slave;
+    struct timespec period_timespec;
+    char *endarg;
+    int opt;
+    int argindex, argvalue;
+
+    modbus_register_t *speed_fb_reg;
+
+    done = 0;
+
+    // assume that nothing is specified on the command line
+    device = "/dev/ttyS0";
+    baud = 9600;
+    bits = 8;
+    parity = 'N';
+    stopbits = 2;
+
+    verbose = 0;
+
+    /* slave / register info */
+    slave = 1;
+
+    // process command line options
+    while ((opt = getopt_long(argc, argv, option_string, long_options, NULL)) != -1) {
+        switch (opt) {
+            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("ERROR: device node name is too long: %s\n", optarg);
+                    retval = -1;
+                    goto out_noclose;
+                }
+                device = strdup(optarg);
+                break;
+
+            case 'r':   // Baud rate, 9600 default
+                argindex=match_string(optarg, ratestrings);
+                if (argindex<0) {
+                    printf("ERROR: invalid baud rate: %s\n", optarg);
+                    retval = -1;
+                    goto out_noclose;
+                }
+                baud = atoi(ratestrings[argindex]);
+                break;
+
+            case 'b':   // serial data bits, probably should be 8 (and defaults to 8)
+                argindex=match_string(optarg, bitstrings);
+                if (argindex<0) {
+                    printf("ERROR: invalid number of bits: %s\n", optarg);
+                    retval = -1;
+                    goto out_noclose;
+                }
+                bits = atoi(bitstrings[argindex]);
+                break;
+
+            case 'p':   // parity, should be a string like "even", "odd", or "none"
+                argindex=match_string(optarg, paritystrings);
+                if (argindex<0) {
+                    printf("ERROR: invalid parity: %s\n", optarg);
+                    retval = -1;
+                    goto out_noclose;
+                }
+                parity = paritychars[argindex];
+                break;
+
+            case 's':   // stop bits, defaults to 2
+                argindex=match_string(optarg, stopstrings);
+                if (argindex<0) {
+                    printf("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("ERROR: invalid slave number: %s\n", optarg);
+                    retval = -1;
+                    goto out_noclose;
+                }
+                slave = argvalue;
+                break;
+
+            case 'v':
+                verbose = 1;
+                break;
+
+            case 'h':
+                usage(argc, argv);
+                exit(0);
+                break;
+
+            case 'S':
+                motor_max_speed = strtof(optarg, &endarg);
+                if ((*endarg != '\0') || (motor_max_speed == 0.0)) {
+                    printf("%s: ERROR: invalid motor max speed: %s\n", modname, optarg);
+                    exit(1);
+                }
+                break;
+
+            case 'F':
+                max_freq = strtof(optarg, &endarg);
+                if ((*endarg != '\0') || (max_freq == 0.0)) {
+                    printf("%s: ERROR: invalid max freq: %s\n", modname, optarg);
+                    exit(1);
+                }
+                break;
+
+            case 'f':
+                min_freq = strtof(optarg, &endarg);
+                if ((*endarg != '\0') || (min_freq == 0.0)) {
+                    printf("%s: ERROR: invalid min freq: %s\n", modname, optarg);
+                    exit(1);
+                }
+                break;
+
+            default:
+                usage(argc, argv);
+                exit(1);
+                break;
+        }
+    }
+
+    if (motor_max_speed == 0.0) {
+        printf("%s: must specify --motor-max-speed\n", modname);
+        exit(1);
+    }
+
+    if (max_freq == 0.0) {
+        printf("%s: must specify --max-frequency\n", modname);
+        exit(1);
+    }
+
+    if (min_freq == 0.0) {
+        printf("%s: must specify --min-frequency\n", modname);
+        exit(1);
+    }
+
+    if (min_freq > max_freq) {
+        printf("%s: min frequency (%f) must be less than max frequency (%f)\n", modname, min_freq, max_freq);
+        exit(1);
+    }
+
+    {
+        struct sigaction sa;
+        int r;
+
+        sa.sa_handler = quit;
+        sigemptyset(&sa.sa_mask);
+
+        // libmodbus sometimes segfaults if its system calls get
+        // interrupted by signals.  This works around it by automatically
+        // restarting the system call instead of having it return EINTR.
+        sa.sa_flags = SA_RESTART;
+
+        r = sigaction(SIGINT, &sa, NULL);
+        if (r != 0) {
+            printf("failed to set SIGINT handler: %s\n", strerror(errno));
+            exit(1);
+        }
+        r = sigaction(SIGTERM, &sa, NULL);
+        if (r != 0) {
+            printf("failed to set SIGTERM handler: %s\n", strerror(errno));
+            exit(1);
+        }
+    }
+
+    printf("%s: device='%s', baud=%d, parity='%c', bits=%d, stopbits=%d, address=%d\n",
+           modname, device, baud, parity, bits, stopbits, slave);
+
+    mb = modbus_new_rtu(device, baud, parity, bits, stopbits);
+    if (mb == NULL) {
+        printf("%s: ERROR: couldn't open modbus serial device: %s\n", modname, modbus_strerror(errno));
+        goto out_noclose;
+    }
+
+    {
+        struct timeval t;
+
+        // Set the response timeout.
+        t.tv_sec = 0;
+        t.tv_usec = 30 * 1000;
+#if (LIBMODBUS_VERSION_CHECK(3, 1, 2))
+        modbus_set_response_timeout(mb, t.tv_sec, t.tv_usec);
+#else
+        modbus_set_response_timeout(mb, &t);
+#endif
+
+        // Disable the byte timeout so it just waits for the complete
+        // response timeout instead.
+#if (LIBMODBUS_VERSION_CHECK(3, 1, 2))
+        t.tv_sec = 0;
+        t.tv_usec = 0;
+        modbus_set_byte_timeout(mb, t.tv_sec, t.tv_usec);
+#else
+        t.tv_sec = -1;
+        modbus_set_byte_timeout(mb, &t);
+#endif
+    }
+
+    retval = modbus_connect(mb);
+    if (retval != 0) {
+        printf("%s: ERROR: couldn't open serial device: %s\n", modname, modbus_strerror(errno));
+        goto out_noclose;
+    }
+
+    modbus_set_debug(mb, verbose);
+
+    modbus_set_slave(mb, slave);
+
+    /* create HAL component */
+    hal_comp_id = hal_init(modname);
+    if (hal_comp_id < 0) {
+        printf("%s: ERROR: hal_init failed\n", modname);
+        retval = hal_comp_id;
+        goto out_close;
+    }
+
+    haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
+    if (haldata == NULL) {
+        printf("%s: ERROR: unable to allocate shared memory\n", modname);
+        retval = -1;
+        goto out_closeHAL;
+    }
+
+    retval = hal_pin_float_newf(HAL_IN, &(haldata->period), hal_comp_id, "%s.period-seconds", modname);
+    if (retval != 0) goto out_closeHAL;
+
+    retval = hal_pin_float_newf(HAL_IN, &(haldata->speed_cmd), hal_comp_id, "%s.speed-cmd", modname);
+    if (retval != 0) goto out_closeHAL;
+
+    retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_cmd), hal_comp_id, "%s.freq-cmd", 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_IN, &(haldata->spindle_on), hal_comp_id, "%s.spindle-on", modname);
+    if (retval != 0) goto out_closeHAL;
+
+    retval = hal_pin_u32_newf(HAL_OUT, &(haldata->modbus_errors), hal_comp_id, "%s.modbus-errors", modname);
+    if (retval != 0) goto out_closeHAL;
+
+    *haldata->period = 0.1;
+
+    *haldata->freq_cmd = 0.0;
+    *haldata->at_speed = 0;
+
+    *haldata->modbus_errors = 0;
+
+    modbus_register = (modbus_register_t *)hal_malloc(20 * sizeof(modbus_register_t));
+    if (modbus_register == NULL) {
+        printf("%s: ERROR: unable to allocate memory\n", modname);
+        retval = -1;
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1001, "freq-fb", 0.01) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1002, "dc-bus-voltage", 0.1) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1003, "output-voltage", 1.0) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1004, "output-current", 1.0) == NULL) {
+        goto out_closeHAL;
+    }
+    speed_fb_reg = add_modbus_register(mb, 0x1005, "speed-fb", 1.0);
+    if (speed_fb_reg == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1006, "output-power", 1.0) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x1009, "input-terminal", 1) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x100a, "AI1", 0.001) == NULL) {
+        goto out_closeHAL;
+    }
+    if (add_modbus_register(mb, 0x100b, "AI2", 0.001) == NULL) {
+        goto out_closeHAL;
+    }
+
+    // Activate HAL component
+    hal_ready(hal_comp_id);
+
+    while (done == 0) {
+        if (*haldata->period < 0.001) *haldata->period = 0.001;
+        if (*haldata->period > 2.0) *haldata->period = 2.0;
+        period_timespec.tv_sec = (time_t)(*haldata->period);
+        period_timespec.tv_nsec = (long)((*haldata->period - period_timespec.tv_sec) * 1000000000l);
+        nanosleep(&period_timespec, NULL);
+
+        read_modbus_registers(mb);
+
+        if (*haldata->spindle_on) {
+	    set_motor_on_forward(mb);
+            *haldata->freq_cmd = (*haldata->speed_cmd / motor_max_speed) * max_freq;
+            set_motor_frequency(mb, *haldata->freq_cmd);
+
+            if ((fabs(*haldata->speed_cmd - *speed_fb_reg->hal_pin) / *haldata->speed_cmd) < 0.02) {
+                *haldata->at_speed = 1;
+            } else {
+                *haldata->at_speed = 0;
+            }
+        } else {
+            set_motor_off(mb);
+            *haldata->at_speed = 0;
+            *haldata->freq_cmd = 0.0;
+        }
+
+    }
+
+    usleep(10 * 1000);
+    set_motor_off(mb);
+
+    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);
+    modbus_free(mb);
+out_noclose:
+    return retval;
+}