Notes on the ModbusRTU subsystem on my CNC lathe.
The basic elements are:
Here is the source for sj200mbbasic.comp
component sj200mbbasic "Produces blocks of bytes for Modbus transactions with a Modbus port component";
option userspace;
pin in u32 mbslaveaddr "Modbus slave address of the SJ200";
pin in bit enable "Enable";
pin in bit runstop "Run/Stop command";
pin in bit fwdrev "Forward/Reverse command";
pin in u32 frequency "Frequency command";
pin out bit statusrunstop "Status of current Run/Stop condition";
pin out bit statusfwdrev "Status of current Forward/Reverse condition";
pin out bit statusready "Status of current Ready condition";
pin out u32 outfrequency "Status of current frequncy";
pin out u32 outcurrent "Status of output current, in tenths of amps";
pin out u32 statusmotion "Status of motion presented as a number, 0=Stopped, 3=Running&Forward, 1=Running&Reverse ";
pin out bit qrdy "Query is ready";
pin out u32 qblock0 "First block of four bytes, starts with Modbus slave ID, then function type, register or coil address (high)";
pin out u32 qblock1 "Second block of four bytes, starts with Modbus register or coil address (low), then data bytes";
pin out u32 qblock2 "Third block of four bytes, all data as needed";
pin in bit rrdy "Response is ready";
pin in u32 rblock0 "First block of four bytes of returned data";
pin in u32 rblock1 "Second block of four bytes of returned data";
//function _ nofp;
description """
This component is meant to be used with a Modbus port component.
""";
license "GPL";
;;
#include <unistd.h>
void user_mainloop(void) {
char transstatus = 0;
while(1) {
FOR_ALL_INSTS() {
if ( enable == TRUE ) {
switch (transstatus) {
case 0: // Load the current Read Coils query and set qrdy
/* Read three coils (func 1) starting at 0x0E-1 */
qblock0 = (mbslaveaddr) | (0x01 << 8) | (0x00 << 16) | (0x0D << 24); /* slaveaddr, function, coil addr high, coil addr low */
qblock1 = (0) | (3 << 8);
usleep(2000);
qrdy = TRUE;
usleep(5000);
transstatus = 1;
break;
case 1: // Check for rrdy, if TRUE record response
while ( 1 ) { // wait for response ready
if ( rrdy == TRUE ) {
qrdy = FALSE;
statusrunstop = rblock0 & 1;
statusfwdrev = (rblock0 >> 16) & 1;
statusready = rblock1 & 1;
break;
}
usleep(2000);
}
transstatus = 2;
break;
case 2: // Load current Read Registers query and set qrdy
/* Read three registers (func 3) starting at 0x1002-1 and set HAL pins */
qblock0 = (mbslaveaddr) | (0x03 << 8) | (0x10 << 16) | (0x01 << 24);
qblock1 = (0) | (3 << 8);
usleep(2000);
qrdy = TRUE;
usleep(5000);
transstatus = 3;
break;
case 3: // Check for rrdy, if TRUE record response
while ( 1 ) { // wait for response ready
if ( rrdy == TRUE ) {
qrdy = FALSE;
outfrequency = rblock0 & 0xFFFF; // 0 to 4000 for 0 to 400.0 Hz by .1 Hz
outcurrent = (rblock0 >> 16) & 0xFFFF; // 0 to 2000 for 0 to 200.0% of rated current by .1% (100% = 8.0 Amps)
statusmotion = rblock1 & 0xFFFF; // 0 = Stop, 1 = Forward, 2 = Reverse
break;
}
usleep(2000);
}
transstatus = 4;
break;
case 4: // Load current Write Coils query and set qrdy
/* Write two coils (func 0xF) starting at 0x0001-1 */
qblock0 = (mbslaveaddr) | (0x0F << 8) | (0x00 << 16) | (0x00 << 24); /* slaveaddr, function, coil addr high, coil addr low */
qblock1 = (0) | (2 << 8) | (2 << 16) | (0 << 24); /* number of coils high, number of coils low, number of bytes, data0 high */
qblock2 = (runstop) | (0 << 8) | (fwdrev << 16); /* data0 low, data1 high, data1 low */
usleep(2000);
qrdy = TRUE;
usleep(5000);
transstatus = 5;
break;
case 5: // Check for rrdy
while ( 1 ) { // wait for response ready
if ( rrdy == TRUE ) {
qrdy = FALSE;
// no application data comes back on Writes
break;
}
usleep(2000);
}
transstatus = 6;
break;
case 6: // Load current Write Registers query and set qrdy
/* Write one registers (func 0x10) at 0x0002-1 */
qblock0 = (mbslaveaddr) | (0x10 << 8) | (0x00 << 16) | (0x01 << 24); /* slaveaddr, function, register addr high, register addr low */
qblock1 = 0 | (1 << 8) | (2 << 16) | ((frequency << 16) & 0xFF000000); /* number of registers high, number of registers low, number of bytes (=2xreg), data high */
qblock2 = frequency & 0XFF; /* data low */
usleep(2000);
qrdy = TRUE;
usleep(5000);
transstatus = 7;
break;
case 7: // Check for rrdy
while ( 1 ) { // wait for response ready
if ( rrdy == TRUE ) {
qrdy = FALSE;
// no application data comes back on Writes
break;
}
usleep(2000);
}
transstatus = 0;
break;
}
}
}
}
}
sj200_mbbasic.comp can be compiled and installed with "comp --install sj200_mbbasic.comp" at a terminal command line prompt. The man page can be installed with "comp --install-doc sj200_mbbasic.comp". The HAL pins, Modbus coils and register addresses needed were found in the SJ200 manual. Other devices or more features can be added by using the above as a template.
Below is mbrtuport.c, which can be compiled with a Makefile, also below, with "make" and "sudo make install". One of these needs to be loaded for each serial port used, but normally only one port would be used because many devices can run on the same port.
/*
mbrtuport.c
Based on a work (test-modbus program, part of libmodbus) which is
Copyright (C) 2001-2005 Stéphane Raimbault <stephane.raimbault@free.fr>
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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
This is a userspace HAL program, which may be loaded using the halcmd "loadusr" command:
loadusr mbrtuport
There are several command-line options. Options that have a set list of possible values may
be set by using any number of characters that are unique. For example, --rate 9 will use
a baud rate of 9600, since no other available baud rates start with "9"
-b or --bits <n> (default 8)
Set number of data bits to <n>, where n must be from 7 or 8
-d or --device <path> (default /dev/ttyS0)
Set the name of the serial device node to use
-g or --debug
Turn on debugging messages. This will also set the verbose flag. Debug mode will cause
all modbus messages to be printed in hex on the terminal.
-n or --name <string> (default mbrtuport)
Set the name of the HAL module. The HAL comp name will be set to <string>, and all pin
and parameter names will begin with <string>.
-p or --parity {even,odd,none} (defalt none)
Set serial parity to even, odd, or none.
-r or --rate <n> (default 4800)
Set baud rate to <n>. It is an error if the rate is not one of the following:
4800, 9600, 19200
-s or --stopbits {1,2} (default 1)
Set serial stop bits to 1 or 2
-v or --verbose
Turn on debug messages. Note that if there are serial errors, this may become annoying.
At the moment, it doesn't make much difference most of the time.
-h or --help
Displays this help message.
An example:
loadusr mbrtuport --bits 8 --device /dev/ttyS0 --parity none --rate 4800 --stopbits 1
*/
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include <errno.h>
#include <getopt.h>
#include "rtapi.h"
#include "hal.h"
#include "modbus.h"
/* Unpacking blocks */
/* al ah fn sl */
/* -- -- nl nh */
#define SLAVE_ADDR (*haldata->pqblock0 & 0xFF)
#define FUNCTION ((*haldata->pqblock0 >> 8) & 0xFF)
#define COILS_ADDR (((*haldata->pqblock0 >> 8) & 0xFF00) | ((*haldata->pqblock0 >> 24) & 0xFF))
#define NUM_COILS (((*haldata->pqblock1 << 8) & 0xFF00) | ((*haldata->pqblock1 >> 8) & 0xFF))
#define REGS_ADDR (((*haldata->pqblock0 >> 8) & 0xFF00) | ((*haldata->pqblock0 >> 24) & 0xFF))
#define NUM_REGS (((*haldata->pqblock1 << 8) & 0xFF00) | ((*haldata->pqblock1 >> 8) & 0xFF))
/* HAL data struct */
typedef struct {
hal_u32_t *pqblock0;
hal_u32_t *pqblock1;
hal_u32_t *pqblock2;
hal_u32_t *prblock0;
hal_u32_t *prblock1;
hal_u32_t *prblock2;
hal_bit_t *prdy;
hal_bit_t *qrdy;
hal_bit_t *rrdy;
// hal_s32_t errorcount;
// hal_float_t looptime;
hal_s32_t retval;
} haldata_t;
static int done;
char *modname = "mbrtuport";
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'},
{"verbose", 0, 0, 'v'},
{0,0,0,0}
};
static char *option_string = "b:d:hn:p:r:s:t:v";
static char *bitstrings[] = {"7", "8", NULL};
//static char *paritystrings[] = {"even", "odd", "none", NULL};
static char *paritystrings[] = {"E", "O", "N", NULL};
static char *ratestrings[] = {"4800", "9600", "19200", "57600", NULL};
static char *stopstrings[] = {"1", "2", NULL};
char qrdyold;
/*************/
/* Functions */
/*************/
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 mbrtuport\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 9 will use\n"
" a baud rate of 9600, since no other available baud rates start with \"9\"\n"
"-b or --bits <n> (default 8)\n"
" Set number of data bits to <n>, where n must be from 7 or 8\n"
"-d or --device <path> (default /dev/ttyS0)\n"
" Set the name of the serial device node to use\n"
"-g or --debug\n"
" Turn on debugging messages. This will also set the verbose flag. Debug mode will cause\n"
" all modbus messages to be printed in hex on the terminal.\n"
"-h or --help\n"
" Displays this help message.\n"
"-n or --name <string> (default mbrtuport)\n"
" Set the name of the HAL module. The HAL comp name will be set to <string>, and all pin\n"
" and parameter names will begin with <string>.\n"
"-p or --parity {even,odd,none} (defalt none)\n"
" Set serial parity to even, odd, or none.\n"
"-r or --rate <n> (default 4800)\n"
" Set baud rate to <n>. It is an error if the rate is not one of the following:\n"
" 4800, 9600, 19200\n"
"-s or --stopbits {1,2} (default 1)\n"
" Set serial stop bits to 1 or 2\n"
"-v or --verbose\n"
" Turn on debug messages. Note that if there are serial errors, this may become annoying.\n"
" At the moment, it doesn't make much difference most of the time.\n");
}
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;
}
static void quit(int sig) {
done = 1;
}
/*************/
/* Main */
/*************/
int main(int argc, char **argv)
{
int retval;
int rc;
modbus_t *ctx;
uint8_t rcoilsdata[10];
uint16_t rregsdata[10];
uint8_t wcoilsdata[10];
uint16_t wregsdata[10];
haldata_t *haldata;
int hal_comp_id;
int baud, bits, stopbits, debug, verbose;
char *device, *parity;
int opt;
int argindex;
done = 0;
// assume that nothing is specified on the command line, set to ModIO normal settings
baud = 9600;
bits = 8;
stopbits = 1;
debug = 0;
verbose = 0;
device = "/dev/ttyS0";
parity = "N";
// process command line options
while ((opt=getopt_long(argc, argv, option_string, long_options, NULL)) != -1) {
switch(opt) {
case 'b': // serial data bits, probably should be 8 (and defaults to 8)
argindex=match_string(optarg, bitstrings);
if (argindex<0) {
printf("mbrtuport: 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("mbrtuport: ERROR: device node name is too long: %s\n", optarg);
retval = -1;
goto out_noclose;
}
device = strdup(optarg);
break;
case 'g':
debug = 1;
verbose = 1;
break;
case 'n': // module base name
if (strlen(optarg) > HAL_NAME_LEN-20) {
printf("mbrtuport: 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("mbrtuport: ERROR: invalid parity: %s\n", optarg);
retval = -1;
goto out_noclose;
}
parity = paritystrings[argindex];
break;
case 'r': // Baud rate, 38400 default
argindex=match_string(optarg, ratestrings);
if (argindex<0) {
printf("mbrtuport: 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("mbrtuport: ERROR: invalid number of stop bits: %s\n", optarg);
retval = -1;
goto out_noclose;
}
stopbits = atoi(stopstrings[argindex]);
break;
case 'v': // verbose mode (print modbus errors and other information), default 0
verbose = 1;
break;
case 'h':
default:
usage(argc, argv);
exit(0);
break;
}
}
printf("%s: device='%s', baud=%d, bits=%d, parity='%s', stopbits=%d, verbose=%d\n", modname, device, baud, bits, parity, stopbits, debug);
/* point TERM and INT signals at our quit function */
/* if a signal is received between here and the main loop,
it should prevent some initialization from happening */
signal(SIGINT, quit);
signal(SIGTERM, quit);
/* Initialize Modbus */
ctx = modbus_new_rtu(device, baud, *parity, bits, stopbits);
if (ctx == NULL) {
fprintf(stderr, "Unable to create the libmodbus context\n");
return -1;
}
/* 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;
}
// Make HAL pins
retval = hal_pin_u32_newf(HAL_IN, &(haldata->pqblock0), hal_comp_id, "%s.pqblock0", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_u32_newf(HAL_IN, &(haldata->pqblock1), hal_comp_id, "%s.pqblock1", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_u32_newf(HAL_IN, &(haldata->pqblock2), hal_comp_id, "%s.pqblock2", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_u32_newf(HAL_OUT, &(haldata->prblock0), hal_comp_id, "%s.prblock0", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_u32_newf(HAL_OUT, &(haldata->prblock1), hal_comp_id, "%s.prblock1", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_u32_newf(HAL_OUT, &(haldata->prblock2), hal_comp_id, "%s.prblock2", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->qrdy), hal_comp_id, "%s.qrdy", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->rrdy), hal_comp_id, "%s.rrdy", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->prdy), hal_comp_id, "%s.prdy", modname);
if (retval!=0) goto out_closeHAL;
// haldata->errorcount = 0;
// haldata->looptime = 0.2;
hal_ready(hal_comp_id);
modbus_set_slave(ctx, SLAVE_ADDR);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
modbus_free(ctx);
goto out_closeHAL;
}
*haldata->prdy = TRUE;
*haldata->rrdy = FALSE;
/* here's the meat of the program. loop until done (which may be never) */
while (done==0) {
usleep(2000); // LinuxCNC will crash on exit if not here, okay with 200, but needs to be higher to stretch out rrdy TRUE
*haldata->prdy = TRUE;
if (*haldata->qrdy == TRUE) {
*haldata->rrdy = FALSE;
modbus_set_slave(ctx, SLAVE_ADDR);
switch(FUNCTION) {
case 0x01: // Read Coils
rc = modbus_read_bits(ctx, COILS_ADDR, NUM_COILS, rcoilsdata);
if (rc == NUM_COILS) {
*haldata->prblock0 = rcoilsdata[0] | (rcoilsdata[1] << 16);
*haldata->prblock1 = rcoilsdata[2] | (rcoilsdata[3] << 16);
*haldata->prblock2 = rcoilsdata[4] | (rcoilsdata[5] << 16);
usleep(1000);
*haldata->rrdy = TRUE;
} else {
printf("FAILED to read coils\n");
}
break;
case 0x03: // Read Registers
rc = modbus_read_registers(ctx, REGS_ADDR, NUM_REGS, rregsdata);
if (rc == NUM_REGS) {
*haldata->prblock0 = rregsdata[0] | (rregsdata[1] << 16);
*haldata->prblock1 = rregsdata[2] | (rregsdata[3] << 16);
*haldata->prblock2 = rregsdata[4] | (rregsdata[5] << 16);
usleep(1000);
*haldata->rrdy = TRUE;
// printf("Read registers\n");
} else {
printf("FAILED to read registers\n");
}
break;
case 0x0F: // Write Coils
wcoilsdata[0] = ((*haldata->pqblock1 >> 16) & 0xFF00) | (*haldata->pqblock2 & 0xFF);
wcoilsdata[1] = ((*haldata->pqblock2 >> 8) & 0xFF00) | ((*haldata->pqblock2 >> 16) & 0xFF);
rc = modbus_write_bits(ctx, COILS_ADDR, NUM_COILS, wcoilsdata);
if (rc == NUM_COILS) {
usleep(1000);
*haldata->rrdy = TRUE;
} else {
printf("FAILED to read coils\n");
}
break;
case 0x10: //write_registers();
wregsdata[0] = ((*haldata->pqblock1 >> 16) & 0xFF00) | (*haldata->pqblock2 & 0xFF);
wregsdata[1] = ((*haldata->pqblock2 >> 8) & 0xFF00) | ((*haldata->pqblock2 >> 16) & 0xFF);
rc = modbus_write_registers(ctx, REGS_ADDR, NUM_REGS, wregsdata);
if (rc == NUM_REGS) {
usleep(1000);
*haldata->rrdy = TRUE;
// printf("Wrote registers\n");
} else {
printf("FAILED to write registers\n");
}
break;
default:
break;
}
}
}
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(ctx);
modbus_free(ctx);
out_noclose:
return retval;
}
The Makefile for installing mbrtuport.c
default: mbrtuport
MODINC = /usr/share/emc/Makefile.modinc
BINDIR = /usr/bin
MODBUS_INCLUDES = /usr/local/include/modbus
MODBUS_LIBS = /usr/local/lib
ifeq "$(MODINC)" ""
$(error Required files for building components not present. Install emc2-dev)
endif
include $(MODINC)
ifeq ($(RUN_IN_PLACE),no)
EXTRA_CFLAGS += -I$(EMC2_HOME)/include/emc2 -Wall
LIBDIR := $(shell ./find-libdir)
ifeq "$(LIBDIR)" ""
$(error LIBDIR not found)
endif
endif
CFLAGS := $(EXTRA_CFLAGS) -URTAPI -U__MODULE__ -DULAPI -Os -I$(MODBUS_INCLUDES)
CFLAGS += $(shell pkg-config --cflags glib-2.0)
LFLAGS := -Wl,-rpath,$(LIBDIR) -L$(LIBDIR) -lemchal -L$(MODBUS_LIBS) -lmodbus
LFLAGS += $(shell pkg-config --libs glib-2.0)
include .o/mbrtuport.d #.o/modbus.d
install: mbrtuport
cp mbrtuport $(BINDIR)
mbrtuport: .o/mbrtuport.o #.o/modbus.o
$(CC) -o $@ $^ $(LFLAGS)
.o/%.o: %.c
mkdir -p .o
$(CC) $(CFLAGS) -o $@ -c $<
.o/%.d: %.c
mkdir -p .o
$(CC) $(CFLAGS) -MM -MT "$@ $(patsubst %.d,%.o,$@)" $< -o $@.tmp \
&& mv $@.tmp $@
clean:
-rm -f mbrtuport
-rm -rf .o
-- The End? --