diff --git a/src/Udon/Driver/BNO055.hpp b/src/Udon/Driver/BNO055.hpp index 7262dc9f..979a0a60 100644 --- a/src/Udon/Driver/BNO055.hpp +++ b/src/Udon/Driver/BNO055.hpp @@ -11,6 +11,7 @@ # include # include # include +# include namespace Udon { @@ -19,14 +20,18 @@ namespace Udon class BNO055 : Adafruit_BNO055 { - /// @brief 回転方向 + /// @brief 回転方向修正用 QuaternionDirection direction; - /// @brief 内積値消去用オフセット + /// @brief 姿勢オフセット Quaternion offset; + /// @brief 最新の姿勢クォータニオン Quaternion quaternion; + /// @brief 最新の生ジャイロ値 [deg/s] + Vec3 gyro; + public: /// @brief コンストラクタ /// @param wire I2C ポート @@ -36,35 +41,36 @@ namespace Udon , direction(direction) , offset(Quaternion::Identity()) , quaternion(Quaternion::Identity()) + , gyro() { } /// @brief 初期化パラメータ struct Parameter { - /// @brief 動作モード(デフォルト: NDOF) + /// @brief 動作モード adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF; - /// @brief 軸の割り当て設定(デフォルト: P1 = 標準) + /// @brief 軸リマップ設定 adafruit_bno055_axis_remap_config_t remapConfig = REMAP_CONFIG_P1; - /// @brief 軸の符号設定(デフォルト: P1 = 標準) + /// @brief 軸符号設定 adafruit_bno055_axis_remap_sign_t remapSign = REMAP_SIGN_P1; - /// @brief 外部クリスタルを使用するか(デフォルト: false) + /// @brief 外部水晶振動子の使用有無 bool useExternalCrystal = false; }; - /// @brief 通信開始 (デフォルト設定) - /// @return 正常に開始できたかどうか + /// @brief 初期化 + /// @return 初期化成功時 true bool begin() { return begin(Parameter()); } - /// @brief 通信開始 (設定指定) + /// @brief 初期化 /// @param parameter 初期化パラメータ - /// @return 正常に開始できたかどうか + /// @return 初期化成功時 true bool begin(const Parameter& parameter) { if (!Adafruit_BNO055::begin(parameter.mode)) @@ -79,41 +85,66 @@ namespace Udon return true; } - /// @brief 値を消去する + /// @brief 動作モードを切り替える + /// @param mode 設定するモード + /// @note 内部で一旦 CONFIG モードに入り、25ms 程度のウェイトが発生する + void setMode(adafruit_bno055_opmode_t mode) + { + Adafruit_BNO055::setMode(mode); + } + + /// @brief 現在の動作モードを取得 + /// @return 現在の動作モード + adafruit_bno055_opmode_t getMode() + { + return Adafruit_BNO055::getMode(); + } + + /// @brief 現在の姿勢を基準姿勢としてオフセットを設定 void clear() { offset = quaternion; } - /// @brief 更新 + /// @brief センサー値を更新 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() }; } - /// @brief クォータニオン角を取得 - /// @return クォータニオン角 + /// @brief 姿勢クォータニオンを取得 + /// @return オフセット適用済みクォータニオン Quaternion getQuaternion() const { return (offset.inverse() * quaternion).directionRevision(direction); // オフセットを引き、回転方向を修正 } - /// @brief オイラー角を取得 + /// @brief 姿勢オイラー角を取得 /// @return オイラー角 Euler getEuler() const { return getQuaternion().toEuler(); } - /// @brief ヨー角(旋回角)を取得 + /// @brief ヨー角を取得 /// @return ヨー角 double getYaw() const { return getQuaternion().toYaw(); } - /// @brief オイラー角をシリアルポートに出力 + /// @brief 生ジャイロ値を取得 [deg/s] + /// @return 各軸角速度 + Vec3 getGyro() const + { + return gyro; + } + + /// @brief 姿勢オイラー角をシリアル出力 void show() const { Show(getEuler());