Commit 8515df8a authored by Sigurd M. Albrektsen's avatar Sigurd M. Albrektsen

Added support for sentiboard format 0x01

parent 4d510d1d
Pipeline #118 failed with stages
in 1 minute and 1 second
......@@ -18,6 +18,14 @@ class Package {
bool SyncPackage(const std::shared_ptr<std::istream> inData);
bool _isGood = false;
bool _hasOnboardTimestamp = false;
double _onboardTimestamp;
uint32_t _tov;
uint32_t _toa;
uint32_t _tot;
uint8_t _protocolVersion;
public:
Package() {
......@@ -27,14 +35,24 @@ class Package {
bool ReadPackage(const std::shared_ptr<std::istream> inData,
bool printErrors);
bool CheckHeader();
bool CheckPackage();
bool CheckHeader(bool printErrors = false);
bool CheckPackage(bool printErrors = false);
bool IsGood() {
return _isGood;
}
size_t SensorIx() {
return _sensor_ix;
}
uint32_t tov() {
return _tov;
}
uint32_t toa() {
return _toa;
}
uint32_t tot() {
return _tot;
}
};
} // namespace sentiboard
......
......@@ -5,6 +5,7 @@
#include <stdint.h>
#include <iostream>
#include <iomanip>
#define SENTIBOARD_MAX_SKIP 512
......@@ -12,9 +13,16 @@
#define SENTIBOARD_CHECKSUM_SIZE 2
#define SENTIBOARD_HEADER_CHECKSUM_POS (SENTIBOARD_HEADER_SIZE \
- SENTIBOARD_CHECKSUM_SIZE)
#define SENTIBOARD_GET_UINT16_AT(buf, pos) \
#define SENTIBOARD_TOV_POS (SENTIBOARD_HEADER_SIZE)
#define SENTIBOARD_TOA_POS (SENTIBOARD_TOV_POS + 4)
#define SENTIBOARD_TOT_POS (SENTIBOARD_TOA_POS + 4)
#define SENTIBOARD_GET_UINT16_AT(buf, pos) \
reinterpret_cast<uint16_t *>(buf + pos)[0]
#define SENTIBOARD_GET_UINT32_AT(buf, pos) \
reinterpret_cast<uint32_t *>(buf + pos)[0]
namespace sentiboard {
uint16_t Checksum(void *raw_data, size_t datalen) {
......@@ -28,20 +36,36 @@ uint16_t Checksum(void *raw_data, size_t datalen) {
return ((B << 8) + A);
}
bool Package::CheckHeader() {
bool Package::CheckHeader(bool printErrors) {
uint16_t checksum = SENTIBOARD_GET_UINT16_AT(_data.data(),
SENTIBOARD_HEADER_CHECKSUM_POS);
uint16_t check_calc = Checksum(_data.data(),
SENTIBOARD_HEADER_SIZE - SENTIBOARD_CHECKSUM_SIZE);
return check_calc == checksum;
bool ret = check_calc == checksum;
if (!ret && printErrors) {
auto flags = std::cerr.flags();
std::cerr << std::showbase;
std::cerr << "Header checksum failed: expected "
<< std::hex << checksum << " got " << check_calc << std::endl;
std::cerr.flags(flags);
}
return ret;
}
bool Package::CheckPackage() {
bool Package::CheckPackage(bool printErrors) {
uint16_t checksum = SENTIBOARD_GET_UINT16_AT(_data.data(),
_data.size() - SENTIBOARD_CHECKSUM_SIZE);
uint16_t check_calc = Checksum(_data.data() + SENTIBOARD_HEADER_SIZE,
_data.size() - SENTIBOARD_HEADER_SIZE - SENTIBOARD_CHECKSUM_SIZE);
return check_calc == checksum;
bool ret = check_calc == checksum;
if (!ret && printErrors) {
auto flags = std::cerr.flags();
std::cerr << std::showbase;
std::cerr << "Package checksum failed: expected " <<
std::hex << checksum << " got " << check_calc << std::endl;
std::cerr.flags(flags);
}
return ret;
}
bool Package::SyncPackage(const std::shared_ptr<std::istream> inData) {
......@@ -50,7 +74,7 @@ bool Package::SyncPackage(const std::shared_ptr<std::istream> inData) {
size_t max_skip = SENTIBOARD_MAX_SKIP;
while (_data[0] != '^' || _data[1] != 'B') {
while (_data[0] != '^' || !(_data[1] == 'B' || _data[1] == 'C')) {
if (max_skip-- <= 0) {
return false;
}
......@@ -58,6 +82,13 @@ bool Package::SyncPackage(const std::shared_ptr<std::istream> inData) {
*inData >> _data[1];
}
// TODO(sigurdal) add warning on data skip
if (_data[1] == 'C') {
_hasOnboardTimestamp = true;
_data[1] = 'B';
}
return true;
}
......@@ -74,16 +105,18 @@ bool Package::ReadPackage(const std::shared_ptr<std::istream> inData,
inData->read(&_data[2], 6);
if (!Package::CheckHeader()) {
if (printErrors) {
std::cerr << "Header checksum failed" << std::endl;
}
if (!Package::CheckHeader(printErrors)) {
return false;
}
if (_hasOnboardTimestamp) {
inData->read((char *)&_onboardTimestamp, 8);
}
uint16_t *headers = reinterpret_cast<uint16_t *>(&_data[0]);
uint16_t datalen = headers[1];
_sensor_ix = headers[2];
_sensor_ix = _data[4];
_protocolVersion = _data[5];
size_t readLength = datalen + SENTIBOARD_CHECKSUM_SIZE;
......@@ -98,7 +131,13 @@ bool Package::ReadPackage(const std::shared_ptr<std::istream> inData,
return false;
}
if (!Package::CheckPackage()) {
_tov = SENTIBOARD_GET_UINT32_AT(_data.data(), SENTIBOARD_TOV_POS);
_toa = SENTIBOARD_GET_UINT32_AT(_data.data(), SENTIBOARD_TOA_POS);
if (_protocolVersion > 0) {
_tot = SENTIBOARD_GET_UINT32_AT(_data.data(), SENTIBOARD_TOT_POS);
}
if (!Package::CheckPackage(printErrors)) {
if (printErrors) {
std::cerr << "Package checksum failed" << std::endl;
}
......
......@@ -12,13 +12,11 @@
namespace sentiboard {
Reader::Reader(std::string path) {
std::cout << "Opening " << path << std::endl;
auto stream = new std::ifstream(path.c_str());
_device = std::shared_ptr<std::istream> (stream);
}
Reader::Reader(const std::shared_ptr<std::istream> stream) {
std::cout << "Using stream" << std::endl;
_device = stream;
}
......
......@@ -254,3 +254,43 @@ TEST_CASE("Recorded 10 ADIS packages, injected errors") {
std::cerr.clear();
}
}
TEST_CASE("Version 0x01 format test") {
std::vector<uint32_t> tovs = {
289062970, 289462975, 289862981, 290262988, 290662995,
291063001, 291463008, 291863014, 292263021, 292663028};
std::vector<uint32_t> toas = {
289093370, 289493605, 289893805, 290293675, 290693885,
291093755, 291493677, 291893475, 292293372, 292693412};
std::vector<uint32_t> tots = {
289069600, 289469731, 289869768, 290269745, 290669849,
291069891, 291469867, 291869521, 292269562, 292669602};
sentiboard::Reader reader(
get_testdata_file("stim300", "10_packs_250hz.dat"));
std::vector<sentiboard::Package> packages;
REQUIRE(reader.IsOpen());
for (size_t ix = 0; ix < 9; ix++) {
sentiboard::Package package = reader.GetPackage();
REQUIRE(package.IsGood());
packages.push_back(package);
CHECK(package.SensorIx() == 2);
CHECK(package.tov() == tovs[ix]);
CHECK(package.toa() == toas[ix]);
CHECK(package.tot() == tots[ix]);
}
sentiboard::Package last_package = reader.GetPackage();
REQUIRE(last_package.IsGood());
CHECK(last_package.SensorIx() == 2);
CHECK(last_package.tov() == tovs[9]);
CHECK(last_package.toa() == toas[9]);
CHECK(last_package.tot() == tots[9]);
reader.PrintMessages(false);
sentiboard::Package error_package = reader.GetPackage();
REQUIRE(!error_package.IsGood());
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment