diff --git a/src/modbus-data.c b/src/modbus-data.c index ae53757..089702d 100644 --- a/src/modbus-data.c +++ b/src/modbus-data.c @@ -24,6 +24,7 @@ #endif #include #include +#include #include "modbus.h" @@ -75,7 +76,7 @@ uint8_t modbus_get_byte_from_bits(const uint8_t *src, int index, return value; } -/* Get a float from 4 bytes in Modbus format */ +/* Get a float from 4 bytes in Modbus format (ABCD) */ float modbus_get_float(const uint16_t *src) { float f; @@ -87,7 +88,19 @@ float modbus_get_float(const uint16_t *src) return f; } -/* Set a float to 4 bytes in Modbus format */ +/* Get a float from 4 bytes in swapped Modbus format (DCBA) */ +float modbus_get_float_swapped(const uint16_t *src) +{ + float f; + uint32_t i; + + i = bswap_32((((uint32_t)src[1]) << 16) + src[0]); + memcpy(&f, &i, sizeof(float)); + + return f; +} + +/* Set a float to 4 bytes in Modbus format (ABCD) */ void modbus_set_float(float f, uint16_t *dest) { uint32_t i; @@ -96,3 +109,14 @@ void modbus_set_float(float f, uint16_t *dest) dest[0] = (uint16_t)i; dest[1] = (uint16_t)(i >> 16); } + +/* Set a float to 4 bytes in swapped Modbus format (DCBA) */ +void modbus_set_float_swapped(float f, uint16_t *dest) +{ + uint32_t i; + + memcpy(&i, &f, sizeof(uint32_t)); + i = bswap_32(i); + dest[0] = (uint16_t)i; + dest[1] = (uint16_t)(i >> 16); +} diff --git a/src/modbus.h b/src/modbus.h index 70a011f..82f5a22 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -226,7 +226,9 @@ EXPORT void modbus_set_bits_from_bytes(uint8_t *dest, int index, unsigned int nb const uint8_t *tab_byte); EXPORT uint8_t modbus_get_byte_from_bits(const uint8_t *src, int index, unsigned int nb_bits); EXPORT float modbus_get_float(const uint16_t *src); +EXPORT float modbus_get_float_swapped(const uint16_t *src); EXPORT void modbus_set_float(float f, uint16_t *dest); +EXPORT void modbus_set_float_swapped(float f, uint16_t *dest); #include "modbus-tcp.h" #include "modbus-rtu.h" diff --git a/tests/unit-test-client.c b/tests/unit-test-client.c index 6785146..1d693d4 100644 --- a/tests/unit-test-client.c +++ b/tests/unit-test-client.c @@ -319,7 +319,7 @@ int main(int argc, char *argv[]) printf("\nTEST FLOATS\n"); /** FLOAT **/ - printf("1/2 Set float: "); + printf("1/4 Set float: "); modbus_set_float(UT_REAL, tab_rp_registers); if (tab_rp_registers[1] == (UT_IREAL >> 16) && tab_rp_registers[0] == (UT_IREAL & 0xFFFF)) { @@ -333,7 +333,7 @@ int main(int argc, char *argv[]) goto close; } - printf("2/2 Get float: "); + printf("2/4 Get float: "); real = modbus_get_float(tab_rp_registers); if (real == UT_REAL) { printf("OK\n"); @@ -342,6 +342,26 @@ int main(int argc, char *argv[]) goto close; } + printf("3/4 Set float swapped: "); + modbus_set_float_swapped(UT_REAL, tab_rp_registers); + if (tab_rp_registers[1] == (UT_IREAL_SWAPPED >> 16) && + tab_rp_registers[0] == (UT_IREAL_SWAPPED & 0xFFFF)) { + printf("OK\n"); + } else { + ireal = (uint32_t) tab_rp_registers[0] & 0xFFFF; + ireal |= (uint32_t) tab_rp_registers[1] << 16; + printf("FAILED (%x != %x)\n", ireal, UT_IREAL_SWAPPED); + goto close; + } + + printf("4/4 Get float swapped: "); + real = modbus_get_float_swapped(tab_rp_registers); + if (real == UT_REAL) { + printf("OK\n"); + } else { + printf("FAILED (%f != %f)\n", real, UT_REAL); + goto close; + } printf("\nAt this point, error messages doesn't mean the test has failed\n"); /** ILLEGAL DATA ADDRESS **/