Driver.hpp

Go to the documentation of this file.

#ifndef ADVANCED_NAVIGATION_ANPP_DRIVER_HPP
#define ADVANCED_NAVIGATION_ANPP_DRIVER_HPP

#include <imu_advanced_navigation_anpp/DeviceInformation.hpp>
#include <imu_advanced_navigation_anpp/Status.hpp>
#include <imu_advanced_navigation_anpp/Configuration.hpp>
#include <imu_advanced_navigation_anpp/CurrentConfiguration.hpp>
#include <iodrivers_base/Driver.hpp>
#include <base/samples/RigidBodyState.hpp>
#include <base/samples/RigidBodyAcceleration.hpp>
#include <base/samples/IMUSensors.hpp>
#include <gps_base/BaseTypes.hpp>
#include <gps_base/UTMConverter.hpp>

namespace imu_advanced_navigation_anpp 
{
    enum RESET_MODE
    {
        RESET_COLD,
        RESET_HOT,
        RESET_FACTORY
    };

    namespace protocol
    {
        struct Header;
    }

    namespace protocol
    {
        struct UnixTime;
        struct Status;
        struct GeodeticPositionStandardDeviation;
        struct GeodeticPosition;
        struct QuaternionOrientation;
        struct EulerOrientationStandardDeviation;
        struct NEDVelocity;
        struct NEDVelocityStandardDeviation;
        struct BodyAcceleration;
        struct BodyVelocity;
        struct AngularVelocity;
        struct AngularAcceleration;
        struct RawSensors;
        struct RawGNSS;
        struct Satellites;
        struct NorthSeekingInitializationStatus;
    }

    class Driver : public iodrivers_base::Driver
    {
    private:
        static constexpr int PACKET_ID_COUNT = 256;

        bool mUseDeviceTime = false;
        uint8_t mLastPacketID = 0;
        base::Time mCurrentTimestamp;
        Eigen::Quaterniond const ned2nwu;

        gps_base::UTMConverter mUTMConverter;
        std::vector<uint32_t> mLastPackets;
        std::vector<std::pair<uint32_t, uint8_t>> mPacketPeriods;

        base::samples::RigidBodyState mWorld;
        base::samples::RigidBodyState mBody;
        base::samples::RigidBodyAcceleration mAcceleration;
        base::samples::IMUSensors mIMUSensors;
        gps_base::Solution mGeodeticPosition;
        gps_base::Solution mGNSSSolution;
        gps_base::SolutionQuality mGNSSSolutionQuality;
        gps_base::SatelliteInfo mGNSSSatelliteInfo;
        Status mStatus;

        void updateWorldFromGeodetic();

        int extractPacket(uint8_t const* buffer, size_t buffer_size) const;
        void setPacketPeriod(uint8_t packet_id, uint32_t period, bool clear_existing = false);

        template<typename Packet>
        void dispatch(uint8_t const* packet, uint8_t const* packet_end);
        void process(protocol::UnixTime const& payload);
        void process(protocol::Status const& payload);
        void process(protocol::GeodeticPositionStandardDeviation const& payload);
        void process(protocol::QuaternionOrientation const& payload);
        void process(protocol::EulerOrientationStandardDeviation const& payload);
        void process(protocol::NEDVelocity const& payload);
        void process(protocol::NEDVelocityStandardDeviation const& payload);
        void process(protocol::BodyAcceleration const& payload);
        void process(protocol::BodyVelocity const& payload);
        void process(protocol::AngularVelocity const& payload);
        void process(protocol::AngularAcceleration const& payload);
        void process(protocol::RawSensors const& payload);
        void process(protocol::RawGNSS const& payload);
        void process(protocol::Satellites const& payload);
        void process(protocol::GeodeticPosition const& payload);
        void process(protocol::NorthSeekingInitializationStatus const& payload);
        void processDetailedSatellites(uint8_t const* packet, uint8_t const* packet_end);

    public:
        Driver();

        void openURI(std::string const& uri);

        void setDeviceBaudrate(uint32_t rate);

        bool getUseDeviceTime() const;

        void setUseDeviceTime(bool enable);
        
        void clearPeriodicPackets();

        void reset(RESET_MODE mode);

        DeviceInformation readDeviceInformation();

        base::Time readTime();

        Status readStatus();

        CurrentConfiguration readConfiguration();

        void setConfiguration(Configuration const& conf);

        Status getIMUStatus() const;

        base::samples::RigidBodyState getWorldRigidBodyState() const;

        base::samples::RigidBodyState getBodyRigidBodyState() const;

        base::samples::RigidBodyAcceleration getAcceleration() const;

        base::samples::IMUSensors getIMUSensors() const;

        gps_base::Solution getGNSSSolution() const;

        gps_base::SolutionQuality getGNSSSolutionQuality() const;

        gps_base::SatelliteInfo getGNSSSatelliteInfo() const;

        void setStatusPeriod(int period);

        void setUTM(int zone, bool north,
                    Eigen::Vector3d const& local_origin = Eigen::Vector3d::Zero());

        void setPositionPeriod(int period, bool with_errors = true);

        void setOrientationPeriod(int period, bool with_errors = true);

        void setNEDVelocityPeriod(int period, bool with_errors = true);

        void setAccelerationPeriod(int period);

        void setBodyVelocityPeriod(int period);

        void setAngularVelocityPeriod(int period);

        void setAngularAccelerationPeriod(int period);

        void setRawSensorsPeriod(int period);

        void setGNSSPeriod(int period);

        void setGNSSSatelliteSummaryPeriod(int period);

        void setGNSSSatelliteDetailsPeriod(int period);

        void setNorthSeekingInitializationStatusPeriod(int period);

        void setCurrentTimestamp(base::Time const& time);

        base::Time getCurrentTimestamp() const;

        int poll();

        void resetPollSynchronization();
    };
}

#endif