VectorNavSensor.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
// VectorNav library includes <winsock2.h>, but <boost/asio.hpp> needs to be included before (even though not used in this file)
// https://stackoverflow.com/questions/9750344/boostasio-winsock-and-winsock-2-compatibility-issue
#ifdef _WIN32
    // Set the proper SDK version before including boost/Asio
    #include <SDKDDKVer.h>
    // Note boost/ASIO includes Windows.h.
    #include <boost/asio.hpp>
#endif //_WIN32
 
#include "Nodes/DataProvider/IMU/Imu.hpp"
#include "Nodes/DataProvider/Protocol/UartSensor.hpp"
#include "vn/sensors.h"
 
#include "NodeData/IMU/VectorNavBinaryOutput.hpp"
 
#include "Navigation/Time/InsTime.hpp"
#include "util/Container/ScrollingBuffer.hpp"
 
#include <vector>
#include <array>
#include <cstdint>
 
namespace NAV
{
class VectorNavFile;
 
class VectorNavSensor : public Imu, public UartSensor
{
  public:
    struct TimeSync
    {
        InsTime ppsTime;       
        uint32_t syncOutCnt{}; 
    };
 
    VectorNavSensor();
    ~VectorNavSensor() override;
    VectorNavSensor(const VectorNavSensor&) = delete;
    VectorNavSensor(VectorNavSensor&&) = delete;
    VectorNavSensor& operator=(const VectorNavSensor&) = delete;
    VectorNavSensor& operator=(VectorNavSensor&&) = delete;
 
    [[nodiscard]] static std::string typeStatic();
 
    [[nodiscard]] std::string type() const override;
 
    [[nodiscard]] static std::string category();
 
    void guiConfig() override;
 
    [[nodiscard]] json save() const override;
 
    void restore(const json& j) override;
 
    bool resetNode() override;
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_ASCII_OUTPUT = 0; 
 
    bool initialize() override;
 
    void deinitialize() override;
 
    static void mergeVectorNavBinaryObservations(const std::shared_ptr<VectorNavBinaryOutput>& target, const std::shared_ptr<VectorNavBinaryOutput>& source);
 
    static void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index);
 
    enum class VectorNavModel : uint8_t
    {
        VN100_VN110,
        VN310,
    };
 
    VectorNavModel _sensorModel = VectorNavModel::VN100_VN110;
 
    vn::sensors::VnSensor _vs;
 
    std::string _connectedSensorPort;
 
    static constexpr double IMU_DEFAULT_FREQUENCY = 800;
 
    std::pair<std::vector<uint16_t>, std::vector<std::string>> _dividerFrequency;
 
    std::array<InsTime, 3> _lastMessageTime{};
 
    std::array<uint64_t, 3> _lastMessageTimeSinceStartup{};
 
    struct
    {
        InsTime lastGnssTime;        
        uint64_t timeSinceStartup{}; 
    } _gnssTimeCounter;
 
    // ###########################################################################################################
    //                                               SYSTEM MODULE
    // ###########################################################################################################
 
    vn::protocol::uart::AsciiAsync _asyncDataOutputType = vn::protocol::uart::AsciiAsync::VNOFF;
 
    static constexpr std::array _possibleAsyncDataOutputFrequency = { 1, 2, 4, 5, 10, 20, 25, 40, 50, 100, 200 };
 
    uint32_t _asyncDataOutputFrequency = 40;
    int _asyncDataOutputFrequencySelected = 7;
 
    int _asciiOutputBufferSize = 10;
 
    ScrollingBuffer<std::string> _asciiOutputBuffer{ static_cast<size_t>(_asciiOutputBufferSize) };
 
    vn::sensors::SynchronizationControlRegister _synchronizationControlRegister{
        vn::protocol::uart::SyncInMode::SYNCINMODE_COUNT,              // SyncInMode
        vn::protocol::uart::SyncInEdge::SYNCINEDGE_RISING,             // SyncInEdge
        0,                                                             // SyncInSkipFactor
        vn::protocol::uart::SyncOutMode::SYNCOUTMODE_NONE,             // SyncOutMode
        vn::protocol::uart::SyncOutPolarity::SYNCOUTPOLARITY_POSITIVE, // SyncOutPolarity
        0,                                                             // SyncOutSkipFactor
        100000000                                                      // SyncOutPulseWidth
    };
 
