Merge pull request #2951 from martinbudden/bf_blackbox_3svariable
Added TAG2_3SVARIABLE encoding
This commit is contained in:
commit
676fc30316
|
@ -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.
|
* Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -27,6 +27,7 @@ void blackboxWriteSignedVBArray(int32_t *array, int count);
|
||||||
void blackboxWriteSigned16VBArray(int16_t *array, int count);
|
void blackboxWriteSigned16VBArray(int16_t *array, int count);
|
||||||
void blackboxWriteS16(int16_t value);
|
void blackboxWriteS16(int16_t value);
|
||||||
void blackboxWriteTag2_3S32(int32_t *values);
|
void blackboxWriteTag2_3S32(int32_t *values);
|
||||||
|
int blackboxWriteTag2_3SVariable(int32_t *values);
|
||||||
void blackboxWriteTag8_4S16(int32_t *values);
|
void blackboxWriteTag8_4S16(int32_t *values);
|
||||||
void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
|
void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
|
||||||
void blackboxWriteU32(int32_t value);
|
void blackboxWriteU32(int32_t value);
|
||||||
|
|
|
@ -97,7 +97,8 @@ typedef enum FlightLogFieldEncoding {
|
||||||
FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
|
FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
|
||||||
FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
|
FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
|
||||||
FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
|
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;
|
} FlightLogFieldEncoding;
|
||||||
|
|
||||||
typedef enum FlightLogFieldSign {
|
typedef enum FlightLogFieldSign {
|
||||||
|
|
|
@ -99,6 +99,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *instance)
|
||||||
|
|
||||||
void serialTestResetBuffers()
|
void serialTestResetBuffers()
|
||||||
{
|
{
|
||||||
|
blackboxPort = &serialTestInstance;
|
||||||
memset(&serialReadBuffer, 0, sizeof(serialReadBuffer));
|
memset(&serialReadBuffer, 0, sizeof(serialReadBuffer));
|
||||||
serialReadPos = 0;
|
serialReadPos = 0;
|
||||||
serialReadEnd = 0;
|
serialReadEnd = 0;
|
||||||
|
@ -108,7 +109,8 @@ void serialTestResetBuffers()
|
||||||
|
|
||||||
TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
|
TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
|
||||||
{
|
{
|
||||||
blackboxPort = &serialTestInstance;
|
serialTestResetBuffers();
|
||||||
|
|
||||||
blackboxWriteUnsignedVB(0);
|
blackboxWriteUnsignedVB(0);
|
||||||
EXPECT_EQ(0, serialWriteBuffer[0]);
|
EXPECT_EQ(0, serialWriteBuffer[0]);
|
||||||
blackboxWriteUnsignedVB(128);
|
blackboxWriteUnsignedVB(128);
|
||||||
|
@ -116,6 +118,128 @@ TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
|
||||||
EXPECT_EQ(1, serialWriteBuffer[2]);
|
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
|
// STUBS
|
||||||
extern "C" {
|
extern "C" {
|
||||||
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
|
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
|
||||||
|
|
Loading…
Reference in New Issue