imu.hpp

Go to the documentation of this file.

/*
 * imu.hpp
 *
 *  Copyright (c) 2014 Kumar Robotics. All rights reserved.
 *
 *  This file is part of galt.
 *
 *  Created on: 2/6/2014
 *                Author: gareth
 */

#ifndef IMU_H_
#define IMU_H_

#include <stdexcept>
#include <memory>
#include <string>
#include <deque>
#include <queue>
#include <vector>
#include <bitset>
#include <map>
#include <boost/bind.hpp>

#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ //  will fail outside of gcc/clang
#define HOST_LITTLE_ENDIAN
#else
#define HOST_BIG_ENDIAN
#endif

namespace imu_3dm_gx4 {

class Imu {
public:
        struct Packet {
                static constexpr uint8_t kHeaderLength = 4;
                static constexpr uint8_t kSyncMSB = 0x75;
                static constexpr uint8_t kSyncLSB = 0x65;

                union {
                        struct {
                                uint8_t syncMSB;
                                uint8_t syncLSB;
                        };
                        uint16_t sync; 
                };

                uint8_t descriptor; 
                uint8_t length;     
                uint8_t payload[255]; 
                union {
                        struct {
                                uint8_t checkMSB;
                                uint8_t checkLSB;
                        };
                        uint16_t checksum; 
                };

                bool isIMUData() const;

                bool isFilterData() const;

                int ackErrorCodeFor(const Packet &command) const;

                void calcChecksum();

                Packet(uint8_t desc = 0);

                std::string toString() const;
        } __attribute__((packed));

        struct Info {
                uint16_t firmwareVersion;  
                std::string modelName;     
                std::string modelNumber;   
                std::string serialNumber;  
                std::string lotNumber;     
                std::string deviceOptions; 

                std::map<std::string, std::string> toMap() const;
        };

        struct DiagnosticFields {
                uint16_t modelNumber;
                uint8_t selector;
                uint32_t statusFlags;
                uint32_t systemTimer;
                uint32_t num1PPSPulses;
                uint32_t last1PPSPulse;
                uint8_t imuStreamEnabled;
                uint8_t filterStreamEnabled;
                uint32_t imuPacketsDropped;
                uint32_t filterPacketsDropped;
                uint32_t comBytesWritten;
                uint32_t comBytesRead;
                uint32_t comNumWriteOverruns;
                uint32_t comNumReadOverruns;
                uint32_t usbBytesWritten;
                uint32_t usbBytesRead;
                uint32_t usbNumWriteOverruns;
                uint32_t usbNumReadOverruns;
                uint32_t numIMUParseErrors;
                uint32_t totalIMUMessages;
                uint32_t lastIMUMessage;

                std::map<std::string, unsigned int> toMap() const;

        } __attribute__((packed));

        struct IMUData {
                enum {
                        Accelerometer = (1 << 0),
                        Gyroscope = (1 << 1),
                        Magnetometer = (1 << 2),
                        Barometer = (1 << 3),
                };

                unsigned int fields; 
                float accel[3]; 
                float gyro[3];  
                float mag[3];   
                float pressure; 
                IMUData() : fields(0) {}
        };

        struct FilterData {
                enum {
                        Quaternion = (1 << 0),
                        Bias = (1 << 1),
                        AngleUnertainty = (1 << 2),
                        BiasUncertainty = (1 << 3),
                };

                unsigned int fields; 
                float quaternion[4]; 
                uint16_t quaternionStatus; 
                float bias[3];       
                uint16_t biasStatus; 
                float angleUncertainty[3];       
                uint16_t angleUncertaintyStatus; 
                float biasUncertainty[3];       
                uint16_t biasUncertaintyStatus; 
                FilterData() : fields(0) {}
        };

        /* Exceptions */

        struct command_error : public std::runtime_error {
                command_error(const Packet &p, uint8_t code);

        private:
                std::string generateString(const Packet &p, uint8_t code);
        };

        struct io_error : public std::runtime_error {
                io_error(const std::string &desc) : std::runtime_error(desc) {}
        };

        struct timeout_error : public std::runtime_error {
                timeout_error(bool write, unsigned int to);

        private:
                std::string generateString(bool write, unsigned int to);
        };

        Imu(const std::string &device, bool verbose);

        virtual ~Imu();

        void connect();

        void runOnce();

        void disconnect();

        void selectBaudRate(unsigned int baud);

        void ping();

        void idle(bool needReply = true);

        void resume();

        void getDeviceInfo(Imu::Info &info);

        void getIMUDataBaseRate(uint16_t &baseRate);

        void getFilterDataBaseRate(uint16_t &baseRate);

        void getDiagnosticInfo(Imu::DiagnosticFields &fields);

        void setIMUDataRate(uint16_t decimation, const std::bitset<4> &sources);

        void setFilterDataRate(uint16_t decimation, const std::bitset<4> &sources);

        void enableMeasurements(bool accel, bool magnetometer);

        void enableBiasEstimation(bool enabled);

        void setHardIronOffset(float offset[3]);

        void setSoftIronMatrix(float matrix[9]);

        void enableIMUStream(bool enabled);

        void enableFilterStream(bool enabled);

        void setIMUDataCallback(const std::function<void(const Imu::IMUData &)> &);

        void
        setFilterDataCallback(const std::function<void(const Imu::FilterData &)> &);

        private:
        //  non-copyable
        Imu(const Imu &) = delete;
        Imu &operator=(const Imu &) = delete;

        int pollInput(unsigned int to);

        std::size_t handleByte(const uint8_t& byte, bool& found);

        int handleRead(size_t);

        void processPacket();

        int writePacket(const Packet &p, unsigned int to);

        void sendPacket(const Packet &p, unsigned int to);

        void receiveResponse(const Packet &command, unsigned int to);

        void sendCommand(const Packet &p, bool readReply = true);

        bool termiosBaudRate(unsigned int baud);

        const std::string device_;
        const bool verbose_;
        int fd_;
        unsigned int rwTimeout_;

        std::vector<uint8_t> buffer_;
        std::deque<uint8_t> queue_;
        size_t srcIndex_, dstIndex_;

        std::function<void(const Imu::IMUData &)>
        imuDataCallback_; 
        std::function<void(const Imu::FilterData &)>
        filterDataCallback_; 

        enum { Idle = 0, Reading, } state_;
        Packet packet_;
};

} //  imu_3dm_gx4

#endif // IMU_H_