Commit 5f55af0d authored by Sigurd M. Albrektsen's avatar Sigurd M. Albrektsen
Browse files

Merge branch 'develop' into 'master' 

Develop

See merge request enfo/sentiboard-utils!9
parents 1a0f7da2 46da1829

converters/rtklib2avg.py

0 → 100755
+84 −0
Original line number Diff line number Diff line 
#!/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])

converters/rtklib2matlab.py

+20 −5
Original line number Diff line number Diff line
@@ -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

parsers/CMakeLists.txt

+2 −1
Original line number Diff line number Diff line
@@ -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"

parsers/include/sentiboard/Package.hpp

+33 −3
Original line number Diff line number Diff line
@@ -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

parsers/include/sentiboard/messages/ImuMessage.hpp

0 → 100644
+54 −0
Original line number Diff line number Diff line 
// 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_