diff --git a/lib/wlib/comm/PacketConstructor.cpp b/lib/wlib/comm/PacketConstructor.cpp index de4d7e19..ef01bb41 100644 --- a/lib/wlib/comm/PacketConstructor.cpp +++ b/lib/wlib/comm/PacketConstructor.cpp @@ -14,29 +14,31 @@ using namespace wlp; +union __float_encoder { + static_assert(sizeof(float) == sizeof(uint32_t), "Expected float to be 32-bits"); + float r; + uint32_t n; +}; + inline uint32_t __encode_float18(float val) { - uint32_t sign = 0; - if (val < 0) { - sign = 1; - val = -val; + if (val == 0.0f) { + // hack for zero values in place of a better compression algorithm + return 0; } - uint16_t int_part = static_cast(val); - uint16_t float_part = static_cast(floorf(((val - int_part) * 100) + 0.5f)); - uint32_t res = (sign << 17) | (int_part << 7) | float_part; - return res; + __float_encoder encoder{}; + encoder.r = val; + uint32_t t = (encoder.n & 0x7ff800) >> 11; + t |= (((encoder.n & 0x7f800000) >> 23) - 0x70) << 12; + t |= (encoder.n & 0x80000000) >> 14; + return t; } inline uint64_t __join_data(uint32_t bits1, uint32_t bits2, uint32_t bits3) { return static_cast(bits1) | (static_cast(bits2) << 18) | (static_cast(bits3) << 36); } -inline void __set_start_and_end(packet64 &packet) { - packet.set(0); - packet.set(63); -} - inline void __set_packet_type(packet64 &packet, const packet_type &type) { - *packet.data() |= *type.data() << 1; + *packet.data() |= *type.data(); } inline void __set_packet_name(packet64 &packet, packet_name name) { @@ -44,13 +46,14 @@ inline void __set_packet_name(packet64 &packet, packet_name name) { } inline void __set_bulk_data(packet64 &packet, uint64_t joined_bits) { - *packet.data() |= static_cast(joined_bits << 9); - *(packet.data() + 1) |= static_cast(joined_bits >> 23); + *packet.data() |= static_cast(joined_bits << 10); + *(packet.data() + 1) |= static_cast(joined_bits >> 22); } packet64 __packet_maker::build(const float data[3], const packet_type &type, const packet_name &name) { packet64 packet; - __set_start_and_end(packet); + // type and name are less than or equal to a byte + // so byte order does not matter __set_packet_type(packet, type); __set_packet_name(packet, name); __set_bulk_data(packet, __join_data( diff --git a/lib/wlib/comm/PacketConstructor.h b/lib/wlib/comm/PacketConstructor.h index 3fb22158..8ea69db3 100644 --- a/lib/wlib/comm/PacketConstructor.h +++ b/lib/wlib/comm/PacketConstructor.h @@ -17,8 +17,8 @@ namespace wlp { typedef Bitset<64> packet64; - typedef Bitset<2> packet_type; - typedef Bitset<6> packet_name; + typedef Bitset<3> packet_type; + typedef Bitset<7> packet_name; struct __packet_maker { static packet64 build(const float data[3], const packet_type &type, const packet_name &name); diff --git a/tests/comm/packet_check.cpp b/tests/comm/packet_check.cpp index d036c4c8..ea25caa0 100644 --- a/tests/comm/packet_check.cpp +++ b/tests/comm/packet_check.cpp @@ -3,19 +3,17 @@ using namespace wlp; -TEST(packet_constructor_test, test_make_packet_sets_start_and_end) { +TEST(packet_constructor_test, test_make_packet_all_zero_data) { float data[3] = {0, 0, 0}; packet64 packet = make_packet64(data, PacketType::SENSOR, 0); - ASSERT_TRUE(packet.test(0)); - ASSERT_TRUE(packet.test(63)); + ASSERT_EQ(0u, packet.data()[0]); + ASSERT_EQ(0u, packet.data()[1]); } TEST(packet_constructor_test, test_make_packet_sets_type) { float data[3] = {0, 0, 0}; int types[4] = {PacketType::SENSOR, PacketType::COMMAND, PacketType::STATE, PacketType::LOG}; - uint64_t base = 1; - base = base + (base << 63); - uint64_t expected[4] = {base, base | 2, base | 4, base | 6}; + uint64_t expected[4] = {0, 1, 2, 3}; for (int i = 0; i < 4; i++) { packet64 packet = make_packet64(data, types[i], 0); ASSERT_EQ(expected[i], packet.to_uint64()); @@ -24,17 +22,15 @@ TEST(packet_constructor_test, test_make_packet_sets_type) { TEST(packet_constructor_test, test_make_packet_sets_name) { float data[3] = {0, 0, 0}; - uint64_t base = 1; - base = base + (base << 63); - uint8_t names[] = {0, 12, 63, 22, 53}; + uint8_t names[] = {0, 12, 63, 22, 53, 110}; for (int i = 0; i < 5; i++) { - ASSERT_EQ(base | static_cast(names[i] << 3), make_packet64(data, 0, names[i]).to_uint64()); + ASSERT_EQ(static_cast(names[i] << 3), make_packet64(data, 0, names[i]).to_uint64()); } } TEST(packet_constructor_test, test_make_packet_sets_data) { float data[3] = {-724.99f, 846.53f, 442.59f}; - uint64_t expected = 11216053491503253429u; + uint64_t expected = 6839376459708669362u; packet64 packet = make_packet64(data, PacketType::STATE, 54); ASSERT_EQ(expected, packet.to_uint64()); }