rotation vector reports

include/BNO08x.hpp

@@ -5,7 +5,6 @@
 #pragma once
 // standard library includes
 #include <inttypes.h>
-#include <math.h>
 #include <stdio.h>
 #include <cstring>
 #include <functional>
@@ -48,6 +47,9 @@ class BNO08x
         void hard_reset();
 
         bool enable_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
+        bool enable_game_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
+        bool enable_ARVR_stabilized_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
+        bool enable_ARVR_stabilized_game_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
         bool enable_gravity(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
         bool enable_linear_accelerometer(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
         bool enable_accelerometer(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
@@ -59,8 +61,15 @@ class BNO08x
         bool enable_raw_mems_accelerometer(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
         bool enable_raw_mems_magnetometer(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg);
 
-        bno08x_quat_w_acc_t get_rotation_vector_quat();
+        bno08x_quat_t get_rotation_vector_quat();
         bno08x_euler_angle_t get_rotation_vector_euler(bool in_degrees = true);
+        bno08x_quat_t get_game_rotation_vector_quat();
+        bno08x_euler_angle_t get_game_rotation_vector_euler(bool in_degrees = true);
+        bno08x_quat_t get_ARVR_stabilized_rotation_vector_quat();
+        bno08x_euler_angle_t get_ARVR_stabilized_rotation_vector_euler(bool in_degrees = true);
+        bno08x_quat_t get_ARVR_stabilized_game_rotation_vector_quat();
+        bno08x_euler_angle_t get_ARVR_stabilized_game_rotation_vector_euler(bool in_degrees = true);
+
         bno08x_accel_data_t get_gravity();
         bno08x_accel_data_t get_linear_accel();
         bno08x_accel_data_t get_accel();
@@ -71,8 +80,6 @@ class BNO08x
         bno08x_raw_magf_data_t get_raw_mems_magnetometer();
         bno08x_step_counter_data_t get_step_counter();
 
-        void quat_to_euler(bno08x_quat_w_acc_t* quat, bno08x_euler_angle_t* euler);
-
         void register_cb(std::function<void()> cb_fxn);
         void print_product_ids();
 
@@ -118,7 +125,10 @@ class BNO08x
                 bno08x_raw_gyro_data_t mems_raw_gyro;
                 bno08x_raw_accel_data_t mems_raw_accel;
                 bno08x_raw_magf_data_t mems_raw_magnetometer;
-                bno08x_quat_w_acc_t rotation_vector;
+                bno08x_quat_t rotation_vector;
+                bno08x_quat_t game_rotation_vector;
+                bno08x_quat_t arvr_s_rotation_vector;
+                bno08x_quat_t arvr_s_game_rotation_vector;
 
                 bno08x_data_t()
                     : gravity({0.0f, 0.0f, 0.0f})
@@ -126,12 +136,13 @@ class BNO08x
                     , acceleration({0.0f, 0.0f, 0.0f})
                     , cal_magnetometer({0.0f, 0.0f, 0.0f})
                     , step_counter({0UL, 0U})
-                    , activity_classifier({0U, false, BNO08xActivity::UNKNOWN, {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U}})
+                    , activity_classifier(bno08x_activity_classifier_data_t())
                     , cal_gyro({0.0f, 0.0f, 0.0f})
                     , mems_raw_gyro({0, 0, 0, 0, 0UL})
                     , mems_raw_accel({0, 0, 0, 0UL})
                     , mems_raw_magnetometer({0, 0, 0, 0UL})
-                    , rotation_vector({0.0f, 0.0f, 0.0f, 0.0f, 0.0f})
+                    , rotation_vector(bno08x_quat_t())
+                    , game_rotation_vector(bno08x_quat_t())
 
                 {
                 }
@@ -151,6 +162,9 @@ class BNO08x
                 uint32_t raw_mems_accelerometer;
                 uint32_t raw_mems_magnetometer;
                 uint32_t rotation_vector;
+                uint32_t game_rotation_vector;
+                uint32_t arvr_s_rotation_vector;
+                uint32_t arvr_s_game_rotation_vector;
 
                 bno08x_usr_report_periods_t()
                     : gravity(0UL)
@@ -164,6 +178,9 @@ class BNO08x
                     , raw_mems_accelerometer(0UL)
                     , raw_mems_magnetometer(0UL)
                     , rotation_vector(0UL)
+                    , game_rotation_vector(0UL)
+                    , arvr_s_rotation_vector(0UL)
+                    , arvr_s_game_rotation_vector(0UL)
                 {
                 }
         } bno08x_usr_report_periods_t;
@@ -192,6 +209,9 @@ class BNO08x
 
         void handle_sensor_report(sh2_SensorValue_t* sensor_val);
         void update_rotation_vector_data(sh2_SensorValue_t* sensor_val);
+        void update_game_rotation_vector_data(sh2_SensorValue_t* sensor_val);
+        void update_arvr_s_rotation_vector_data(sh2_SensorValue_t* sensor_val);
+        void update_arvr_s_game_rotation_vector_data(sh2_SensorValue_t* sensor_val);
         void update_gravity_data(sh2_SensorValue_t* sensor_val);
         void update_linear_accelerometer_data(sh2_SensorValue_t* sensor_val);
         void update_accelerometer_data(sh2_SensorValue_t* sensor_val);

include/BNO08x_global_types.hpp

@@ -4,6 +4,7 @@
  */
 #pragma once
 
+#include <math.h>
 #include <driver/gpio.h>
 #include <driver/spi_common.h>
 #include <driver/spi_master.h>
@@ -111,6 +112,14 @@ typedef struct bno08x_activity_classifier_data_t
         BNO08xActivity mostLikelyState;
         uint8_t confidence[10];
 
+        bno08x_activity_classifier_data_t()
+            : page(0U)
+            , lastPage(false)
+            , mostLikelyState(BNO08xActivity::UNDEFINED)
+            , confidence({})
+        {
+        }
+
         // conversion from sh2_PersonalActivityClassifier_t
         bno08x_activity_classifier_data_t& operator=(const sh2_PersonalActivityClassifier_t& source)
         {
@@ -119,14 +128,53 @@ typedef struct bno08x_activity_classifier_data_t
             this->mostLikelyState = static_cast<BNO08xActivity>(source.mostLikelyState);
 
             for (int i = 0; i < 10; ++i)
-            {
                 this->confidence[i] = source.confidence[i];
-            }
 
             return *this;
         }
 } bno08x_activity_classifier_data_t;
 
+typedef struct bno08x_quat_t
+{
+        float real;
+        float i;
+        float j;
+        float k;
+        float accuracy;
+
+        bno08x_quat_t()
+            : real(0.0f)
+            , i(0.0f)
+            , j(0.0f)
+            , k(0.0f)
+            , accuracy(0.0f)
+        {
+        }
+
+        // overloaded assignment operators to handle both sh2 structs:
+
+        bno08x_quat_t& operator=(const sh2_RotationVectorWAcc_t& source)
+        {
+            this->real = source.real;
+            this->i = source.i;
+            this->j = source.j;
+            this->k = source.k;
+            this->accuracy = source.accuracy;
+            return *this;
+        }
+
+        bno08x_quat_t& operator=(const sh2_RotationVector_t& source)
+        {
+            this->real = source.real;
+            this->i = source.i;
+            this->j = source.j;
+            this->k = source.k;
+            this->accuracy = 0.0f;
+            return *this;
+        }
+
+} bno08x_quat_t;
+
 typedef struct bno08x_euler_angle_t
 {
         float x;
@@ -134,6 +182,23 @@ typedef struct bno08x_euler_angle_t
         float z;
         float accuracy;
 
+        bno08x_euler_angle_t()
+            : x(0.0f)
+            , y(0.0f)
+            , z(0.0f)
+            , accuracy(0.0f)
+        {
+        }
+
+        bno08x_euler_angle_t& operator=(const bno08x_quat_t& source)
+        {
+            this->x = atan2(2.0f * (source.real * source.i + source.j * source.k), 1.0f - 2.0f * (source.i * source.i + source.j * source.j));
+            this->y = asin(2.0f * (source.real * source.j - source.k * source.i));
+            this->z = atan2(2.0f * (source.real * source.k + source.i * source.j), 1.0f - 2.0f * (source.j * source.j + source.k * source.k));
+            this->accuracy = source.accuracy;
+            return *this;
+        }
+
         // overloaded *= operator for rad2deg conversions
         template <typename T>
         bno08x_euler_angle_t& operator*=(T value)
@@ -144,11 +209,11 @@ typedef struct bno08x_euler_angle_t
             accuracy *= static_cast<float>(value);
             return *this;
         }
+
 } bno08x_euler_angle_t;
 
-typedef sh2_RotationVectorWAcc_t bno08x_quat_w_acc_t; ///< Quaternion data with accuracy.
+typedef sh2_Accelerometer_t bno08x_accel_data_t; ///< Acceleration data.
-typedef sh2_Accelerometer_t bno08x_accel_data_t;      ///< Acceleration data.
+typedef sh2_MagneticField_t bno08x_magf_data_t;  ///< Magnetic field data.
-typedef sh2_MagneticField_t bno08x_magf_data_t;       ///< Magnetic field data.
 typedef sh2_StepCounter bno08x_step_counter_data_t;
 typedef sh2_Gyroscope_t bno08x_gyro_data_t;
 typedef sh2_RawGyroscope_t bno08x_raw_gyro_data_t;

source/BNO08x.cpp

@@ -294,7 +294,25 @@ void BNO08x::handle_sensor_report(sh2_SensorValue_t* sensor_val)
         case SH2_ROTATION_VECTOR:
             update_rotation_vector_data(sensor_val);
             euler = get_rotation_vector_euler();
-            ESP_LOGW(TAG, "rot vec euler: %.3lf %.3lf %.3lf", euler.x, euler.y, euler.z);
+            ESP_LOGW(TAG, "rot vec euler: %.3lf %.3lf %.3lf %.3lf", euler.x, euler.y, euler.z, euler.accuracy);
+            break;
+
+        case SH2_GAME_ROTATION_VECTOR:
+            update_game_rotation_vector_data(sensor_val);
+            euler = get_game_rotation_vector_euler();
+            ESP_LOGW(TAG, "game rot vec euler: %.3lf %.3lf %.3lf", euler.x, euler.y, euler.z);
+            break;
+
+        case SH2_ARVR_STABILIZED_RV:
+            update_arvr_s_rotation_vector_data(sensor_val);
+            euler = get_ARVR_stabilized_rotation_vector_euler();
+            ESP_LOGW(TAG, "arvr s rot vec euler: %.3lf %.3lf %.3lf", euler.x, euler.y, euler.z);
+            break;
+
+        case SH2_ARVR_STABILIZED_GRV:
+            update_arvr_s_game_rotation_vector_data(sensor_val);
+            euler = get_ARVR_stabilized_game_rotation_vector_euler();
+            ESP_LOGW(TAG, "arvr s game rot vec euler: %.3lf %.3lf %.3lf", euler.x, euler.y, euler.z);
             break;
 
         default:
@@ -317,6 +335,48 @@ void BNO08x::update_rotation_vector_data(sh2_SensorValue_t* sensor_val)
     unlock_user_data();
 }
 
+/**
+ * @brief Updates game rotation vector data from decoded sensor event.
+ *
+ * @param sensor_val The sh2_SensorValue_t struct used in sh2_decodeSensorEvent() call.
+ *
+ * @return void, nothing to return
+ */
+void BNO08x::update_game_rotation_vector_data(sh2_SensorValue_t* sensor_val)
+{
+    lock_user_data();
+    data.game_rotation_vector = sensor_val->un.gameRotationVector;
+    unlock_user_data();
+}
+
+/**
+ * @brief Updates ARVR stabilized rotation vector data from decoded sensor event.
+ *
+ * @param sensor_val The sh2_SensorValue_t struct used in sh2_decodeSensorEvent() call.
+ *
+ * @return void, nothing to return
+ */
+void BNO08x::update_arvr_s_rotation_vector_data(sh2_SensorValue_t* sensor_val)
+{
+    lock_user_data();
+    data.arvr_s_rotation_vector = sensor_val->un.arvrStabilizedRV;
+    unlock_user_data();
+}
+
+/**
+ * @brief Updates ARVR stabilized game rotation vector data from decoded sensor event.
+ *
+ * @param sensor_val The sh2_SensorValue_t struct used in sh2_decodeSensorEvent() call.
+ *
+ * @return void, nothing to return
+ */
+void BNO08x::update_arvr_s_game_rotation_vector_data(sh2_SensorValue_t* sensor_val)
+{
+    lock_user_data();
+    data.arvr_s_game_rotation_vector = sensor_val->un.arvrStabilizedGRV;
+    unlock_user_data();
+}
+
 /**
  * @brief Updates gravity data from decoded sensor event.
  *
@@ -1109,7 +1169,7 @@ void BNO08x::hard_reset()
 }
 
 /**
- * @brief Sends command to rotation vector reports. (See Ref. Manual 6.5.18)
+ * @brief Sends command to enable rotation vector reports. (See Ref. Manual 6.5.18)
  *
  * @param report_period_us The period/interval of the report in microseconds.
  * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults.
@@ -1130,6 +1190,72 @@ bool BNO08x::enable_rotation_vector(uint32_t time_between_reports, sh2_SensorCon
     }
 }
 
+/**
+ * @brief Sends command to enable game rotation vector reports. (See Ref. Manual 6.5.19)
+ *
+ * @param report_period_us The period/interval of the report in microseconds.
+ * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults.
+ *
+ * @return ESP_OK if report was successfully enabled.
+ */
+bool BNO08x::enable_game_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg)
+{
+    if (enable_report(SH2_GAME_ROTATION_VECTOR, time_between_reports, sensor_cfg) != ESP_OK)
+    {
+        return false;
+    }
+    else
+    {
+        user_report_periods.game_rotation_vector = time_between_reports;
+        xEventGroupSetBits(evt_grp_report_en, EVT_GRP_RPT_GAME_ROTATION_VECTOR_BIT_EN);
+        return true;
+    }
+}
+
+/**
+ * @brief Sends command to enable ARVR stabilized rotation vector reports. (See Ref. Manual 6.5.19)
+ *
+ * @param report_period_us The period/interval of the report in microseconds.
+ * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults.
+ *
+ * @return ESP_OK if report was successfully enabled.
+ */
+bool BNO08x::enable_ARVR_stabilized_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg)
+{
+    if (enable_report(SH2_ARVR_STABILIZED_RV, time_between_reports, sensor_cfg) != ESP_OK)
+    {
+        return false;
+    }
+    else
+    {
+        user_report_periods.arvr_s_rotation_vector = time_between_reports;
+        xEventGroupSetBits(evt_grp_report_en, EVT_GRP_RPT_ARVR_S_ROTATION_VECTOR_BIT_EN);
+        return true;
+    }
+}
+
+/**
+ * @brief Sends command to enable ARVR stabilized rotation vector reports. (See Ref. Manual 6.5.19)
+ *
+ * @param report_period_us The period/interval of the report in microseconds.
+ * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults.
+ *
+ * @return ESP_OK if report was successfully enabled.
+ */
+bool BNO08x::enable_ARVR_stabilized_game_rotation_vector(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg)
+{
+    if (enable_report(SH2_ARVR_STABILIZED_GRV, time_between_reports, sensor_cfg) != ESP_OK)
+    {
+        return false;
+    }
+    else
+    {
+        user_report_periods.arvr_s_game_rotation_vector = time_between_reports;
+        xEventGroupSetBits(evt_grp_report_en, EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT_EN);
+        return true;
+    }
+}
+
 /**
  * @brief Sends command to enable gravity reports. (See Ref. Manual 6.5.11)
  *
@@ -1355,16 +1481,16 @@ bool BNO08x::enable_raw_mems_magnetometer(uint32_t time_between_reports, sh2_Sen
  *
  * @return Struct containing requested data.
  */