    TimeSync _timeSyncOut;
 
    bool _syncInPin = false;
 
    bool _coupleImuRateOutput = true;
 
    static void coupleImuFilterRates(NAV::VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField);
 
    vn::sensors::CommunicationProtocolControlRegister _communicationProtocolControlRegister{
        vn::protocol::uart::CountMode::COUNTMODE_NONE,           // SerialCount
        vn::protocol::uart::StatusMode::STATUSMODE_OFF,          // SerialStatus
        vn::protocol::uart::CountMode::COUNTMODE_NONE,           // SPICount
        vn::protocol::uart::StatusMode::STATUSMODE_OFF,          // SPIStatus
        vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CHECKSUM, // SerialChecksum
        vn::protocol::uart::ChecksumMode::CHECKSUMMODE_OFF,      // SPIChecksum
        vn::protocol::uart::ErrorMode::ERRORMODE_SEND            // ErrorMode
    };
 
    enum class BinaryRegisterMerge : uint8_t
    {
        None,            
        Output1_Output2, 
        Output1_Output3, 
        Output2_Output3, 
    };
 
    BinaryRegisterMerge _binaryOutputRegisterMerge = BinaryRegisterMerge::None;
 
    std::shared_ptr<VectorNavBinaryOutput> _binaryOutputRegisterMergeObservation = nullptr;
    size_t _binaryOutputRegisterMergeIndex{};
 
    std::array<vn::sensors::BinaryOutputRegister, 3> _binaryOutputRegister = { vn::sensors::BinaryOutputRegister{
                                                                                   vn::protocol::uart::AsyncMode::ASYNCMODE_PORT1,        // AsyncMode
                                                                                   800,                                                   // RateDivisor
                                                                                   vn::protocol::uart::CommonGroup::COMMONGROUP_NONE,     // CommonGroup
                                                                                   vn::protocol::uart::TimeGroup::TIMEGROUP_NONE,         // TimeGroup
                                                                                   vn::protocol::uart::ImuGroup::IMUGROUP_NONE,           // IMUGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE,           // GNSS1Group
                                                                                   vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
                                                                                   vn::protocol::uart::InsGroup::INSGROUP_NONE,           // INSGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE            // GNSS2Group
                                                                               },
                                                                               vn::sensors::BinaryOutputRegister{
                                                                                   vn::protocol::uart::AsyncMode::ASYNCMODE_NONE,         // AsyncMode
                                                                                   800,                                                   // RateDivisor
                                                                                   vn::protocol::uart::CommonGroup::COMMONGROUP_NONE,     // CommonGroup
                                                                                   vn::protocol::uart::TimeGroup::TIMEGROUP_NONE,         // TimeGroup
                                                                                   vn::protocol::uart::ImuGroup::IMUGROUP_NONE,           // IMUGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE,           // GNSS1Group
                                                                                   vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
                                                                                   vn::protocol::uart::InsGroup::INSGROUP_NONE,           // INSGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE            // GNSS2Group
                                                                               },
                                                                               vn::sensors::BinaryOutputRegister{
                                                                                   vn::protocol::uart::AsyncMode::ASYNCMODE_NONE,         // AsyncMode
                                                                                   800,                                                   // RateDivisor
                                                                                   vn::protocol::uart::CommonGroup::COMMONGROUP_NONE,     // CommonGroup
                                                                                   vn::protocol::uart::TimeGroup::TIMEGROUP_NONE,         // TimeGroup
                                                                                   vn::protocol::uart::ImuGroup::IMUGROUP_NONE,           // IMUGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE,           // GNSS1Group
                                                                                   vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
                                                                                   vn::protocol::uart::InsGroup::INSGROUP_NONE,           // INSGroup
                                                                                   vn::protocol::uart::GpsGroup::GPSGROUP_NONE            // GNSS2Group
                                                                               } };
    std::array<size_t, 3> _binaryOutputSelectedFrequency{};
 
    // ###########################################################################################################
    //                                               IMU SUBSYSTEM
    // ###########################################################################################################
 
