From 20f18acc27577aa0eeaba44f77823ed1b07199ec Mon Sep 17 00:00:00 2001 From: myles-parfeniuk Date: Sun, 24 Nov 2024 11:16:07 -0800 Subject: [PATCH] tare(), get_sample_count(), has_new_data() features --- include/BNO08x.hpp | 7 ++- include/BNO08xRpt.hpp | 3 ++ include/BNO08x_global_types.hpp | 20 ++++++++ include/report/BNO08xRptGameRV.hpp | 4 ++ include/report/BNO08xRptRV.hpp | 4 ++ include/report/BNO08xRptRVGeneric.hpp | 1 + include/report/BNO08xRptRVGeomag.hpp | 4 ++ source/BNO08x.cpp | 23 +-------- source/BNO08xRpt.cpp | 68 +++++++++++++++++++++++++-- source/report/BNO08xRptGameRV.cpp | 48 ++++++++++++++++++- source/report/BNO08xRptRV.cpp | 46 ++++++++++++++++++ source/report/BNO08xRptRVGeneric.cpp | 35 ++++++++++++++ source/report/BNO08xRptRVGeomag.cpp | 45 ++++++++++++++++++ 13 files changed, 279 insertions(+), 29 deletions(-) diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 7a1bae5..a8c1a40 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -70,7 +70,6 @@ class BNO08x void hard_reset(); bool data_available(); - bool data_available(uint8_t& report_ID); void register_cb(std::function cb_fxn); void register_cb(std::function cb_fxn); @@ -188,13 +187,15 @@ class BNO08x esp_err_t deinit_sh2_HAL(); esp_err_t wait_for_hint(); - + esp_err_t re_enable_reports(); sh2_Hal_t sh2_HAL; ///< sh2 hardware abstraction layer struct for use with sh2 HAL lib. EventGroupHandle_t evt_grp_bno08x_task; /// usr_reports = {{SH2_ACCELEROMETER, &accelerometer}, {SH2_LINEAR_ACCELERATION, &linear_accelerometer}, {SH2_GRAVITY, &gravity}, {SH2_MAGNETIC_FIELD_CALIBRATED, &cal_magnetometer}, {SH2_MAGNETIC_FIELD_UNCALIBRATED, &uncal_magnetometer}, {SH2_GYROSCOPE_CALIBRATED, &cal_gyro}, {SH2_GYROSCOPE_UNCALIBRATED, &uncal_gyro}, {SH2_ROTATION_VECTOR, &rv}, diff --git a/include/BNO08xRpt.hpp b/include/BNO08xRpt.hpp index 6a0b25f..cd1812c 100644 --- a/include/BNO08xRpt.hpp +++ b/include/BNO08xRpt.hpp @@ -27,7 +27,10 @@ class BNO08xRpt bool enable(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg = default_sensor_cfg); bool disable(sh2_SensorConfig_t sensor_cfg = default_sensor_cfg); void register_cb(std::function cb_fxn); + bool has_new_data(); bool flush(); + bool get_sample_counts(bno08x_sample_counts_t& sample_counts); + bool clear_sample_counts(); virtual void update_data(sh2_SensorValue_t* sensor_val) = 0; diff --git a/include/BNO08x_global_types.hpp b/include/BNO08x_global_types.hpp index ffeac65..c9c2893 100644 --- a/include/BNO08x_global_types.hpp +++ b/include/BNO08x_global_types.hpp @@ -482,6 +482,26 @@ typedef struct bno08x_shake_detector_t } bno08x_shake_detector_t; +/// @brief Struct to represent sample counts, returned from BNO08xRpt::get_sample_counts() +typedef struct bno08x_sample_counts_t +{ + uint32_t offered; ///< Number of samples produced by underlying data source. + uint32_t on; ///< Number of "offered" samples while this sensor was requested by host. + uint32_t accepted; ///< Number of "on" samples that passed decimation filter. + uint32_t attempted; ///< Number of "accepted" samples that passed threshold requirements and had transmission to the host attempted. + + // conversion from sh2_PersonalActivityClassifier_t + bno08x_sample_counts_t& operator=(const sh2_Counts_t& source) + { + this->offered = source.offered; + this->on = source.on; + this->accepted = source.accepted; + this->attempted = source.attempted; + + return *this; + } +} bno08x_sample_counts_t; + typedef sh2_Accelerometer_t bno08x_accel_t; ///< Acceleration data. typedef sh2_StepCounter bno08x_step_counter_t; typedef sh2_RawGyroscope_t bno08x_raw_gyro_t; diff --git a/include/report/BNO08xRptGameRV.hpp b/include/report/BNO08xRptGameRV.hpp index 2b7e327..bbaa571 100644 --- a/include/report/BNO08xRptGameRV.hpp +++ b/include/report/BNO08xRptGameRV.hpp @@ -13,6 +13,10 @@ class BNO08xRptGameRV : public BNO08xRptRVGeneric { } + bool tare(bool x = true, bool y = true, bool z = true); + bool tare_persist(); + void tare_clear(); + private: void update_data(sh2_SensorValue_t* sensor_val) override; static const constexpr char* TAG = "BNO08xRptGameRV"; diff --git a/include/report/BNO08xRptRV.hpp b/include/report/BNO08xRptRV.hpp index 29ce14b..1700483 100644 --- a/include/report/BNO08xRptRV.hpp +++ b/include/report/BNO08xRptRV.hpp @@ -13,6 +13,10 @@ class BNO08xRptRV : public BNO08xRptRVGeneric { } + bool tare(bool x = true, bool y = true, bool z = true); + bool tare_persist(); + void tare_clear(); + private: void update_data(sh2_SensorValue_t* sensor_val) override; static const constexpr char* TAG = "BNO08xRptRV"; diff --git a/include/report/BNO08xRptRVGeneric.hpp b/include/report/BNO08xRptRVGeneric.hpp index dfbd697..0ff89b7 100644 --- a/include/report/BNO08xRptRVGeneric.hpp +++ b/include/report/BNO08xRptRVGeneric.hpp @@ -16,6 +16,7 @@ class BNO08xRptRVGeneric : public BNO08xRpt : BNO08xRpt(imu, report_ID, period_us, rpt_bit) { } + bool tare(bool x, bool y, bool z, sh2_TareBasis_t basis); bno08x_quat_t data; static const constexpr char* TAG = "BNO08xRptRVGeneric"; }; diff --git a/include/report/BNO08xRptRVGeomag.hpp b/include/report/BNO08xRptRVGeomag.hpp index 9696787..5776057 100644 --- a/include/report/BNO08xRptRVGeomag.hpp +++ b/include/report/BNO08xRptRVGeomag.hpp @@ -13,6 +13,10 @@ class BNO08xRptRVGeomag : public BNO08xRptRVGeneric { } + bool tare(bool x = true, bool y = true, bool z = true); + bool tare_persist(); + void tare_clear(); + private: void update_data(sh2_SensorValue_t* sensor_val) override; static const constexpr char* TAG = "BNO08xRptRVGeomag"; diff --git a/source/BNO08x.cpp b/source/BNO08x.cpp index 296d4fe..fb35eb9 100644 --- a/source/BNO08x.cpp +++ b/source/BNO08x.cpp @@ -38,6 +38,7 @@ BNO08x::BNO08x(bno08x_config_t imu_config) , sem_kill_tasks(NULL) , evt_grp_bno08x_task(xEventGroupCreate()) , evt_grp_report_en(xEventGroupCreate()) + , evt_grp_report_data_available(xEventGroupCreate()) , queue_rx_sensor_event(xQueueCreate(5, sizeof(sh2_SensorEvent_t))) , queue_cb_report_id(xQueueCreate(5, sizeof(uint8_t))) , imu_config(imu_config) @@ -80,6 +81,7 @@ BNO08x::~BNO08x() // delete event groups vEventGroupDelete(evt_grp_bno08x_task); vEventGroupDelete(evt_grp_report_en); + vEventGroupDelete(evt_grp_report_data_available); // delete all queues vQueueDelete(queue_rx_sensor_event); @@ -1101,27 +1103,6 @@ bool BNO08x::data_available() return false; } -/** - * @brief Polls for new data/report to become available, overloaded with param for report identification. - * - * @param report_ID Reference to save most recent report ID. - * - * @return True if new data/report became available before DATA_AVAILABLE_TIMEOUT_MS. - */ -bool BNO08x::data_available(uint8_t& report_ID) -{ - - if (xEventGroupWaitBits(evt_grp_bno08x_task, EVT_GRP_BNO08x_TASK_DATA_AVAILABLE, pdTRUE, pdFALSE, DATA_AVAILABLE_TIMEOUT_MS) & - EVT_GRP_BNO08x_TASK_DATA_AVAILABLE) - { - report_ID = most_recent_rpt; - return true; - } - - report_ID = 0U; - return false; -} - /** * @brief Registers a callback to execute when new data from a report is received. * diff --git a/source/BNO08xRpt.cpp b/source/BNO08xRpt.cpp index a2c65c6..2599bda 100644 --- a/source/BNO08xRpt.cpp +++ b/source/BNO08xRpt.cpp @@ -15,8 +15,9 @@ bool BNO08xRpt::enable(uint32_t time_between_reports, sh2_SensorConfig_t sensor_ xEventGroupClearBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit - imu->lock_sh2_HAL(); sensor_cfg.reportInterval_us = time_between_reports; + + imu->lock_sh2_HAL(); sh2_res = sh2_setSensorConfig(ID, &sensor_cfg); imu->unlock_sh2_HAL(); @@ -68,6 +69,25 @@ void BNO08xRpt::register_cb(std::function cb_fxn) imu->cb_list.push_back({ID, cb_fxn}); } +/** + * @brief Checks if a new report has been received since the last time this function was called. + * + * + * @return True if a new report was received since the last time this function was called. + */ +bool BNO08xRpt::has_new_data() +{ + bool new_data = false; + + if (xEventGroupGetBits(imu->evt_grp_report_data_available) & rpt_bit) + { + new_data = true; + xEventGroupClearBits(imu->evt_grp_report_data_available, rpt_bit); + } + + return new_data; +} + /** * @brief Flush all buffered reports for this sensor/report module. * @@ -84,6 +104,49 @@ bool BNO08xRpt::flush() return (success != SH2_OK) ? false : true; } +/** + * @brief Gets sample counts for this sensor (see SH-2 ref manual 6.4.3.1) + * + * @param Struct to store requested data. + * + * @return True get counts operation succeeded. + */ +bool BNO08xRpt::get_sample_counts(bno08x_sample_counts_t& sample_counts) +{ + int success = SH2_OK; + sh2_Counts_t pCounts; + + imu->lock_sh2_HAL(); + success = sh2_getCounts(ID, &pCounts); + imu->unlock_sh2_HAL(); + + if (success != SH2_OK) + { + return false; + } + else + { + sample_counts = pCounts; + return true; + } +} + +/** + * @brief Clears BNO08x internal sample counts for this sensor. (see SH-2 ref manual 6.4.3.1) + * + * @return True clear counts operation succeeded. + */ +bool BNO08xRpt::clear_sample_counts() +{ + int success = SH2_OK; + + imu->lock_sh2_HAL(); + success = sh2_clearCounts(ID); + imu->unlock_sh2_HAL(); + + return (success != SH2_OK) ? false : true; +} + /** * @brief Signals to BNO08x::data_available() that a new report has arrived. * @@ -91,7 +154,6 @@ bool BNO08xRpt::flush() */ void BNO08xRpt::signal_data_available() { - xEventGroupClearBits(imu->evt_grp_bno08x_task, BNO08x::EVT_GRP_BNO08x_TASK_DATA_AVAILABLE); - imu->most_recent_rpt = ID; + xEventGroupSetBits(imu->evt_grp_report_data_available, rpt_bit); xEventGroupSetBits(imu->evt_grp_bno08x_task, BNO08x::EVT_GRP_BNO08x_TASK_DATA_AVAILABLE); } diff --git a/source/report/BNO08xRptGameRV.cpp b/source/report/BNO08xRptGameRV.cpp index f218624..a2d53b9 100644 --- a/source/report/BNO08xRptGameRV.cpp +++ b/source/report/BNO08xRptGameRV.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptRV.hpp" +#include "BNO08xRptGameRV.hpp" #include "BNO08x.hpp" /** @@ -16,4 +16,50 @@ void BNO08xRptGameRV::update_data(sh2_SensorValue_t* sensor_val) if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) signal_data_available(); +} + +/** + * @brief Tares game rotation vector axis. + * + * @param x If true tare x axis (optional, default true). + * @param y If true tare y axis (optional, default true). + * @param z If true tare z axis (optional, default true). + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptGameRV::tare(bool x, bool y, bool z) +{ + + return BNO08xRptRVGeneric::tare(x, y, z, SH2_TARE_BASIS_GAMING_ROTATION_VECTOR); +} + +/** + * @brief Saves most recent tare operation to BNO08x internal flash, such that it persists on reset. + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptGameRV::tare_persist() +{ + int success = SH2_ERR; + + imu->lock_sh2_HAL(); + success = sh2_persistTare(); + imu->unlock_sh2_HAL(); + + if (success != SH2_OK) + return false; + else + return true; +} + +/** + * @brief Clears most recent tare operation. + * + * @return void, nothing to return + */ +void BNO08xRptGameRV::tare_clear() +{ + imu->lock_sh2_HAL(); + sh2_clearTare(); + imu->unlock_sh2_HAL(); } \ No newline at end of file diff --git a/source/report/BNO08xRptRV.cpp b/source/report/BNO08xRptRV.cpp index a8a3493..911e673 100644 --- a/source/report/BNO08xRptRV.cpp +++ b/source/report/BNO08xRptRV.cpp @@ -16,4 +16,50 @@ void BNO08xRptRV::update_data(sh2_SensorValue_t* sensor_val) if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) signal_data_available(); +} + +/** + * @brief Tares rotation vector axis. + * + * @param x If true tare x axis (optional, default true). + * @param y If true tare y axis (optional, default true). + * @param z If true tare z axis (optional, default true). + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptRV::tare(bool x, bool y, bool z) +{ + + return BNO08xRptRVGeneric::tare(x, y, z, SH2_TARE_BASIS_ROTATION_VECTOR); +} + +/** + * @brief Saves most recent tare operation to BNO08x internal flash, such that it persists on reset. + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptRV::tare_persist() +{ + int success = SH2_ERR; + + imu->lock_sh2_HAL(); + success = sh2_persistTare(); + imu->unlock_sh2_HAL(); + + if (success != SH2_OK) + return false; + else + return true; +} + +/** + * @brief Clears most recent tare operation. + * + * @return void, nothing to return + */ +void BNO08xRptRV::tare_clear() +{ + imu->lock_sh2_HAL(); + sh2_clearTare(); + imu->unlock_sh2_HAL(); } \ No newline at end of file diff --git a/source/report/BNO08xRptRVGeneric.cpp b/source/report/BNO08xRptRVGeneric.cpp index feeb916..5ebdb5f 100644 --- a/source/report/BNO08xRptRVGeneric.cpp +++ b/source/report/BNO08xRptRVGeneric.cpp @@ -40,3 +40,38 @@ bno08x_euler_angle_t BNO08xRptRVGeneric::get_euler(bool in_degrees) return rqdata; } + +/** + * @brief Tares vector basis according to axis flags. + * + * @param x If true tare x axis. + * @param y If true tare y axis. + * @param z If true tare z axis. + * @param basis Rotation vector basis to undergo tare operation. + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptRVGeneric::tare(bool x, bool y, bool z, sh2_TareBasis_t basis) +{ + int success = SH2_ERR; + + uint8_t axis_flag = 0U; + + if (x) + axis_flag |= SH2_TARE_X; + + if (y) + axis_flag |= SH2_TARE_Y; + + if (z) + axis_flag |= SH2_TARE_Z; + + imu->lock_sh2_HAL(); + success = sh2_setTareNow(axis_flag, basis); + imu->unlock_sh2_HAL(); + + if (success != SH2_OK) + return false; + else + return true; +} diff --git a/source/report/BNO08xRptRVGeomag.cpp b/source/report/BNO08xRptRVGeomag.cpp index f781611..71f3bfa 100644 --- a/source/report/BNO08xRptRVGeomag.cpp +++ b/source/report/BNO08xRptRVGeomag.cpp @@ -16,4 +16,49 @@ void BNO08xRptRVGeomag::update_data(sh2_SensorValue_t* sensor_val) if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) signal_data_available(); +} + +/** + * @brief Tares geomagnetic rotation vector. + * + * @param x If true tare x axis (optional, default true). + * @param y If true tare y axis (optional, default true). + * @param z If true tare z axis (optional, default true). + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptRVGeomag::tare(bool x, bool y, bool z) +{ + return BNO08xRptRVGeneric::tare(x, y, z, SH2_TARE_BASIS_GEOMAGNETIC_ROTATION_VECTOR); +} + +/** + * @brief Saves most recent tare operation to BNO08x internal flash, such that it persists on reset. + * + * @return True if tare operation succeeded. + */ +bool BNO08xRptRVGeomag::tare_persist() +{ + int success = SH2_ERR; + + imu->lock_sh2_HAL(); + success = sh2_persistTare(); + imu->unlock_sh2_HAL(); + + if (success != SH2_OK) + return false; + else + return true; +} + +/** + * @brief Clears most recent tare operation. + * + * @return void, nothing to return + */ +void BNO08xRptRVGeomag::tare_clear() +{ + imu->lock_sh2_HAL(); + sh2_clearTare(); + imu->unlock_sh2_HAL(); } \ No newline at end of file