-bno08x_quat_w_acc_t BNO08x::get_rotation_vector_quat()
+bno08x_quat_t BNO08x::get_rotation_vector_quat()
 {
     lock_user_data();
-    bno08x_quat_w_acc_t rqdata = data.rotation_vector;
+    bno08x_quat_t rqdata = data.rotation_vector;
     unlock_user_data();
     return rqdata;
 }
 
 /**
- * @brief Grabs most recent rotation vector data in form of an euler angle, units are in degrees or rads depending on sole input param.
+ * @brief Grabs most recent rotation vector data in form of an euler angle, units are in degrees or rads.
  *
  * @param in_degrees If true returned euler angle is in degrees, if false in radians
  *
@@ -1373,11 +1499,118 @@ bno08x_quat_w_acc_t BNO08x::get_rotation_vector_quat()
 bno08x_euler_angle_t BNO08x::get_rotation_vector_euler(bool in_degrees)
 {
     bno08x_euler_angle_t rqdata;
-    bno08x_quat_w_acc_t quat = get_rotation_vector_quat();
+    bno08x_quat_t quat = get_rotation_vector_quat();
 
-    quat_to_euler(&quat, &rqdata);
+    rqdata = quat; // conversion handled by overloaded operator
-    rqdata.accuracy = quat.accuracy;
 
+    // convert to degrees if requested
+    if (in_degrees)
+        rqdata *= RAD_2_DEG;
+
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent game rotation vector data in form of unit quaternion. No accuracy data available with this report.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_quat_t BNO08x::get_game_rotation_vector_quat()
+{
+    lock_user_data();
+    bno08x_quat_t rqdata = data.game_rotation_vector;
+    unlock_user_data();
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent rotation vector data in form of an euler angle, units are in degrees or rads. No accuracy data available with this report.
+ *
+ *
+ * @param in_degrees If true returned euler angle is in degrees, if false in radians.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_euler_angle_t BNO08x::get_game_rotation_vector_euler(bool in_degrees)
+{
+    bno08x_euler_angle_t rqdata;
+    bno08x_quat_t quat = get_game_rotation_vector_quat();
+
+    rqdata = quat; // conversion handled by overloaded operator
+
+    // convert to degrees if requested
+    if (in_degrees)
+        rqdata *= RAD_2_DEG;
+
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent ARVR stabilized rotation vector data in form of unit quaternion. No accuracy data available with this report.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_quat_t BNO08x::get_ARVR_stabilized_rotation_vector_quat()
+{
+    lock_user_data();
+    bno08x_quat_t rqdata = data.arvr_s_rotation_vector;
+    unlock_user_data();
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent ARVR stabilized rotation vector data in form of an euler angle, units are in degrees or rads. No accuracy data available
+ * with this report.
+ *
+ *
+ * @param in_degrees If true returned euler angle is in degrees, if false in radians.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_euler_angle_t BNO08x::get_ARVR_stabilized_rotation_vector_euler(bool in_degrees)
+{
+    bno08x_euler_angle_t rqdata;
+    bno08x_quat_t quat = get_ARVR_stabilized_rotation_vector_quat();
+
+    rqdata = quat; // conversion handled by overloaded operator
+
+    // convert to degrees if requested
+    if (in_degrees)
+        rqdata *= RAD_2_DEG;
+
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent ARVR stabilized  game rotation vector data in form of unit quaternion. No accuracy data available with this report.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_quat_t BNO08x::get_ARVR_stabilized_game_rotation_vector_quat()
+{
+    lock_user_data();
+    bno08x_quat_t rqdata = data.arvr_s_game_rotation_vector;
+    unlock_user_data();
+    return rqdata;
+}
+
+/**
+ * @brief Grabs most recent ARVR stabilized game rotation vector data in form of an euler angle, units are in degrees or rads. No accuracy data
+ * available with this report.
+ *
+ *
+ * @param in_degrees If true returned euler angle is in degrees, if false in radians.
+ *
+ * @return Struct containing requested data.
+ */
+bno08x_euler_angle_t BNO08x::get_ARVR_stabilized_game_rotation_vector_euler(bool in_degrees)
+{
+    bno08x_euler_angle_t rqdata;
+    bno08x_quat_t quat = get_ARVR_stabilized_game_rotation_vector_quat();
+
+    rqdata = quat; // conversion handled by overloaded operator
+
+    // convert to degrees if requested
     if (in_degrees)
         rqdata *= RAD_2_DEG;
 
@@ -1508,18 +1741,6 @@ bno08x_step_counter_data_t BNO08x::get_step_counter()
     return rqdata;
 }
 
