diff --git a/src/main/blackbox/blackbox_encoding.c b/src/main/blackbox/blackbox_encoding.c index dfa708e145..d0893bca81 100644 --- a/src/main/blackbox/blackbox_encoding.c +++ b/src/main/blackbox/blackbox_encoding.c @@ -231,6 +231,129 @@ void blackboxWriteTag2_3S32(int32_t *values) } } +/** + * Write a 2 bit tag followed by 3 signed fields of 2, 554, 877 or 32 bits + */ +int blackboxWriteTag2_3SVariable(int32_t *values) +{ + static const int FIELD_COUNT = 3; + enum { + BITS_2 = 0, + BITS_554 = 1, + BITS_877 = 2, + BITS_32 = 3 + }; + + enum { + BYTES_1 = 0, + BYTES_2 = 1, + BYTES_3 = 2, + BYTES_4 = 3 + }; + + + /* + * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes + * below: + * + * Selector possibilities + * + * 2 bits per field ss11 2233, + * 554 bits per field ss11 1112 2222 3333 + * 877 bits per field ss11 1111 1122 2222 2333 3333 + * 32 bits per field sstt tttt followed by fields of various byte counts + */ + int selector = BITS_2; + int selector2 = 0; + // Require more than 877 bits? + if (values[0] >= 256 || values[0] < -256 + || values[1] >= 128 || values[1] < -128 + || values[2] >= 128 || values[2] < -128) { + selector = BITS_32; + // Require more than 554 bits? + } else if (values[0] >= 16 || values[0] < -16 + || values[1] >= 16 || values[1] < -16 + || values[2] >= 8 || values[2] < -8) { + selector = BITS_877; + // Require more than 2 bits? + } else if (values[0] >= 2 || values[0] < -2 + || values[1] >= 2 || values[1] < -2 + || values[2] >= 2 || values[2] < -2) { + selector = BITS_554; + } + + switch (selector) { + case BITS_2: + blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); + break; + case BITS_554: + // 554 bits per field ss11 1112 2222 3333 + blackboxWrite((selector << 6) | ((values[0] & 0x1F) << 1) | ((values[1] & 0x1F) >> 4)); + blackboxWrite(((values[1] & 0x0F) << 4) | (values[2] & 0x0F)); + break; + case BITS_877: + // 877 bits per field ss11 1111 1122 2222 2333 3333 + blackboxWrite((selector << 6) | ((values[0] & 0xFF) >> 2)); + blackboxWrite(((values[0] & 0x03) << 6) | ((values[1] & 0x7F) >> 1)); + blackboxWrite(((values[1] & 0x01) << 7) | (values[2] & 0x7F)); + break; + case BITS_32: + /* + * Do another round to compute a selector for each field, assuming that they are at least 8 bits each + * + * Selector2 field possibilities + * 0 - 8 bits + * 1 - 16 bits + * 2 - 24 bits + * 3 - 32 bits + */ + selector2 = 0; + //Encode in reverse order so the first field is in the low bits: + for (int x = FIELD_COUNT - 1; x >= 0; x--) { + selector2 <<= 2; + + if (values[x] < 128 && values[x] >= -128) { + selector2 |= BYTES_1; + } else if (values[x] < 32768 && values[x] >= -32768) { + selector2 |= BYTES_2; + } else if (values[x] < 8388608 && values[x] >= -8388608) { + selector2 |= BYTES_3; + } else { + selector2 |= BYTES_4; + } + } + + //Write the selectors + blackboxWrite((selector << 6) | selector2); + + //And now the values according to the selectors we picked for them + for (int x = 0; x < FIELD_COUNT; x++, selector2 >>= 2) { + switch (selector2 & 0x03) { + case BYTES_1: + blackboxWrite(values[x]); + break; + case BYTES_2: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + case BYTES_3: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + break; + case BYTES_4: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + blackboxWrite(values[x] >> 24); + break; + } + } + break; + } + return selector; +} + /** * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. */ diff --git a/src/main/blackbox/blackbox_encoding.h b/src/main/blackbox/blackbox_encoding.h index 1293ed45ce..2cdb6d8388 100644 --- a/src/main/blackbox/blackbox_encoding.h +++ b/src/main/blackbox/blackbox_encoding.h @@ -27,6 +27,7 @@ void blackboxWriteSignedVBArray(int32_t *array, int count); void blackboxWriteSigned16VBArray(int16_t *array, int count); void blackboxWriteS16(int16_t value); void blackboxWriteTag2_3S32(int32_t *values); +int blackboxWriteTag2_3SVariable(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); void blackboxWriteU32(int32_t value); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 3d34216c5a..b1b9af001e 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -97,7 +97,8 @@ typedef enum FlightLogFieldEncoding { FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6, FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7, FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8, - FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero + FLIGHT_LOG_FIELD_ENCODING_NULL = 9, // Nothing is written to the file, take value to be zero + FLIGHT_LOG_FIELD_ENCODING_TAG2_3SVARIABLE = 10 } FlightLogFieldEncoding; typedef enum FlightLogFieldSign { diff --git a/src/test/unit/blackbox_encoding_unittest.cc b/src/test/unit/blackbox_encoding_unittest.cc index 652ae161a4..16ba59aa65 100644 --- a/src/test/unit/blackbox_encoding_unittest.cc +++ b/src/test/unit/blackbox_encoding_unittest.cc @@ -99,6 +99,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *instance) void serialTestResetBuffers() { + blackboxPort = &serialTestInstance; memset(&serialReadBuffer, 0, sizeof(serialReadBuffer)); serialReadPos = 0; serialReadEnd = 0; @@ -108,7 +109,8 @@ void serialTestResetBuffers() TEST(BlackboxEncodingTest, TestWriteUnsignedVB) { - blackboxPort = &serialTestInstance; + serialTestResetBuffers(); + blackboxWriteUnsignedVB(0); EXPECT_EQ(0, serialWriteBuffer[0]); blackboxWriteUnsignedVB(128); @@ -116,6 +118,128 @@ TEST(BlackboxEncodingTest, TestWriteUnsignedVB) EXPECT_EQ(1, serialWriteBuffer[2]); } +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS2) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 2 bits per field ss11 2233, + v[0] = 0; + v[1] = 0; + v[2] = 0; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0, buf[0]); + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = 1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0x10, buf[0]); // 00010000 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = 1; + v[1] = 1; + v[2] = 1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x15, buf[0]); // 00010101 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = -1; + v[1] = -1; + v[2] = -1; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x3F, buf[0]); // 00111111 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; + + v[0] = -2; + v[1] = -2; + v[2] = -2; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(0, selector); + EXPECT_EQ(0x2A, buf[0]); // 00101010 + EXPECT_EQ(0, buf[1]); // ensure next byte has not been written + ++buf; +} + +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS554) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 554 bits per field ss11 1112 2222 3333 + // 5 bits per field [-16, 15], 4 bits per field [-8, 7] + v[0] = 15; + v[1] = 15; + v[2] = 7; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x5E, buf[0]); // 0101 1110 + EXPECT_EQ(0xF7, buf[1]); // 1111 0111 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; + + v[0] = -16; + v[1] = -16; + v[2] = -8; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x61, buf[0]); // 0110 0001 + EXPECT_EQ(0x08, buf[1]); // 0000 1000 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; + + v[0] = 7; + v[1] = 8; + v[2] = 5; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(1, selector); + EXPECT_EQ(0x4E, buf[0]); // 0100 1110 + EXPECT_EQ(0x85, buf[1]); // 1000 0101 + EXPECT_EQ(0, buf[2]); // ensure next byte has not been written + buf += 2; +} + +TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS887) +{ + serialTestResetBuffers(); + uint8_t *buf = &serialWriteBuffer[0]; + int selector; + int32_t v[3]; + + // 877 bits per field ss11 1111 1122 2222 2333 3333 + // 8 bits per field [-128, 127], 7 bits per field [-64, 63] + v[0] = 127; + v[1] = 63; + v[2] = 63; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(2, selector); + EXPECT_EQ(0x9F, buf[0]); // 1001 1111 + EXPECT_EQ(0xDF, buf[1]); // 1101 1111 + EXPECT_EQ(0xBF, buf[2]); // 1011 1111 + EXPECT_EQ(0, buf[3]); // ensure next byte has not been written + buf += 3; + + v[0] = -128; + v[1] = -64; + v[2] = -64; + selector = blackboxWriteTag2_3SVariable(v); + EXPECT_EQ(2, selector); + EXPECT_EQ(0xA0, buf[0]); // 1010 0000 + EXPECT_EQ(0x20, buf[1]); // 0010 0000 + EXPECT_EQ(0x40, buf[2]); // 0100 0000 + EXPECT_EQ(0, buf[3]); // ensure next byte has not been written + buf += 3; +} // STUBS extern "C" { PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);