    vn::math::mat3f _referenceFrameRotationMatrix{ { 1, 0, 0 },
                                                   { 0, 1, 0 },
                                                   { 0, 0, 1 } };
 
    vn::sensors::ImuFilteringConfigurationRegister _imuFilteringConfigurationRegister{
        4,                                                      // MagWindowSize
        4,                                                      // AccelWindowSize
        4,                                                      // GyroWindowSize
        4,                                                      // TempWindowSize
        4,                                                      // PresWindowSize
        vn::protocol::uart::FilterMode::FILTERMODE_NOFILTERING, // MagFilterMode
        vn::protocol::uart::FilterMode::FILTERMODE_BOTH,        // AccelFilterMode
        vn::protocol::uart::FilterMode::FILTERMODE_BOTH,        // GyroFilterMode
        vn::protocol::uart::FilterMode::FILTERMODE_BOTH,        // TempFilterMode
        vn::protocol::uart::FilterMode::FILTERMODE_NOFILTERING  // PresFilterMode
    };
 
    vn::sensors::DeltaThetaAndDeltaVelocityConfigurationRegister _deltaThetaAndDeltaVelocityConfigurationRegister{
        vn::protocol::uart::IntegrationFrame::INTEGRATIONFRAME_BODY,       // IntegrationFrame
        vn::protocol::uart::CompensationMode::COMPENSATIONMODE_NONE,       // GyroCompensation
        vn::protocol::uart::AccCompensationMode::ACCCOMPENSATIONMODE_NONE, // AccelCompensation
        vn::protocol::uart::EarthRateCorrection::EARTHRATECORR_NONE        // EarthRateCorrection
    };
 
    // ###########################################################################################################
    //                                              GNSS SUBSYSTEM
    // ###########################################################################################################
 
    vn::sensors::GpsConfigurationRegister _gpsConfigurationRegister{
        vn::protocol::uart::GpsMode::GPSMODE_ONBOARDGPS,       // Mode
        vn::protocol::uart::PpsSource::PPSSOURCE_GPSPPSRISING, // PpsSource
        vn::protocol::uart::GpsRate::GPSRATE_5HZ,              // Rate
        vn::protocol::uart::AntPower::ANTPOWER_INTERNAL        // AntPower
    };
 
    vn::math::vec3f _gpsAntennaOffset{
        0, 0, 0 // [m]
    };
 
    vn::sensors::GpsCompassBaselineRegister _gpsCompassBaselineRegister{
        vn::math::vec3f{ 1.0F, 0.0F, 0.0F },      // Position [m]
        vn::math::vec3f{ 0.254F, 0.254F, 0.254F } // Uncertainty [m]
    };
 
    // ###########################################################################################################
    //                                            ATTITUDE SUBSYSTEM
    // ###########################################################################################################
 
    vn::sensors::VpeBasicControlRegister _vpeBasicControlRegister{
        vn::protocol::uart::VpeEnable::VPEENABLE_ENABLE,       // Enable
        vn::protocol::uart::HeadingMode::HEADINGMODE_RELATIVE, // HeadingMode
        vn::protocol::uart::VpeMode::VPEMODE_MODE1,            // FilteringMode
        vn::protocol::uart::VpeMode::VPEMODE_MODE1             // TuningMode
    };
 
    vn::sensors::VpeMagnetometerBasicTuningRegister _vpeMagnetometerBasicTuningRegister{
        vn::math::vec3f{ 4.0F, 4.0F, 4.0F }, // BaseTuning [0 - 10]
        vn::math::vec3f{ 5.0F, 5.0F, 5.0F }, // AdaptiveTuning [0 - 10]
        vn::math::vec3f{ 5.5F, 5.5F, 5.5F }  // AdaptiveFiltering [0 - 10]
    };
 
    vn::sensors::VpeAccelerometerBasicTuningRegister _vpeAccelerometerBasicTuningRegister{
        vn::math::vec3f{ 6.0F, 6.0F, 6.0F }, // BaseTuning [0 - 10]
        vn::math::vec3f{ 3.0F, 3.0F, 3.0F }, // AdaptiveTuning [0 - 10]
        vn::math::vec3f{ 5.0F, 5.0F, 5.0F }  // AdaptiveFiltering [0 - 10]
    };
 
