yesense_imu.h

浏览该文件的文档.

#pragma once
 
#include "atomic"
#include "yesense_sdk/analysis_data.h"
#include "Encos_device.hpp"
#include <fstream>
#include <termios.h>
#include <array>
 
namespace bitbot
{
    class EncosBus;

    struct ImuRuntimeData
    {
        std::atomic<float> roll = 0;

        std::atomic<float> pitch = 0;

        std::atomic<float> yaw = 0;

        std::atomic<float> a_x = 0;

        std::atomic<float> a_y = 0;

        std::atomic<float> a_z = 0;

        std::atomic<float> w_x = 0;

        std::atomic<float> w_y = 0;

        std::atomic<float> w_z = 0;

        std::atomic<float> IMU_temp = 0;
    };

    struct ImuData
    {
        ImuRuntimeData runtime;
    };

    class YesenseIMU : public Encos_VirtualBusDevice
    {
        friend class EncosBus;
 
    public:
        YesenseIMU(const pugi::xml_node& imu_node);

        virtual ~YesenseIMU();

        float GetRoll();

        float GetPitch();

        float GetYaw();

        float GetAccX();

        float GetAccY();

        float GetAccZ();

        float GetGyroX();

        float GetGyroY();

        float GetGyroZ();

        float GetIMUTemperature();
 
    private:
        void UpdateRuntimeData() override;
        void UpdateImuDataFromBus();
        void ReadOnce(); // call this function in the bus loop
        void WriteOnce() {};
 
        std::array<float, 3> cvtEuler(const float* quat);
 
    private:
        ImuData imu_data_;
 
        constexpr static int RX_BUF_LEN = 1024;
        unsigned char g_recv_buf[RX_BUF_LEN * 2] = { 0 };
        unsigned short g_recv_buf_idx = 0;
        protocol_info_t g_output_info = { 0 };
 
        int fd;
        int nread;
        char buffer[RX_BUF_LEN];
        std::string dev;
        struct termios newtio;
        unsigned short cnt = 0;
        int pos = 0;
        speed_t speed = B921600;
 
        constexpr static float r2d = 180.0f / M_PI;
        constexpr static float d2r = M_PI / 180.0f;
    };
}