Added initial parser (STIM)

parsers/CMakeLists.txt

@@ -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
 )
 

parsers/include/sentiboard/Package.hpp

@@ -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

parsers/include/sentiboard/messages/ImuMessage.hpp

+// 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_

parsers/include/sentiboard/messages/SentiboardMessage.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_

parsers/include/sentiboard/sensors/SentiboardParser.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_

parsers/include/sentiboard/sensors/StimParser.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_

parsers/src/Package.cpp

@@ -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);

parsers/tests/stim_parser_test.cpp

+// 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);
+    }
+}