    vn::sensors::VpeGyroBasicTuningRegister _vpeGyroBasicTuningRegister{
        vn::math::vec3f{ 8.0F, 8.0F, 8.0F }, // VarianceAngularWalk [0 - 10]
        vn::math::vec3f{ 4.0F, 4.0F, 4.0F }, // BaseTuning [0 - 10]
        vn::math::vec3f{ 0.0F, 0.0F, 0.0F }  // AdaptiveTuning [0 - 10]
    };
 
    vn::math::vec3f _filterStartupGyroBias{
        0, 0, 0 // [rad/s]
    };
 
    // ###########################################################################################################
    //                                               INS SUBSYSTEM
    // ###########################################################################################################
 
    vn::sensors::InsBasicConfigurationRegisterVn300 _insBasicConfigurationRegisterVn300{
        vn::protocol::uart::Scenario::SCENARIO_GPSMOVINGBASELINEDYNAMIC, // Scenario
        true,                                                            // AhrsAiding
        true                                                             // EstBaseline
    };
 
    vn::sensors::StartupFilterBiasEstimateRegister _startupFilterBiasEstimateRegister{
        vn::math::vec3f{ 0, 0, 0 }, // GyroBias [rad/s]
        vn::math::vec3f{ 0, 0, 0 }, // AccelBias [m/s^2]
        0.0F                        // PressureBiasIn [m]
    };
 
    // ###########################################################################################################
    //                                    HARD/SOFT IRON ESTIMATOR SUBSYSTEM
    // ###########################################################################################################
 
    vn::sensors::MagnetometerCalibrationControlRegister _magnetometerCalibrationControlRegister{
        vn::protocol::uart::HsiMode::HSIMODE_RUN,            // HSIMode
        vn::protocol::uart::HsiOutput::HSIOUTPUT_USEONBOARD, // HSIOutput
        5                                                    // ConvergeRate
    };
 
    // ###########################################################################################################
    //                                      WORLD MAGNETIC & GRAVITY MODULE
    // ###########################################################################################################
 
    vn::sensors::MagneticAndGravityReferenceVectorsRegister _magneticAndGravityReferenceVectorsRegister{
        vn::math::vec3f{ 1.0F, 0.0F, 1.8F },      // MagRef [Gauss]
        vn::math::vec3f{ 0.0F, 0.0F, -9.793746F } // AccRef [m/s^2]
    };
 
    vn::sensors::ReferenceVectorConfigurationRegister _referenceVectorConfigurationRegister{
        true,                      // UseMagModel
        true,                      // UseGravityModel
        1000,                      // RecalcThreshold [m]
        0.0F,                      // Year [years]
        vn::math::vec3d{ 0, 0, 0 } // Position (Lat Lon Alt [deg deg m])
    };
 
    // ###########################################################################################################
    //                                              VELOCITY AIDING
    // ###########################################################################################################
 
    vn::sensors::VelocityCompensationControlRegister _velocityCompensationControlRegister{
        vn::protocol::uart::VelocityCompensationMode::VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT, // Mode
        0.1F,                                                                                   // VelocityTuning
        0.01F                                                                                   // RateTuning
    };
 
    // ###########################################################################################################
    //                                       Binary Group GUI Definitions
    // ###########################################################################################################
 
    struct BinaryGroupData
    {
        const char* name = nullptr;
        int flagsValue = 0;
        void (*tooltip)() = nullptr;
        bool (*isEnabled)(VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& bor, uint32_t binaryField) =
            [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; };
        void (*toggleFields)(VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) = nullptr;
    };
 
    // static const std::array<BinaryGroupData, 15> _binaryGroupCommon;
 
    static const std::array<BinaryGroupData, 10> _binaryGroupTime;
 
    static const std::array<BinaryGroupData, 11> _binaryGroupIMU;
 
    static const std::array<BinaryGroupData, 16> _binaryGroupGNSS;
 
    static const std::array<BinaryGroupData, 9> _binaryGroupAttitude;
 
    static const std::array<BinaryGroupData, 11> _binaryGroupINS;
 
    friend class NAV::VectorNavFile;
};
 
} // namespace NAV