Modifications for new packet specs

lib/wlib/comm/PacketConstructor.cpp

@@ -14,43 +14,46 @@
 
 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<uint16_t>(val);
-    uint16_t float_part = static_cast<uint16_t>(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<uint64_t>(bits1) | (static_cast<uint64_t>(bits2) << 18) | (static_cast<uint64_t>(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) {
     *packet.data() |= *name.data() << 3;
 }
 
 inline void __set_bulk_data(packet64 &packet, uint64_t joined_bits) {
-    *packet.data() |= static_cast<uint32_t>(joined_bits << 9);
-    *(packet.data() + 1) |= static_cast<uint32_t>(joined_bits >> 23);
+    *packet.data() |= static_cast<uint32_t>(joined_bits << 10);
+    *(packet.data() + 1) |= static_cast<uint32_t>(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(

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);

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<uint64_t>(names[i] << 3), make_packet64(data, 0, names[i]).to_uint64());
+        ASSERT_EQ(static_cast<uint64_t>(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());
 }