Cansat
/
esp32_BNO08x
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rotation vector reports

This commit is contained in:
myles-parfeniuk 2024-11-21 01:01:35 -08:00
parent 5ba792d8a8
commit 19beed4492
3 changed files with 351 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
include/BNO08x.hpp
View File

@ -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);

75
include/BNO08x_global_types.hpp
View File

@ -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_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;

275
source/BNO08x.cpp
View File

@ -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;