Commit b7233868 authored by Sigurd M. Albrektsen's avatar Sigurd M. Albrektsen

Added initial parser (STIM)

parent f53f638b
......@@ -55,6 +55,7 @@ endif()
set(SENTIBOARD_TEST_SRCS
${TEST_SRC_DIR}/testmain.cpp
${TEST_SRC_DIR}/sentiboard_reader_test.cpp
${TEST_SRC_DIR}/stim_parser_test.cpp
${TEST_SRC_DIR}/test_utils.cpp
)
......
......@@ -19,11 +19,11 @@ class Package {
bool SyncPackage(const std::shared_ptr<std::istream> inData);
bool _isGood = false;
bool _hasOnboardTimestamp = false;
double _onboardTimestamp;
double _onboardTimestamp = 0;
uint32_t _tov;
uint32_t _toa;
uint32_t _tot;
uint32_t _tov = 0;
uint32_t _toa = 0;
uint32_t _tot = 0;
uint8_t _protocolVersion;
......@@ -40,19 +40,31 @@ class Package {
bool IsGood() {
return _isGood;
}
size_t SensorIx() {
size_t SensorIx() const {
return _sensor_ix;
}
size_t OnboardTimestamp() const {
return _onboardTimestamp;
}
uint32_t tov() {
uint32_t tov() const {
return _tov;
}
uint32_t toa() {
uint32_t toa() const {
return _toa;
}
uint32_t tot() {
uint32_t tot() const {
return _tot;
}
const size_t data_start_position() const;
const uint8_t *data() const {
return reinterpret_cast<const uint8_t *>(&_data[data_start_position()]);
}
const size_t data_len() const {
return _data.size() - data_start_position() - 2;
}
};
} // namespace sentiboard
......
// Copyright [2017] <Sigurd M. Albrektsen>
#ifndef INCLUDE_SENTIBOARD_MESSAGES_IMUMESSAGE_HPP_
#define INCLUDE_SENTIBOARD_MESSAGES_IMUMESSAGE_HPP_
#include <tuple>
#include "SentiboardMessage.hpp"
#include "sentiboard/Package.hpp"
class ImuMessage : public SentiboardMessage {
public:
ImuMessage() { }
ImuMessage(const sentiboard::Package package,
std::tuple<float, float, float> accl,
std::tuple<float, float, float> gyro,
bool has_delta_values)
: SentiboardMessage(package) {
populate(package, accl, gyro, has_delta_values);
}
bool populate(const sentiboard::Package package,
std::tuple<float, float, float> accl,
std::tuple<float, float, float> gyro,
bool has_delta_values) {
accl_ = accl;
gyro_ = gyro;
has_delta_values_ = has_delta_values;
if(!SentiboardMessage::initialized()) {
SentiboardMessage::populate(package);
}
return true;
}
void has_delta_values(bool val) {
has_delta_values_ = true;
}
bool has_delta_values() {
return has_delta_values_;
}
std::tuple<float, float, float> accl() {
return accl_;
}
std::tuple<float, float, float> gyro() {
return gyro_;
}
private:
std::tuple<float, float, float> accl_;
std::tuple<float, float, float> gyro_;
bool has_delta_values_;
};
#endif // INCLUDE_SENTIBOARD_MESSAGES_IMUMESSAGE_HPP_
// Copyright [2017] <Sigurd M. Albrektsen>
#ifndef INCLUDE_SENTIBOARD_MESSAGES_SENTIBOARDMESSAGE_HPP_
#define INCLUDE_SENTIBOARD_MESSAGES_SENTIBOARDMESSAGE_HPP_
#include "sentiboard/Package.hpp"
class SentiboardMessage {
public:
SentiboardMessage(const sentiboard::Package& package) {
populate(package);
}
SentiboardMessage() {
initialized_ = false;
}
bool populate(const sentiboard::Package& package) {
sensor_ix_ = package.SensorIx();
tov_ = package.tov();
toa_ = package.toa();
tot_ = package.tot();
onboard_timestamp_ = package.OnboardTimestamp();
initialized_ = true;
return true;
}
~SentiboardMessage() { }
SentiboardMessage(const SentiboardMessage& other) {// copy constructor
sensor_ix_ = other.sensor_ix_;
tov_ = other.tov_;
toa_ = other.toa_;
tot_ = other.tot_;
onboard_timestamp_ = other.onboard_timestamp_;
}
//copy assignment
SentiboardMessage& operator=(const SentiboardMessage& other) {
sensor_ix_ = other.sensor_ix_;
tov_ = other.tov_;
toa_ = other.toa_;
tot_ = other.tot_;
onboard_timestamp_ = other.onboard_timestamp_;
return *this;
}
//Copy constructor
SentiboardMessage(const SentiboardMessage &&other) {
sensor_ix_ = other.sensor_ix_;
tov_ = other.tov_;
toa_ = other.toa_;
tot_ = other.tot_;
onboard_timestamp_ = other.onboard_timestamp_;
}
//move assignment
SentiboardMessage& operator=(SentiboardMessage&& other) {
if(this == &other) {
return *this;
}
this->sensor_ix_ = other.sensor_ix_;
this->tov_ = other.tov_;
this->toa_ = other.toa_;
this->tot_ = other.tot_;
this->onboard_timestamp_ = other.onboard_timestamp_;
return *this;
}
uint8_t sensor_ix() const {
return sensor_ix_;
}
uint32_t tov() const {
return tov_;
}
uint32_t toa() const {
return toa_;
}
uint32_t tot() const {
return tot_;
}
double onboard_timestamp() const {
return onboard_timestamp_;
}
bool initialized() {
return initialized_;
}
private:
uint8_t sensor_ix_;
uint32_t tov_;
uint32_t toa_;
uint32_t tot_;
double onboard_timestamp_;
bool initialized_;
};
#endif // INCLUDE_SENTIBOARD_MESSAGES_SENTIBOARDMESSAGE_HPP_
// Copyright [2017] <Sigurd M. Albrektsen>
#ifndef INCLUDE_SENTIBOARD_SENSORS_SENTIBOARDPARSER_HPP_
#define INCLUDE_SENTIBOARD_SENSORS_SENTIBOARDPARSER_HPP_
#include "sentiboard/messages/SentiboardMessage.hpp"
class SentiboardParser {
public:
virtual SentiboardMessage parse(const sentiboard::Package package) {
SentiboardMessage msg(package);
return msg;
}
};
#endif // INCLUDE_SENTIBOARD_SENSORS_SENTIBOARDPARSER_HPP_
// Copyright [2017] <Sigurd M. Albrektsen>
#ifndef INCLUDE_SENTIBOARD_SENSORS_STIMPARSER_HPP_
#define INCLUDE_SENTIBOARD_SENSORS_STIMPARSER_HPP_
#include "sentiboard/messages/ImuMessage.hpp"
#include "SentiboardParser.hpp"
#include <iostream>
class StimParser : public SentiboardParser {
private:
float stim_bytes_to_float(const uint8_t *ptr, uint8_t div) {
return stim_bytes_to_float((int8_t)ptr[0], ptr[1], ptr[2], div);
}
float stim_bytes_to_float(int8_t a, uint8_t b, uint8_t c, uint8_t div) {
float tmp = (a << 16) + (b << 8) + c;
return tmp / (1 << div);
}
public:
bool parse(const sentiboard::Package package, ImuMessage *message,
bool delta_values) {
if (package.data_len() != 38) {
return false;
}
const uint8_t *data = package.data();
if (data[0] != 0x93) {
return false;
}
uint8_t gyro_div = delta_values ? 21 : 14;
float g_x = stim_bytes_to_float(&data[1], gyro_div);
float g_y = stim_bytes_to_float(&data[4], gyro_div);
float g_z = stim_bytes_to_float(&data[7], gyro_div);
std::tuple<float, float, float> gyro(g_x, g_y, g_z);
// TODO: implement other than 10g
uint8_t accl_div = delta_values ? 22 : 19;
float a_x = stim_bytes_to_float(&data[11], accl_div);
float a_y = stim_bytes_to_float(&data[14], accl_div);
float a_z = stim_bytes_to_float(&data[17], accl_div);
std::tuple<float, float, float> accl(a_x, a_y, a_z);
message->populate(package, accl, gyro, delta_values);
return true;
}
};
#endif // INCLUDE_SENTIBOARD_SENSORS_STIMPARSER_HPP_
......@@ -92,6 +92,13 @@ bool Package::SyncPackage(const std::shared_ptr<std::istream> inData) {
return true;
}
const size_t Package::data_start_position() const {
if (_protocolVersion > 0) {
return SENTIBOARD_TOT_POS + 4;
}
return SENTIBOARD_TOA_POS + 4;
}
bool Package::ReadPackage(const std::shared_ptr<std::istream> inData,
bool printErrors) {
_data.resize(8);
......
// Copyright [2017] <Sigurd M. Albrektsen>
#include <unistd.h>
#include <stdio.h>
#include <iostream>
#include <vector>
#include "framework/test_framework.hpp"
#include "sentiboard/Reader.hpp"
#include "sentiboard/sensors/SentiboardParser.hpp"
#include "sentiboard/sensors/StimParser.hpp"
#include "sentiboard/messages/ImuMessage.hpp"
#include "test_utils.hpp"
TEST_CASE("Stim message parser") {
sentiboard::Reader reader(
get_testdata_file("stim300", "10_packs_250hz.dat"));
auto parser = StimParser();
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};
for (size_t ix = 0; ix < 10; ix++) {
sentiboard::Package package = reader.GetPackage();
REQUIRE(package.IsGood());
ImuMessage message;
REQUIRE(parser.parse(package, &message, true));
CHECK(message.tov() == tovs[ix]);
CHECK(message.toa() == toas[ix]);
CHECK(message.tot() == tots[ix]);
REQUIRE(message.initialized());
REQUIRE(message.has_delta_values());
float acc_x, acc_y, acc_z;
std::tie(acc_x, acc_y, acc_z) = message.accl();
// The STIM ran at 250 Hz
const float rate = 250;
acc_x *= rate;
acc_y *= rate;
acc_z *= rate;
double norm2 = (acc_x*acc_x) + (acc_y*acc_y) + (acc_z*acc_z);
CHECK(norm2 > 9.5*9.5);
CHECK(norm2 < 10*10);
}
}
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