- new drivers
This commit is contained in:
zolliker
297
modbus.c
Normal file
297
modbus.c
Normal file
@@ -0,0 +1,297 @@
|
||||
/*---------------------------------------------------------------------------
|
||||
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"
|
||||
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static void double2ieee(double input, char ieee[4]) {
|
||||
|
||||
/* convert double to IEEE 32 bit floating number (denormalized numbers are considered as zero) */
|
||||
|
||||
long mantissa;
|
||||
int exponent;
|
||||
|
||||
if (input == 0) {
|
||||
ieee[0]= 0;
|
||||
ieee[1]= 0;
|
||||
ieee[2]= 0;
|
||||
ieee[3]= 0;
|
||||
} else {
|
||||
mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
|
||||
exponent = exponent - 1 + 127;
|
||||
if (exponent < 0) {
|
||||
exponent = 0;
|
||||
} else if (exponent > 0xFE) {
|
||||
exponent = 0xFE;
|
||||
}
|
||||
if (input < 0) {
|
||||
ieee[0] = 0x80 | (exponent >> 1);
|
||||
} else {
|
||||
ieee[0] = exponent >> 1;
|
||||
}
|
||||
ieee[1] = (exponent & 1) << 7 | ((mantissa & 0x7F0000) >> 16);
|
||||
ieee[2] = (mantissa & 0xFF00) >> 8;
|
||||
ieee[3] = mantissa & 0xFF;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static double ieee2double(char ieee[4]) {
|
||||
|
||||
/* IEEE 32 bit floating number to double (denormalized numbers are considered as zero) */
|
||||
|
||||
long mantissa;
|
||||
double output;
|
||||
int exponent;
|
||||
|
||||
mantissa = ((ieee[1] << 16) & 0x7FFFFF)
|
||||
| ((ieee[2] << 8) & 0xFF00)
|
||||
| ((ieee[3] ) & 0xFF);
|
||||
|
||||
exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
|
||||
if (exponent == 0 && mantissa == 0) {
|
||||
return 0.0;
|
||||
}
|
||||
output = ldexp(mantissa, -23) + 1.0;
|
||||
if (ieee[0] & 0x80) {
|
||||
output = -output;
|
||||
}
|
||||
return output * ldexp(1, exponent - 127);
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
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 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 */
|
||||
/* always adds 2 crc bytes to message */
|
||||
/* 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[50];
|
||||
int i;
|
||||
|
||||
if (length > 16) length = 16;
|
||||
for (i=0; i<length; i++) {
|
||||
snprintf(buf+i*3, 4, " %2.2X", (unsigned int)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_ILL_ANS;
|
||||
}
|
||||
if (eab->ans[1] & 0x80) {
|
||||
iret = EASE_ILL_ANS;
|
||||
} 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 */
|
||||
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;
|
||||
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;
|
||||
}
|
||||
ModBusWrite(eab, l);
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
void ModBusRequestFloats(EaseBase *eab, int adr, int npar) {
|
||||
int l;
|
||||
|
||||
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);
|
||||
ModBusWrite(eab, 6);
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
float ModBusGetFloat(EaseBase *eab, int adr) {
|
||||
int startAdr;
|
||||
int i;
|
||||
|
||||
if (eab->state != EASE_read) {
|
||||
return 0.0;
|
||||
}
|
||||
startAdr = word2fadr(eab->cmd + 2);
|
||||
i = adr - startAdr;
|
||||
if (i < 0 || i >= word2uint(eab->cmd + 4)) {
|
||||
return 0.0;
|
||||
}
|
||||
return ieee2double(eab->ans + 3 + i * 4);
|
||||
}
|
||||
Reference in New Issue
Block a user