BNO055.hpp

[詳解]

//
//    BNO055 9軸センサー
//
//    Copyright (c) 2022 udonrobo
//
 
#pragma once
 
#if defined(ARDUINO) && !defined(UDON_TEENSY_I2C_SLAVE_MODE)
 
#    include <Udon/Thirdparty/Adafruit_BNO055/Adafruit_BNO055.h>
#    include <Udon/Types/Euler.hpp>
#    include <Udon/Types/Quaternion.hpp>
#    include <Udon/Types/Vector3D.hpp>
 
namespace Udon
{

    class BNO055
        : Adafruit_BNO055
    {
        QuaternionDirection direction;

        Quaternion offset;

        Quaternion quaternion;

        Vec3 gyro;
 
    public:
        BNO055(TwoWire& wire, const QuaternionDirection& direction = { true, true, true })
            : Adafruit_BNO055(-1, 0x28, &wire)
            , direction(direction)
            , offset(Quaternion::Identity())
            , quaternion(Quaternion::Identity())
            , gyro()
        {
        }

        struct Parameter
        {
            adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF;

            adafruit_bno055_axis_remap_config_t remapConfig = REMAP_CONFIG_P1;

            adafruit_bno055_axis_remap_sign_t remapSign = REMAP_SIGN_P1;

            bool useExternalCrystal = false;
        };

        bool begin()
        {
            return begin(Parameter());
        }

        bool begin(const Parameter& parameter)
        {
            if (!Adafruit_BNO055::begin(parameter.mode))
            {
                return false;
            }
 
            setAxisRemap(parameter.remapConfig);
            setAxisSign(parameter.remapSign);
            setExtCrystalUse(parameter.useExternalCrystal);
 
            return true;
        }

        void setMode(adafruit_bno055_opmode_t mode)
        {
            Adafruit_BNO055::setMode(mode);
        }

        adafruit_bno055_opmode_t getMode()
        {
            return Adafruit_BNO055::getMode();
        }

        void clear()
        {
            offset = quaternion;
        }

        void update()
        {
            const auto q = Adafruit_BNO055::getQuat();
            quaternion   = { q.x(), q.y(), q.z(), q.w() };
 
            const auto g = Adafruit_BNO055::getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
            gyro = { g.x(), g.y(), g.z() };
        }

        Quaternion getQuaternion() const
        {
            return (offset.inverse() * quaternion).directionRevision(direction);    // オフセットを引き、回転方向を修正
        }

        Euler getEuler() const
        {
            return getQuaternion().toEuler();
        }

        double getYaw() const
        {
            return getQuaternion().toYaw();
        }

        Vec3 getGyro() const
        {
            return gyro;
        }

        void show() const
        {
            Show(getEuler());
        }
    };
 
}    // namespace Udon
 
#endif