397 lines
11 KiB
C
397 lines
11 KiB
C
/*---------------------------------------------------------------------------
|
|
modbus.c
|
|
|
|
Communication routines for Eurotherm ModBus instruments
|
|
|
|
Markus Zolliker, Aug 2005
|
|
----------------------------------------------------------------------------
|
|
|
|
there is no error return value, eab->errCode is used. On success, eab->errCode
|
|
is not changed, i.e. an existing errCode is not overwritten.
|
|
*/
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <fortify.h>
|
|
#include <math.h>
|
|
#include "sics.h"
|
|
#include "modbus.h"
|
|
#include "cnvrt.h"
|
|
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static void uint2word(unsigned int adr, char word[2])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
uword[0] = adr / 256;
|
|
uword[1] = adr % 256;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static unsigned int word2uint(char word[2])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
return uword[0] * 256 + uword[1];
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static long quad2long(char word[4])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
return ((uword[0] * 256 + uword[1]) * 256 + uword[2]) * 256 + uword[3];
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static void long2quad(long val, char word[4])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
uword[3] = val % 256;
|
|
val = val / 256;
|
|
uword[2] = val % 256;
|
|
val = val / 256;
|
|
uword[1] = val % 256;
|
|
val = val / 256;
|
|
uword[0] = val;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static void fadr2word(int adr, char word[2])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
uword[0] = (adr * 2) / 256 + 0x80;
|
|
uword[1] = (adr * 2) % 256;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int word2fadr(char word[2])
|
|
{
|
|
unsigned char *uword;
|
|
uword = (void *) word;
|
|
return ((uword[0] & 0x7F) * 256 + uword[1]) / 2;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int calc_crc(char *inp, int inpLen, int addCrc)
|
|
{
|
|
|
|
/* CRC runs cyclic Redundancy Check Algorithm on input inp */
|
|
/* Returns value of 16 bit CRC after completion and */
|
|
/* adds 2 crc bytes if addCrc is not 0 */
|
|
/* returns 0 if incoming message has correct CRC */
|
|
|
|
unsigned int crc = 0xffff;
|
|
unsigned int next;
|
|
int carry;
|
|
int n;
|
|
|
|
while (inpLen--) {
|
|
next = *(unsigned char *) inp;
|
|
crc ^= next;
|
|
for (n = 0; n < 8; n++) {
|
|
carry = crc & 1;
|
|
crc >>= 1;
|
|
if (carry) {
|
|
crc ^= 0xA001;
|
|
}
|
|
}
|
|
inp++;
|
|
}
|
|
if (addCrc) {
|
|
inp[0] = crc % 256;
|
|
inp[1] = crc / 256;
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusDump(EaseBase * eab, int iout, char *fmt, char *buffer,
|
|
int length)
|
|
{
|
|
char buf[128];
|
|
int i;
|
|
|
|
if (length > 40)
|
|
length = 40;
|
|
for (i = 0; i < length; i++) {
|
|
snprintf(buf + i * 3, 4, " %2.2X", (unsigned char) buffer[i]);
|
|
}
|
|
buf[i * 3] = '\0';
|
|
ParPrintf(eab, iout, fmt, buf);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusWrite(EaseBase * eab, int length)
|
|
{
|
|
int iret;
|
|
char trash[64];
|
|
int l;
|
|
|
|
assert(length < sizeof(eab->cmd));
|
|
|
|
if (eab->errCode || eab->state == EASE_expect)
|
|
return;
|
|
while (availableRS232(eab->ser) == 1) {
|
|
l = sizeof(trash);
|
|
iret = readRS232TillTerm(eab->ser, trash, &l);
|
|
if (iret < 0)
|
|
break;
|
|
ModBusDump(eab, -2, "trash %s\n", trash, l);
|
|
}
|
|
calc_crc(eab->cmd, length, 1);
|
|
iret = writeRS232(eab->ser, eab->cmd, length + 2);
|
|
if (iret < 0) {
|
|
eab->errCode = iret;
|
|
return;
|
|
}
|
|
ModBusDump(eab, -2, "cmd: %s", eab->cmd, length + 2);
|
|
eab->state = EASE_expect;
|
|
eab->cmdtime = time(NULL);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
int ModBusHandler(void *object)
|
|
{
|
|
int iret, l;
|
|
EaseBase *eab = EaseBaseCast(object);
|
|
|
|
if (eab->state < EASE_idle)
|
|
goto quit;
|
|
if (availableNetRS232(eab->ser) || availableRS232(eab->ser)) {
|
|
eab->msg[0] = '\0';
|
|
l = 5;
|
|
iret = readRS232TillTerm(eab->ser, eab->ans, &l);
|
|
if (eab->state != EASE_expect && eab->state != EASE_lost) {
|
|
if (iret == 1) {
|
|
ModBusDump(eab, eError, "unexpected answer %s", eab->ans, l);
|
|
}
|
|
goto quit;
|
|
}
|
|
if (iret == 1) {
|
|
if (eab->cmd[0] != eab->ans[0]
|
|
|| eab->cmd[1] != (eab->ans[1] & 0x7F)) {
|
|
iret = EASE_FAULT;
|
|
ModBusDump(eab, eError, "bad answer: %s", eab->ans, l);
|
|
} else if (eab->ans[1] & 0x80) {
|
|
iret = EASE_FAULT;
|
|
if (eab->ans[2] == 2) {
|
|
ParPrintf(eab, eError, "ERROR: illegal address");
|
|
} else if (eab->ans[2] == 3) {
|
|
ParPrintf(eab, eError, "ERROR: illegal data");
|
|
} else {
|
|
ModBusDump(eab, eError, "bad answer: %s", eab->ans, l);
|
|
};
|
|
} else if (eab->ans[1] == 16) { /* answer from write n words */
|
|
l = 3; /* finish response */
|
|
iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
|
|
l += 5;
|
|
} else if (eab->ans[1] == 3) { /* answer for read n words */
|
|
l = eab->ans[2]; /* bytes to read */
|
|
if (l < 0 || l >= sizeof(eab->ans) - 5) {
|
|
l = 64;
|
|
}
|
|
iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
|
|
l += 5;
|
|
} else if (eab->ans[1] == 8) { /* loopback info */
|
|
l = 1;
|
|
iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
|
|
l += 5;
|
|
if (eab->state == EASE_lost) {
|
|
if (eab->ans[4] == 44 && eab->ans[5] == 55) {
|
|
eab->state = EASE_idle;
|
|
}
|
|
goto quit;
|
|
}
|
|
} else {
|
|
iret = EASE_ILL_ANS;
|
|
}
|
|
if (eab->state == EASE_lost) {
|
|
goto quit;
|
|
}
|
|
ModBusDump(eab, -2, "ans: %s", eab->ans, l);
|
|
if (iret == 1) {
|
|
if (calc_crc(eab->ans, l, 0)) {
|
|
iret = MODBUS_BAD_CRC;
|
|
}
|
|
}
|
|
}
|
|
if (iret != 1) {
|
|
eab->errCode = iret;
|
|
eab->state = EASE_idle;
|
|
goto error;
|
|
}
|
|
eab->state = EASE_read;
|
|
} else if (eab->state == EASE_expect) {
|
|
if (time(NULL) > eab->cmdtime + 20) {
|
|
eab->state = EASE_lost;
|
|
}
|
|
} else if (eab->state == EASE_lost) {
|
|
if (time(NULL) > eab->cmdtime) {
|
|
eab->cmd[1] = 8;
|
|
eab->cmd[2] = 0;
|
|
eab->cmd[3] = 0;
|
|
eab->cmd[4] = 44;
|
|
eab->cmd[5] = 55;
|
|
ParPrintf(eab, -2, "loopback");
|
|
ModBusWrite(eab, 6);
|
|
eab->state = EASE_lost;
|
|
}
|
|
}
|
|
goto quit;
|
|
error:
|
|
/* EaseWriteError(eab); */
|
|
quit:
|
|
return EaseHandler(eab);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusPutFloats(EaseBase * eab, int adr, int npar, float val[])
|
|
{
|
|
int l, n;
|
|
|
|
assert(npar <= 5);
|
|
|
|
eab->cmd[0] = 1; /* device address */
|
|
eab->cmd[1] = 16; /* write n words */
|
|
fadr2word(adr, eab->cmd + 2);
|
|
uint2word(npar * 2, eab->cmd + 4);
|
|
eab->cmd[6] = npar * 4; /* number of bytes */
|
|
l = 7;
|
|
for (n = 0; n < npar; n++) {
|
|
double2ieee(val[n], eab->cmd + l);
|
|
l += 4;
|
|
}
|
|
if (npar == 1) {
|
|
ParPrintf(eab, -1, "write #%d %.5g", adr, val[0]);
|
|
} else {
|
|
ParPrintf(eab, -1, "write #%d %.5g %.5g ... (%d)", adr, val[0], val[1], npar);
|
|
}
|
|
ModBusWrite(eab, l);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusRequestValues(EaseBase * eab, int adr, int npar)
|
|
{
|
|
|
|
assert(npar <= 14);
|
|
|
|
eab->cmd[0] = 1; /* device address */
|
|
eab->cmd[1] = 3; /* read n words */
|
|
fadr2word(adr, eab->cmd + 2);
|
|
uint2word(npar * 2, eab->cmd + 4);
|
|
ParPrintf(eab, -2, "read #%d (%d values)", adr, npar);
|
|
ModBusWrite(eab, 6);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static double ModBus2double(char ieee[4])
|
|
{
|
|
long t;
|
|
/* convert 32 bit values by guessing the type.
|
|
will not work when a floating point value is a very low positive number
|
|
(below 2^-125) or when a time is divisible by 65536 ms */
|
|
if (ieee[0]) { /* guess its a float */
|
|
return ieee2double(ieee);
|
|
}
|
|
if (ieee[1] != 0 && ieee[2] == 0 && ieee[3] == 0) {
|
|
/* guess its an int, a float 0 will also be returned correctly */
|
|
return word2uint(ieee);
|
|
}
|
|
/* guess its a time */
|
|
t = word2uint(ieee);
|
|
return (t * 65536 + word2uint(ieee + 2)) * 0.001;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
float ModBusGet(EaseBase * eab, int adr, int type)
|
|
{
|
|
int startAdr;
|
|
int i, n;
|
|
|
|
if (eab->state != EASE_read) {
|
|
return 0.0;
|
|
}
|
|
n = word2uint(eab->cmd + 4);
|
|
if (n <= 2) return ModBusGetValue(eab, type);
|
|
startAdr = word2fadr(eab->cmd + 2);
|
|
i = adr - startAdr;
|
|
if (i < 0 || i >= n) {
|
|
return 0.0;
|
|
}
|
|
if (type == modBusTime) {
|
|
return quad2long(eab->ans + 3 + i * 4) * 0.001;
|
|
}
|
|
return ieee2double(eab->ans + 3 + i * 4);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusRequestValue(EaseBase * eab, int adr, int type)
|
|
{
|
|
|
|
eab->cmd[0] = 1; /* device address */
|
|
eab->cmd[1] = 3; /* read n words */
|
|
if (type == modBusInt) {
|
|
uint2word(adr, eab->cmd + 2);
|
|
uint2word(1, eab->cmd + 4);
|
|
} else {
|
|
fadr2word(adr, eab->cmd + 2);
|
|
uint2word(2, eab->cmd + 4);
|
|
}
|
|
ParPrintf(eab, -2, "read #%d", adr);
|
|
ModBusWrite(eab, 6);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void ModBusPutValue(EaseBase * eab, int adr, int type, float val)
|
|
{
|
|
int l;
|
|
|
|
eab->cmd[0] = 1; /* device address */
|
|
eab->cmd[1] = 16; /* write n words */
|
|
if (type == modBusInt) {
|
|
uint2word(adr, eab->cmd + 2);
|
|
uint2word(1, eab->cmd + 4);
|
|
eab->cmd[6] = 2;
|
|
uint2word((int) (val), eab->cmd + 7);
|
|
ParPrintf(eab, -1, "write #%d %.5g", adr, val);
|
|
ModBusWrite(eab, 9);
|
|
return;
|
|
}
|
|
fadr2word(adr, eab->cmd + 2);
|
|
uint2word(2, eab->cmd + 4);
|
|
eab->cmd[6] = 4; /* number of bytes */
|
|
if (type == modBusFloat) {
|
|
double2ieee(val, eab->cmd + 7);
|
|
} else if (type == modBusTime) {
|
|
long2quad((long) (val * 1000. + 0.5), eab->cmd + 7);
|
|
} else {
|
|
uint2word((int) (val + 0.5), eab->cmd + 7);
|
|
eab->cmd[9] = 0;
|
|
eab->cmd[10] = 0;
|
|
}
|
|
ParPrintf(eab, -1, "write #%d %.5g", adr, val);
|
|
ModBusWrite(eab, 11);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
float ModBusGetValue(EaseBase * eab, int type)
|
|
{
|
|
|
|
if (eab->state != EASE_read) {
|
|
return 0.0;
|
|
}
|
|
if (type == modBusFloat) {
|
|
return ieee2double(eab->ans + 3);
|
|
} else if (type == modBusTime) {
|
|
return quad2long(eab->ans + 3) * 0.001;
|
|
} else {
|
|
return word2uint(eab->ans + 3);
|
|
}
|
|
}
|