diff --git a/mbus/mbus-protocol.c b/mbus/mbus-protocol.c index c0a4c37..3ee1461 100755 --- a/mbus/mbus-protocol.c +++ b/mbus/mbus-protocol.c @@ -12,6 +12,7 @@ #include #include #include +#include #include "mbus-protocol.h" @@ -648,8 +649,52 @@ mbus_data_int_encode(unsigned char *int_data, size_t int_data_size, int value) float mbus_data_float_decode(unsigned char *float_data) { +#ifdef _HAS_NON_IEEE754_FLOAT + float val = 0.0f; + long temp = 0, fraction; + int sign,exponent; + size_t i; + if (float_data) - return *(float *) float_data; + { + for (i = 4; i > 0; i--) + { + temp = (temp << 8) + float_data[i-1]; + } + + // first bit = sign bit + sign = (temp >> 31) ? -1 : 1; + + // decode 8 bit exponent + exponent = ((temp & 0x7F800000) >> 23) - 127; + + // decode explicit 23 bit fraction + fraction = temp & 0x007FFFFF; + + if ((exponent != -127) && + (exponent != 128)) + { + // normalized value, add bit 24 + fraction |= 0x800000; + } + + // calculate float value + val = (float) sign * fraction * pow(2.0f, -23.0f + exponent); + + return val; + } +#else + if (float_data) + { + union { + uint32_t u32; + float f; + } data; + memcpy(&(data.u32), float_data, sizeof(uint32_t)); + data.u32 = ntohl(data.u32); + return data.f; + } +#endif return -1.0f; }