diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 3af8165..4ccebeb 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -5,7 +5,6 @@ #pragma once // standard library includes #include -#include #include #include #include @@ -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 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); diff --git a/include/BNO08x_global_types.hpp b/include/BNO08x_global_types.hpp index bcb491f..ffca515 100644 --- a/include/BNO08x_global_types.hpp +++ b/include/BNO08x_global_types.hpp @@ -4,6 +4,7 @@ */ #pragma once +#include #include #include #include @@ -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(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 bno08x_euler_angle_t& operator*=(T value) @@ -144,11 +209,11 @@ typedef struct bno08x_euler_angle_t accuracy *= static_cast(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_MagneticField_t bno08x_magf_data_t; ///< Magnetic field data. +typedef sh2_Accelerometer_t bno08x_accel_data_t; ///< Acceleration 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; diff --git a/source/BNO08x.cpp b/source/BNO08x.cpp index 4b29523..1660fcf 100644 --- a/source/BNO08x.cpp +++ b/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.accuracy = quat.accuracy; + 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 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;