-void BNO08x::quat_to_euler(bno08x_quat_w_acc_t* quat, bno08x_euler_angle_t* euler)
-{
-    // roll
-    euler->x = atan2(2.0f * (quat->real * quat->i + quat->j * quat->k), 1.0f - 2.0f * (quat->i * quat->i + quat->j * quat->j));
-
-    // pitch
-    euler->y = asin(2.0f * (quat->real * quat->j - quat->k * quat->i));
-
-    // yaw
-    euler->z = atan2(2.0f * (quat->real * quat->k + quat->i * quat->j), 1.0f - 2.0f * (quat->j * quat->j + quat->k * quat->k));
-}
-
 /**
  * @brief Waits for HINT pin assertion or HOST_INT_TIMEOUT_DEFAULT_MS to elapse.
  *
@@ -1609,7 +1830,19 @@ esp_err_t BNO08x::re_enable_reports()
             return ESP_FAIL;
 
     if (report_en_bits & EVT_GRP_RPT_ROTATION_VECTOR_BIT_EN)
-        if (!enable_activity_classifier(user_report_periods.rotation_vector))
+        if (!enable_rotation_vector(user_report_periods.rotation_vector))
+            return ESP_FAIL;
+
+    if (report_en_bits & EVT_GRP_RPT_GAME_ROTATION_VECTOR_BIT_EN)
+        if (!enable_game_rotation_vector(user_report_periods.game_rotation_vector))
+            return ESP_FAIL;
+
+    if (report_en_bits & EVT_GRP_RPT_ARVR_S_ROTATION_VECTOR_BIT_EN)
+        if (!enable_ARVR_stabilized_rotation_vector(user_report_periods.arvr_s_rotation_vector))
+            return ESP_FAIL;
+
+    if (report_en_bits & EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT_EN)
+        if (!enable_ARVR_stabilized_game_rotation_vector(user_report_periods.arvr_s_game_rotation_vector))
             return ESP_FAIL;
 
     return ESP_OK;