mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
BitStream::read() - implementation and tests
This commit is contained in:
Pavel Kirienko
parent
3cf6a5ff60
commit
cb0ff11eea
@ -43,6 +43,7 @@ public:
|
||||
}
|
||||
|
||||
int write(const uint8_t* bytes, const int bitlen);
|
||||
int read(uint8_t* bytes, const int bitlen);
|
||||
|
||||
std::string toString() const;
|
||||
};
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <cassert>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/marshalling/bit_stream.hpp>
|
||||
|
||||
@ -18,6 +19,7 @@ int BitStream::write(const uint8_t* bytes, const int bitlen)
|
||||
assert(MAX_BYTES_PER_RW >= bytelen);
|
||||
tmp[0] = tmp[bytelen - 1] = 0;
|
||||
|
||||
std::fill(tmp, tmp + bytelen, 0);
|
||||
copyBitArray(bytes, 0, bitlen, tmp, bit_offset_ % 8);
|
||||
|
||||
const int new_bit_offset = bit_offset_ + bitlen;
|
||||
@ -34,13 +36,32 @@ int BitStream::write(const uint8_t* bytes, const int bitlen)
|
||||
* within the next write() operation.
|
||||
*/
|
||||
const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
|
||||
bit_offset_ = new_bit_offset;
|
||||
bit_offset_ = new_bit_offset; // TODO: DO NOT UPDATE ON FAILURE
|
||||
|
||||
if (write_res < 0)
|
||||
return write_res;
|
||||
return (write_res == bytelen) ? 0 : -1;
|
||||
}
|
||||
|
||||
int BitStream::read(uint8_t* bytes, const int bitlen)
|
||||
{
|
||||
uint8_t tmp[MAX_BYTES_PER_RW + 1];
|
||||
|
||||
const int bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
|
||||
assert(MAX_BYTES_PER_RW >= bytelen);
|
||||
|
||||
const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
|
||||
if (read_res < 0)
|
||||
return read_res;
|
||||
if (read_res != bytelen)
|
||||
return -1;
|
||||
|
||||
std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0);
|
||||
copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0);
|
||||
bit_offset_ += bitlen;
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string BitStream::toString() const
|
||||
{
|
||||
std::ostringstream os;
|
||||
|
||||
@ -38,7 +38,7 @@ TEST(BitStream, ToString)
|
||||
}
|
||||
|
||||
|
||||
TEST(BitStream, BitOrder)
|
||||
TEST(BitStream, BitOrderSimple)
|
||||
{
|
||||
/*
|
||||
* a = 1010
|
||||
@ -48,21 +48,37 @@ TEST(BitStream, BitOrder)
|
||||
* e = 1110
|
||||
* f = 1111
|
||||
*/
|
||||
{ // Partial write
|
||||
uavcan::StaticTransferBuffer<32> buf;
|
||||
{ // Write
|
||||
const uint8_t data[] = {0xad, 0xbe}; // adbe
|
||||
uavcan::StaticTransferBuffer<32> buf;
|
||||
uavcan::BitStream bs(buf);
|
||||
ASSERT_EQ(0, bs.write(data, 12)); // adb0
|
||||
ASSERT_EQ("10101101 10110000", bs.toString()); // adb0
|
||||
}
|
||||
{ // Multiple partial write
|
||||
{ // Read
|
||||
uavcan::BitStream bs(buf);
|
||||
ASSERT_EQ("10101101 10110000", bs.toString()); // Same data
|
||||
uint8_t data[] = {0xFF, 0xFF}; // Uninitialized
|
||||
ASSERT_EQ(0, bs.read(data, 12));
|
||||
ASSERT_EQ(0xad, data[0]);
|
||||
ASSERT_EQ(0xb0, data[1]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(BitStream, BitOrderComplex)
|
||||
{
|
||||
static const std::string REFERENCE =
|
||||
"10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100";
|
||||
|
||||
uavcan::StaticTransferBuffer<32> buf;
|
||||
{ // Write
|
||||
const uint8_t data1[] = {0xad, 0xbe}; // 10101101 10111110
|
||||
const uint8_t data2[] = {0xfc}; // 11111100
|
||||
const uint8_t data3[] = {0xde, 0xad, 0xbe, 0xef}; // 11011110 10101101 10111110 11101111
|
||||
const uint8_t data4[] = {0x12, 0x34, 0x56, 0x78, // 00010010 00110100 01010110 01111000
|
||||
0x9a, 0xbc, 0xde, 0xf0}; // 10011010 10111100 11011110 11110000
|
||||
|
||||
uavcan::StaticTransferBuffer<32> buf;
|
||||
uavcan::BitStream bs(buf);
|
||||
ASSERT_EQ(0, bs.write(data1, 11)); // 10101101 101
|
||||
std::cout << bs.toString() << std::endl;
|
||||
@ -75,13 +91,39 @@ TEST(BitStream, BitOrder)
|
||||
ASSERT_EQ(0, bs.write(data4, 4)); // 0001
|
||||
std::cout << bs.toString() << std::endl;
|
||||
|
||||
static const std::string REFERENCE =
|
||||
"10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100";
|
||||
|
||||
std::cout << "Reference:\n" << REFERENCE << std::endl;
|
||||
|
||||
ASSERT_EQ(REFERENCE, bs.toString());
|
||||
}
|
||||
{ // Read back in the same order
|
||||
uint8_t data[8];
|
||||
std::fill(data, data + sizeof(data), 0xA5); // Filling with garbage
|
||||
uavcan::BitStream bs(buf);
|
||||
ASSERT_EQ(REFERENCE, bs.toString());
|
||||
|
||||
ASSERT_EQ(0, bs.read(data, 11)); // 10101101 10100000
|
||||
ASSERT_EQ(0xad, data[0]);
|
||||
ASSERT_EQ(0xa0, data[1]);
|
||||
|
||||
ASSERT_EQ(0, bs.read(data, 6)); // 11111100
|
||||
ASSERT_EQ(0xfc, data[0]);
|
||||
|
||||
ASSERT_EQ(0, bs.read(data, 25)); // 11011110 10101101 10111110 10000000
|
||||
ASSERT_EQ(0xde, data[0]);
|
||||
ASSERT_EQ(0xad, data[1]);
|
||||
ASSERT_EQ(0xbe, data[2]);
|
||||
ASSERT_EQ(0x80, data[3]);
|
||||
|
||||
ASSERT_EQ(0, bs.read(data, 64)); // Data - see above
|
||||
ASSERT_EQ(0x12, data[0]);
|
||||
ASSERT_EQ(0x34, data[1]);
|
||||
ASSERT_EQ(0x56, data[2]);
|
||||
ASSERT_EQ(0x78, data[3]);
|
||||
ASSERT_EQ(0x9a, data[4]);
|
||||
ASSERT_EQ(0xbc, data[5]);
|
||||
ASSERT_EQ(0xde, data[6]);
|
||||
ASSERT_EQ(0xf0, data[7]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -89,7 +131,7 @@ TEST(BitStream, BitByBit)
|
||||
{
|
||||
static const int NUM_BYTES = 1024;
|
||||
uavcan::StaticTransferBuffer<NUM_BYTES> buf;
|
||||
uavcan::BitStream bs(buf);
|
||||
uavcan::BitStream bs_wr(buf);
|
||||
|
||||
std::string binary_string;
|
||||
unsigned int counter = 0;
|
||||
@ -100,7 +142,7 @@ TEST(BitStream, BitByBit)
|
||||
const bool value = counter % 3 == 0;
|
||||
binary_string.push_back(value ? '1' : '0');
|
||||
const uint8_t data[] = { value << 7 };
|
||||
ASSERT_EQ(0, bs.write(data, 1));
|
||||
ASSERT_EQ(0, bs_wr.write(data, 1));
|
||||
}
|
||||
binary_string.push_back(' ');
|
||||
}
|
||||
@ -109,11 +151,43 @@ TEST(BitStream, BitByBit)
|
||||
/*
|
||||
* Currently we have no free buffer space, so next write() must fail
|
||||
*/
|
||||
const uint8_t data[] = { 0xFF };
|
||||
ASSERT_EQ(-1, bs.write(data, 1));
|
||||
const uint8_t dummy_data_wr[] = { 0xFF };
|
||||
ASSERT_EQ(-1, bs_wr.write(dummy_data_wr, 1));
|
||||
|
||||
/*
|
||||
* Bitstream content validation
|
||||
*/
|
||||
// std::cout << bs.toString() << std::endl;
|
||||
// std::cout << "Reference:\n" << binary_string << std::endl;
|
||||
ASSERT_EQ(binary_string, bs_wr.toString());
|
||||
|
||||
ASSERT_EQ(binary_string, bs.toString());
|
||||
/*
|
||||
* Read back
|
||||
*/
|
||||
uavcan::BitStream bs_rd(buf);
|
||||
counter = 0;
|
||||
for (int byte = 0; byte < NUM_BYTES; byte++)
|
||||
{
|
||||
for (int bit = 0; bit < 8; bit++, counter++)
|
||||
{
|
||||
const bool value = counter % 3 == 0;
|
||||
uint8_t data[1];
|
||||
ASSERT_EQ(0, bs_rd.read(data, 1));
|
||||
if (value)
|
||||
{
|
||||
ASSERT_EQ(0x80, data[0]);
|
||||
}
|
||||
else
|
||||
{
|
||||
ASSERT_EQ(0, data[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Making sure that reading out of buffer range will fail with error
|
||||
*/
|
||||
uint8_t dummy_data_rd[] = { 0xFF };
|
||||
ASSERT_EQ(-1, bs_wr.read(dummy_data_rd, 1));
|
||||
ASSERT_EQ(0xFF, dummy_data_rd[0]);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user