linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Adapted indenation to new agreed upon system

Browse Source 
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
2009-02-13 09:01:03 +00:00
parent 6c7bb14fad
commit eb72d5c486
151 changed files with 38234 additions and 38208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

261
modbus.c
View File

@@ -21,7 +21,8 @@ is not changed, i.e. an existing errCode is not overwritten.
/*-------------------------------------------------------------------------*/
static void double2ieee(double input, char ieee[4]) {
static void double2ieee(double input, char ieee[4])
{
/* convert double to IEEE 32 bit floating number (denormalized numbers are considered as zero) */
@@ -29,10 +30,10 @@ static void double2ieee(double input, char ieee[4]) {
int exponent;
if (input == 0) {
ieee[0]= 0;
ieee[1]= 0;
ieee[2]= 0;
ieee[3]= 0;
ieee[0] = 0;
ieee[1] = 0;
ieee[2] = 0;
ieee[3] = 0;
} else {
mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
exponent = exponent - 1 + 127;
@@ -54,19 +55,20 @@ static void double2ieee(double input, char ieee[4]) {
}
/*-------------------------------------------------------------------------*/
static double ieee2double(char ieee[4]) {
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 */
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;
}
@@ -76,62 +78,79 @@ static double ieee2double(char ieee[4]) {
}
return output * ldexp(1, exponent - 127);
}
/*----------------------------------------------------------------------------*/
static void uint2word(unsigned int adr, char word[2]) {
static void uint2word(unsigned int adr, char word[2])
{
unsigned char *uword;
uword = (void *)word;
uword = (void *) word;
uword[0] = adr / 256;
uword[1] = adr % 256;
}
/*----------------------------------------------------------------------------*/
static unsigned int word2uint(char word[2]) {
static unsigned int word2uint(char word[2])
{
unsigned char *uword;
uword = (void *)word;
return uword[0]*256 + uword[1];
uword = (void *) word;
return uword[0] * 256 + uword[1];
}
/*----------------------------------------------------------------------------*/
static long quad2long(char word[4]) {
static long quad2long(char word[4])
{
unsigned char *uword;
uword = (void *)word;
return ((uword[0]*256 + uword[1])*256 + uword[2])*256 + uword[3];
uword = (void *) word;
return ((uword[0] * 256 + uword[1]) * 256 + uword[2]) * 256 + uword[3];
}
/*----------------------------------------------------------------------------*/
static void long2quad(long val, char word[4]) {
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 = (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]) {
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;
uword = (void *) word;
uword[0] = (adr * 2) / 256 + 0x80;
uword[1] = (adr * 2) % 256;
}
/*----------------------------------------------------------------------------*/
static int word2fadr(char word[2]) {
static int word2fadr(char word[2])
{
unsigned char *uword;
uword = (void *)word;
return ((uword[0] & 0x7F)*256 + uword[1]) / 2;
uword = (void *) word;
return ((uword[0] & 0x7F) * 256 + uword[1]) / 2;
}
/*----------------------------------------------------------------------------*/
static int calc_crc(char *inp, int inpLen, int addCrc) {
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 crc = 0xffff;
unsigned int next;
int carry;
int n;
while (inpLen--) {
next = *(unsigned char *)inp;
next = *(unsigned char *) inp;
crc ^= next;
for (n = 0; n < 8; n++) {
carry = crc & 1;
@@ -147,50 +166,61 @@ static int calc_crc(char *inp, int inpLen, int addCrc) {
inp[1] = crc / 256;
}
return crc;
}
}
/*----------------------------------------------------------------------------*/
void ModBusDump(EaseBase *eab, int iout, char *fmt, char *buffer, int length) {
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]);
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';
buf[i * 3] = '\0';
ParPrintf(eab, iout, fmt, buf);
}
/*----------------------------------------------------------------------------*/
void ModBusWrite(EaseBase *eab, int length) {
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;
assert(length < sizeof(eab->cmd));
if (eab->errCode || eab->state == EASE_expect)
return;
while (availableRS232(eab->ser) == 1) {
l=sizeof(trash);
l = sizeof(trash);
iret = readRS232TillTerm(eab->ser, trash, &l);
if (iret < 0) break;
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);
iret = writeRS232(eab->ser, eab->cmd, length + 2);
if (iret < 0) {
eab->errCode = iret;
return;
}
ModBusDump(eab, -2, "cmd: %s", eab->cmd, length+2);
ModBusDump(eab, -2, "cmd: %s", eab->cmd, length + 2);
eab->state = EASE_expect;
eab->cmdtime = time(NULL);
}
/*----------------------------------------------------------------------------*/
int ModBusHandler(void *object) {
int ModBusHandler(void *object)
{
int iret, l;
EaseBase *eab = EaseBaseCast(object);
if (eab->state < EASE_idle) goto quit;
if (eab->state < EASE_idle)
goto quit;
if (availableNetRS232(eab->ser) || availableRS232(eab->ser)) {
eab->msg[0] = '\0';
l = 5;
@@ -202,7 +232,8 @@ int ModBusHandler(void *object) {
goto quit;
}
if (iret == 1) {
if (eab->cmd[0] != eab->ans[0] || eab->cmd[1] != (eab->ans[1] & 0x7F)) {
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) {
@@ -214,21 +245,21 @@ int ModBusHandler(void *object) {
} 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;
} 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;
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;
@@ -255,7 +286,7 @@ int ModBusHandler(void *object) {
}
eab->state = EASE_read;
} else if (eab->state == EASE_expect) {
if (time(NULL) > eab->cmdtime+20) {
if (time(NULL) > eab->cmdtime + 20) {
eab->state = EASE_lost;
}
} else if (eab->state == EASE_lost) {
@@ -275,42 +306,48 @@ error:
quit:
return EaseHandler(eab);
}
/*----------------------------------------------------------------------------*/
void ModBusPutFloats(EaseBase *eab, int adr, int npar, float val[]) {
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 */
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 */
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);
for (n = 0; n < npar; n++) {
double2ieee(val[n], eab->cmd + l);
l += 4;
}
ModBusWrite(eab, l);
}
/*----------------------------------------------------------------------------*/
void ModBusRequestValues(EaseBase *eab, int adr, int npar) {
void ModBusRequestValues(EaseBase * eab, int adr, int npar)
{
assert(npar <= 14);
eab->cmd[0] = 1; /* device address */
eab->cmd[1] = 3; /* read n words */
eab->cmd[0] = 1; /* device address */
eab->cmd[1] = 3; /* read n words */
fadr2word(adr, eab->cmd + 2);
uint2word(npar*2, eab->cmd + 4);
uint2word(npar * 2, eab->cmd + 4);
ModBusWrite(eab, 6);
}
/*----------------------------------------------------------------------------*/
static double ModBus2double(char ieee[4]) {
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 */
if (ieee[0]) { /* guess its a float */
return ieee2double(ieee);
}
if (ieee[1] != 0 && ieee[2] == 0 && ieee[3] == 0) {
@@ -319,13 +356,15 @@ static double ModBus2double(char ieee[4]) {
}
/* guess its a time */
t = word2uint(ieee);
return (t * 65536 + word2uint(ieee+2)) * 0.001;
return (t * 65536 + word2uint(ieee + 2)) * 0.001;
}
/*----------------------------------------------------------------------------*/
float ModBusGet(EaseBase *eab, int adr, int type) {
float ModBusGet(EaseBase * eab, int adr, int type)
{
int startAdr;
int i;
if (eab->state != EASE_read) {
return 0.0;
}
@@ -339,11 +378,13 @@ float ModBusGet(EaseBase *eab, int adr, int type) {
}
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 */
/*----------------------------------------------------------------------------*/
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);
@@ -353,45 +394,49 @@ void ModBusRequestValue(EaseBase *eab, int adr, int type) {
}
ModBusWrite(eab, 6);
}
/*----------------------------------------------------------------------------*/
void ModBusPutValue(EaseBase *eab, int adr, int type, float val) {
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 */
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);
uint2word((int) (val), eab->cmd + 7);
ModBusWrite(eab, 9);
return;
}
fadr2word(adr, eab->cmd + 2);
uint2word(2, eab->cmd + 4);
eab->cmd[6] = 4; /* number of bytes */
eab->cmd[6] = 4; /* number of bytes */
if (type == modBusFloat) {
double2ieee(val, eab->cmd+7);
double2ieee(val, eab->cmd + 7);
} else if (type == modBusTime) {
long2quad((long)(val * 1000. + 0.5), eab->cmd+7);
long2quad((long) (val * 1000. + 0.5), eab->cmd + 7);
} else {
uint2word((int)(val + 0.5), eab->cmd+7);
uint2word((int) (val + 0.5), eab->cmd + 7);
eab->cmd[9] = 0;
eab->cmd[10] = 0;
}
ModBusWrite(eab, 11);
}
/*----------------------------------------------------------------------------*/
float ModBusGetValue(EaseBase *eab, int type) {
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;
return quad2long(eab->ans + 3) * 0.001;
} else {
return word2uint(eab->ans+3);
}
return word2uint(eab->ans + 3);
}
}