Program Listing for File BufferedPva.hpp

↰ Return to documentation for file (src/navtk/inertial/BufferedPva.hpp)

#pragma once

#include <memory>
#include <utility>

#include <navtk/aspn.hpp>
#include <navtk/filtering/containers/TimestampedDataSeries.hpp>
#include <navtk/not_null.hpp>
#include <navtk/utils/Ordered.hpp>

namespace {

template <typename D, typename T>
bool safe_deref(const navtk::filtering::TimestampedDataSeries<D, T>& buf,
                typename navtk::utils::RingBuffer<std::shared_ptr<D>>::const_iterator& it) {
    return it != buf.cend() && *it != nullptr;
}

}  // namespace

namespace navtk {
namespace inertial {

class BufferedPva {

public:
    virtual ~BufferedPva() = default;

    BufferedPva(const BufferedPva&) = default;

    BufferedPva& operator=(const BufferedPva&) = default;

    BufferedPva(BufferedPva&&) = default;

    BufferedPva& operator=(BufferedPva&&) = default;

    virtual void add_data(not_null<std::shared_ptr<aspn_xtensor::AspnBase>> data) = 0;

    std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> calc_pva(
        const aspn_xtensor::TypeTimestamp& time) const;

    not_null<std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude>> calc_pva() const;

    std::pair<aspn_xtensor::TypeTimestamp, aspn_xtensor::TypeTimestamp> time_span() const;

    std::pair<int64_t, int64_t> nsec_time_span() const;

    bool in_range(const aspn_xtensor::TypeTimestamp& t) const;

    virtual std::shared_ptr<aspn_xtensor::MeasurementImu> calc_force_and_rate(
        const aspn_xtensor::TypeTimestamp& time) const;

    virtual std::shared_ptr<aspn_xtensor::MeasurementImu> calc_force_and_rate(
        const aspn_xtensor::TypeTimestamp& time1, const aspn_xtensor::TypeTimestamp& time2) const;

protected:
    BufferedPva(std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> pva = nullptr,
                double expected_dt                                                     = 1.0,
                double buffer_length                                                   = 60.0);

    BufferedPva(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva,
                double expected_dt   = 1.0,
                double buffer_length = 60.0);

    filtering::TimestampedDataSeries<aspn_xtensor::MeasurementPositionVelocityAttitude,
                                     aspn_xtensor::TypeTimestamp>
        pva_buf;

    std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> calc_pva_no_check(
        const aspn_xtensor::TypeTimestamp& time) const;
};

}  // namespace inertial
}  // namespace navtk