diff --git a/site_ansto/hardsup/sct_modbusprot.c b/site_ansto/hardsup/sct_modbusprot.c index bb0a7376..88197c92 100644 --- a/site_ansto/hardsup/sct_modbusprot.c +++ b/site_ansto/hardsup/sct_modbusprot.c @@ -6,11 +6,71 @@ #include #include #include +#include static int dbgprintf(char* fmtstr, ...); static int dbgprintx(const char* msg, const unsigned char* cp, int blen); static unsigned int debug_modbus = 1; +/*-------------------------------------------------------------------------*/ +/* IEEE code copied from psi/modbus.c */ +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); +} +/*-------------------------------------------------------------------------*/ + /** @brief encode modbus request * TODO: clean me up */ @@ -71,19 +131,22 @@ int ModbusWriteStart(Ascon *a) { if (cmd == 16) { /* write registers */ buff = endp + 1; if (do_float) { - union { unsigned char v[4]; float val; } u; - u.val = strtof(buff, &endp); + struct { char v[4]; } ieee; /* big endian */ + double val = strtod(buff, &endp); if (endp == buff) { - dbgprintf("modbus-er: Bad value: %f from %s\n", u.val, buff); + dbgprintf("modbus-er: Bad value: %f from %s\n", val, buff); a->state = AsconIdle; AsconError(a, "Bad value", 0); return 0; } + double2ieee(val, ieee.v); + dbgprintf("strtod:%f\n", val); + dbgprintx("double2ieee", (unsigned char*)ieee.v, 4); temp[idx++] = 4; /* byte count */ - temp[idx++] = u.v[1]; - temp[idx++] = u.v[0]; - temp[idx++] = u.v[3]; - temp[idx++] = u.v[2]; + temp[idx++] = ieee.v[2]; /* watlow RUI specific byte ordering */ + temp[idx++] = ieee.v[3]; + temp[idx++] = ieee.v[0]; + temp[idx++] = ieee.v[1]; } else { unsigned int val; val = strtoul(buff, &endp, 10); @@ -137,12 +200,16 @@ int ModbusReading(Ascon *a) { rlen = snprintf(temp, 64, "%d", (((unsigned int)cp[9]) << 8) + (unsigned int)cp[10]); } else if (cp[7] == 3 && cp[8] == 4) { - union { unsigned char v[4]; float val; } u; - u.v[1] = cp[9]; - u.v[0] = cp[10]; - u.v[3] = cp[11]; - u.v[2] = cp[12]; - rlen = snprintf(temp, 64, "%g", u.val); + struct { char v[4]; } ieee; + double val; + ieee.v[2] = cp[9]; /* watlow RUI specific byte ordering */ + ieee.v[3] = cp[10]; + ieee.v[0] = cp[11]; + ieee.v[1] = cp[12]; + val = ieee2double(ieee.v); + dbgprintx("rawhex", (unsigned char*)&cp[9], 4); + dbgprintf("ieee2double:%f\n", val); + rlen = snprintf(temp, 64, "%g", val); } else if (cp[7] == 16 && cp[11] == 1) { rlen = snprintf(temp, 64, "OK int");