Commit 5f55af0d authored by enfo's avatar enfo

Merge branch 'develop' into 'master'

Develop

See merge request enfo/sentiboard-utils!9
parents 1a0f7da2 46da1829
Pipeline #123 passed with stages
in 1 minute and 1 second
#!/usr/bin/env python
import sys
import os
import numpy as np
dir_path = os.path.dirname(os.path.realpath(__file__))
sys.path.append(os.path.join(dir_path, '..'))
def print_usage():
print('Usage:')
print(' python rtklib2matlab.py [rover.pos]')
sys.exit(1)
def parseline(line, names):
values = line.split()
converters = [int] + [float]*4 + [int]*2 + [float]*8
conv_vals = [c(v) for c, v in zip(converters, values) ]
return dict(zip(names, conv_vals))
def fix_name(name):
name = name.strip().lower()
if '(' in name:
name = name[:name.find('(')]
name = name.replace('-', '_')
return name
if __name__ == '__main__':
import sys
if len(sys.argv) < 2:
print('No file specified.')
print('')
print_usage()
if len(sys.argv) == 3:
min_quality = int(sys.argv[2])
else:
min_quality = 1
infile = sys.argv[1]
print('Extracting average from %s with Q <= %d' % (infile, min_quality))
names = ['week', 'tow', 'ecef_x', 'ecef_y', 'ecef_z', 'Q', 'ns', 'sdx', 'sdy',
'sdz', 'sdxy', 'sdyz', 'sdzx', 'age', 'ratio']
n_lines = 0
with open(infile, 'rU') as f:
for line in f:
if line.startswith('%'):
if 'ratio' in line:
names = [fix_name(name) for name in line[1:].split()]
if 'gpst' in names:
names.insert(0, 'week')
continue
n_lines += 1
f.seek(0)
arr = np.zeros((4, n_lines))
arr_ix=0
for line in f:
if line.startswith('%'):
continue
data_line = parseline(line, names)
for key, val in data_line.items():
if key in ['x_ecef', 'lat']:
arr[0, arr_ix] = val
elif key in ['y_ecef', 'lon']:
arr[1, arr_ix] = val
elif key in ['z_ecef', 'alt']:
arr[2, arr_ix] = val
elif key == 'q':
arr[3, arr_ix] = val
arr_ix += 1
print('')
print('Found %d lines' % arr_ix)
legal_arr = arr[:, arr[3,:] > 0]
valid_arr = legal_arr[:, legal_arr[3,:] <= min_quality]
print('Found %d quality points.' % valid_arr.shape[1])
print('')
print('Mean: %s' % np.mean(valid_arr, axis=1)[:3])
......@@ -11,14 +11,22 @@ def print_usage():
print(' python rtklib2matlab.py [rover.pos]')
sys.exit(1)
def parseline(line):
names = ['week', 'tow', 'ecef_x', 'ecef_y', 'ecef_z', 'Q', 'ns', 'sdx', 'sdy',
'sdz', 'sdxy', 'sdyz', 'sdzx', 'age', 'ratio']
values = line.split()
def parseline(line, names):
values = line.split('|')
converters = [int] + [float]*4 + [int]*2 + [float]*8
conv_vals = [c(v) for c, v in zip(converters, values) ]
return dict(zip(names, conv_vals))
def handle_name(name):
name = name.strip().lower()
if '(' in name:
name = name[:name.index('(')]
name = name.replace('-', '_')
return name
if __name__ == '__main__':
import sys
......@@ -32,6 +40,9 @@ if __name__ == '__main__':
outfile = os.path.splitext(infile)[0] + '.mat'
print('Converting %s to to %s' % (infile, outfile))
names = ['week', 'tow', 'ecef_x', 'ecef_y', 'ecef_z', 'Q', 'ns', 'sdx', 'sdy',
'sdz', 'sdxy', 'sdyz', 'sdzx', 'age', 'ratio']
n_lines = 0
with open(infile, 'rU') as f:
for line in f:
......@@ -46,8 +57,12 @@ if __name__ == '__main__':
c_line = 0
for line in f:
if line.startswith('%'):
if 'age' in line:
names = [handle_name(name) for name in line.strip('% ').split('|')]
if names[0] == 'gpst':
names.insert(0, 'week')
continue
data_line = parseline(line)
data_line = parseline(line, names)
for key, val in data_line.items():
try:
data[key][c_line] = val
......
......@@ -55,6 +55,8 @@ 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
)
add_executable(${TEST_TARGET}
......@@ -79,4 +81,3 @@ if (CMAKE_BUILD_TYPE STREQUAL "Coverage")
SETUP_TARGET_FOR_COVERAGE(test_coverage ${TEST_TARGET} ${PROJECT_SOURCE_DIR}/coverage)
endif() #CMAKE_BUILD_TYPE STREQUAL "Coverage"
......@@ -18,6 +18,14 @@ class Package {
bool SyncPackage(const std::shared_ptr<std::istream> inData);
bool _isGood = false;
bool _hasOnboardTimestamp = false;
double _onboardTimestamp = 0;
uint32_t _tov = 0;
uint32_t _toa = 0;
uint32_t _tot = 0;
uint8_t _protocolVersion;
public:
Package() {
......@@ -27,14 +35,36 @@ 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() {
size_t SensorIx() const {
return _sensor_ix;
}
size_t OnboardTimestamp() const {
return _onboardTimestamp;
}
uint32_t tov() const {
return _tov;
}
uint32_t toa() const {
return _toa;
}
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:
explicit 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() { }
// 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_;
}
// 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 <tuple>
#include "sentiboard/messages/ImuMessage.hpp"
#include "SentiboardParser.hpp"
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(sigurdal): 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_
......@@ -178,17 +178,29 @@ class StimParser(object):
accl_x, accl_y, accl_z,
incl_x, incl_y, incl_z, temp)
'''
#print package[:20]
stim_package = StimPackage(pack.tov, pack.toa, pack.tot, pack.timestamp,
delta_values = True
if delta_values:
stim_package = StimPackage(pack.tov, pack.toa, pack.tot, pack.timestamp,
GX3, GX2, GX1, GY3, GY2, GY1, GZ3, GZ2, GZ1, GS,
AX3, AX2, AX1, AY3, AY2, AY1, AZ3, AZ2, AZ1, AS,
IX3, IX2, IX1, IY3, IY2, IY1, IZ3, IZ2, IZ1, IS,
COUNT, LATENCY, CRC,
angular_rate=False, incremental_angle=True,
acceleration=False, incremental_velocity=True,
incl_acceleration=False, incl_incremental_velocity=True,
incl_average=False, g_range='10g')
else:
stim_package = StimPackage(pack.tov, pack.toa, pack.tot, pack.timestamp,
GX3, GX2, GX1, GY3, GY2, GY1, GZ3, GZ2, GZ1, GS,
AX3, AX2, AX1, AY3, AY2, AY1, AZ3, AZ2, AZ1, AS,
IX3, IX2, IX1, IY3, IY2, IY1, IZ3, IZ2, IZ1, IS,
COUNT, LATENCY, CRC,
angular_rate=False, incremental_angle=True,
acceleration=False, incremental_velocity=True,
incl_acceleration=False, incl_incremental_velocity=True,
angular_rate=True, incremental_angle=False,
acceleration=True, incremental_velocity=False,
incl_acceleration=True, incl_incremental_velocity=False,
incl_average=False, g_range='10g')
return stim_package
......@@ -213,7 +225,7 @@ if __name__ == '__main__':
try:
while True:
package_buf = reader.get_package()
if package_buf == None:
break
try:
......
......@@ -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,9 +82,23 @@ 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;
}
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);
......@@ -74,16 +112,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 +138,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;
}
......
......@@ -8,30 +8,7 @@
#include "framework/test_framework.hpp"
#include "sentiboard/Reader.hpp"
const char kPathSeparator =
#ifdef _WIN32
'\\';
#else
'/';
#endif
std::string get_foldername(const std::string& str) {
size_t found;
found = str.find_last_of("/\\");
return str.substr(0, found);
}
std::string get_testdata_directory() {
std::string dir = get_foldername(__FILE__) + kPathSeparator + "test_files"
+ kPathSeparator;
return dir;
}
std::string get_testdata_file(const std::string& sensor,
const std::string& filename) {
return get_testdata_directory() + kPathSeparator + sensor + kPathSeparator
+ filename;
}
#include "test_utils.hpp"
TEST_CASE("Live Read Test", "[!mayfail]") {
sentiboard::Reader reader("/dev/ttySentiboard02");
......@@ -277,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();