From eec5c656d9fa1000a05a29f1ddb7dc394d015273 Mon Sep 17 00:00:00 2001 From: myles-parfeniuk Date: Sat, 23 Nov 2024 20:08:15 -0800 Subject: [PATCH] signal_update_data() --- include/BNO08x.hpp | 3 +- include/BNO08xRpt.hpp | 3 + source/BNO08x.cpp | 52 ++++----------- source/BNO08xRpt.cpp | 63 ++++++++++++++----- .../report/BNO08xRptARVRStabilizedGameRV.cpp | 3 + source/report/BNO08xRptARVRStabilizedRV.cpp | 3 + source/report/BNO08xRptAcceleration.cpp | 5 +- source/report/BNO08xRptActivityClassifier.cpp | 3 + source/report/BNO08xRptCalGyro.cpp | 5 +- source/report/BNO08xRptCalMagnetometer.cpp | 5 +- source/report/BNO08xRptGameRV.cpp | 3 + source/report/BNO08xRptGravity.cpp | 5 +- source/report/BNO08xRptIGyroRV.cpp | 3 + source/report/BNO08xRptLinearAcceleration.cpp | 5 +- source/report/BNO08xRptRV.cpp | 3 + source/report/BNO08xRptRVGeomag.cpp | 3 + .../report/BNO08xRptRawMEMSAccelerometer.cpp | 5 +- source/report/BNO08xRptRawMEMSGyro.cpp | 5 +- .../report/BNO08xRptRawMEMSMagnetometer.cpp | 5 +- source/report/BNO08xRptStepCounter.cpp | 3 + source/report/BNO08xRptUncalGyro.cpp | 5 +- source/report/BNO08xRptUncalMagnetometer.cpp | 5 +- source/report/BNO08xShakeDetector.cpp | 3 + source/report/BNO08xStabilityClassifier.cpp | 3 + source/report/BNO08xTapDetector.cpp | 3 + 25 files changed, 137 insertions(+), 67 deletions(-) diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 5dd8f62..7a1bae5 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -188,8 +188,7 @@ class BNO08x esp_err_t deinit_sh2_HAL(); esp_err_t wait_for_hint(); - - esp_err_t enable_report(sh2_SensorId_t sensor_ID, uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg); + esp_err_t re_enable_reports(); sh2_Hal_t sh2_HAL; ///< sh2 hardware abstraction layer struct for use with sh2 HAL lib. diff --git a/include/BNO08xRpt.hpp b/include/BNO08xRpt.hpp index d14db10..6a0b25f 100644 --- a/include/BNO08xRpt.hpp +++ b/include/BNO08xRpt.hpp @@ -27,6 +27,7 @@ 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 flush(); virtual void update_data(sh2_SensorValue_t* sensor_val) = 0; @@ -57,6 +58,8 @@ class BNO08xRpt { } + void signal_data_available(); + static const constexpr float RAD_2_DEG = (180.0f / M_PI); ///< Constant for radian to degree conversions, sed in quaternion to euler function conversions. diff --git a/source/BNO08x.cpp b/source/BNO08x.cpp index 740256a..296d4fe 100644 --- a/source/BNO08x.cpp +++ b/source/BNO08x.cpp @@ -354,21 +354,19 @@ void BNO08x::handle_sensor_report(sh2_SensorValue_t* sensor_val) // update respective report with new data usr_reports.at(rpt_ID)->update_data(sensor_val); - // send report ids to cb_task for callback execution - if (cb_list.size() != 0) - if (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE) - { - // clang-format off - #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS - ESP_LOGE(TAG, "Callback queue full, callback execution for report missed."); - #endif - // clang-format on - } - - // signal data_available() - xEventGroupClearBits(evt_grp_bno08x_task, EVT_GRP_BNO08x_TASK_DATA_AVAILABLE); - most_recent_rpt = sensor_val->sensorId; - xEventGroupSetBits(evt_grp_bno08x_task, EVT_GRP_BNO08x_TASK_DATA_AVAILABLE); + // send report ids to cb_task for callback execution (only if this report is enabled) + if (usr_reports.at(rpt_ID)->rpt_bit & xEventGroupGetBits(evt_grp_report_en)) + { + if (cb_list.size() != 0) + if (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE) + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Callback queue full, callback execution for report missed."); + #endif + // clang-format on + } + } } /** @@ -1063,30 +1061,6 @@ esp_err_t BNO08x::wait_for_hint() return ESP_ERR_TIMEOUT; } -/** - * @brief Enables a sensor report such that the BNO08x begins sending it. - * - * @param sensor_ID The ID of the sensor for the respective report to be enabled. - * @param report_period_us The period/interval of the report in microseconds. - * @param sensor_cfg Sensor special configuration. - * - * @return ESP_OK if report was successfully enabled. - */ -esp_err_t BNO08x::enable_report(sh2_SensorId_t sensor_ID, uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg) -{ - int sh2_res = SH2_ERR; - - lock_sh2_HAL(); - sensor_cfg.reportInterval_us = time_between_reports; - sh2_res = sh2_setSensorConfig(sensor_ID, &sensor_cfg); - unlock_sh2_HAL(); - - if (sh2_res != SH2_OK) - return ESP_FAIL; - else - return ESP_OK; -} - /** * @brief Re-enables all reports enabled by user (called when BNO08x reset is detected by sh2 HAL lib). * diff --git a/source/BNO08xRpt.cpp b/source/BNO08xRpt.cpp index 5300414..a2c65c6 100644 --- a/source/BNO08xRpt.cpp +++ b/source/BNO08xRpt.cpp @@ -11,23 +11,26 @@ */ bool BNO08xRpt::enable(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg) { - EventBits_t evt_grp_report_en_bits = xEventGroupGetBits(imu->evt_grp_report_en); + int sh2_res = SH2_OK; - // already enabled - if (!(evt_grp_report_en_bits & rpt_bit)) + xEventGroupClearBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit + + imu->lock_sh2_HAL(); + sensor_cfg.reportInterval_us = time_between_reports; + sh2_res = sh2_setSensorConfig(ID, &sensor_cfg); + imu->unlock_sh2_HAL(); + + if (sh2_res != SH2_OK) { - if (imu->enable_report(ID, time_between_reports, sensor_cfg) != ESP_OK) - { - return false; // Return false if enable_report fails - } - else - { - period_us = time_between_reports; // Update the period - xEventGroupSetBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit - } + return false; + } + else + { + flush(); + period_us = time_between_reports; // Update the period + xEventGroupSetBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit + return true; } - - return true; } /** @@ -44,7 +47,7 @@ bool BNO08xRpt::disable(sh2_SensorConfig_t sensor_cfg) if (evt_grp_report_en_bits & rpt_bit) { - if (imu->enable_report(ID, 0UL, sensor_cfg) != ESP_OK) + if (!enable(0UL, sensor_cfg)) return false; else xEventGroupClearBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit @@ -63,4 +66,32 @@ bool BNO08xRpt::disable(sh2_SensorConfig_t sensor_cfg) void BNO08xRpt::register_cb(std::function cb_fxn) { imu->cb_list.push_back({ID, cb_fxn}); -} \ No newline at end of file +} + +/** + * @brief Flush all buffered reports for this sensor/report module. + * + * @return True if flush operation succeeded. + */ +bool BNO08xRpt::flush() +{ + int success = SH2_OK; + + imu->lock_sh2_HAL(); + success = sh2_flush(ID); + imu->unlock_sh2_HAL(); + + return (success != SH2_OK) ? false : true; +} + +/** + * @brief Signals to BNO08x::data_available() that a new report has arrived. + * + * @return void, nothing to return + */ +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_bno08x_task, BNO08x::EVT_GRP_BNO08x_TASK_DATA_AVAILABLE); +} diff --git a/source/report/BNO08xRptARVRStabilizedGameRV.cpp b/source/report/BNO08xRptARVRStabilizedGameRV.cpp index 8623ad1..4ed2f07 100644 --- a/source/report/BNO08xRptARVRStabilizedGameRV.cpp +++ b/source/report/BNO08xRptARVRStabilizedGameRV.cpp @@ -13,4 +13,7 @@ void BNO08xRptARVRStabilizedGameRV::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.arvrStabilizedGRV; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } \ No newline at end of file diff --git a/source/report/BNO08xRptARVRStabilizedRV.cpp b/source/report/BNO08xRptARVRStabilizedRV.cpp index d38025d..757510e 100644 --- a/source/report/BNO08xRptARVRStabilizedRV.cpp +++ b/source/report/BNO08xRptARVRStabilizedRV.cpp @@ -13,4 +13,7 @@ void BNO08xRptARVRStabilizedRV::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.arvrStabilizedRV; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } \ No newline at end of file diff --git a/source/report/BNO08xRptAcceleration.cpp b/source/report/BNO08xRptAcceleration.cpp index e61cbec..c6b7b0e 100644 --- a/source/report/BNO08xRptAcceleration.cpp +++ b/source/report/BNO08xRptAcceleration.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptAcceleration.hpp" +#include "BNO08xRptAcceleration.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptAcceleration::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.accelerometer; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptActivityClassifier.cpp b/source/report/BNO08xRptActivityClassifier.cpp index 7e433d1..e8eb6ae 100644 --- a/source/report/BNO08xRptActivityClassifier.cpp +++ b/source/report/BNO08xRptActivityClassifier.cpp @@ -29,6 +29,9 @@ void BNO08xRptActivityClassifier::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.personalActivityClassifier; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptCalGyro.cpp b/source/report/BNO08xRptCalGyro.cpp index 17c20e1..fb5054d 100644 --- a/source/report/BNO08xRptCalGyro.cpp +++ b/source/report/BNO08xRptCalGyro.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptCalGyro.hpp" +#include "BNO08xRptCalGyro.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptCalGyro::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.gyroscope; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptCalMagnetometer.cpp b/source/report/BNO08xRptCalMagnetometer.cpp index d1c5f66..b800312 100644 --- a/source/report/BNO08xRptCalMagnetometer.cpp +++ b/source/report/BNO08xRptCalMagnetometer.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptCalMagnetometer.hpp" +#include "BNO08xRptCalMagnetometer.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptCalMagnetometer::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.magneticField; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptGameRV.cpp b/source/report/BNO08xRptGameRV.cpp index 4e962cc..f218624 100644 --- a/source/report/BNO08xRptGameRV.cpp +++ b/source/report/BNO08xRptGameRV.cpp @@ -13,4 +13,7 @@ void BNO08xRptGameRV::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.gameRotationVector; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } \ No newline at end of file diff --git a/source/report/BNO08xRptGravity.cpp b/source/report/BNO08xRptGravity.cpp index 277a893..2325af1 100644 --- a/source/report/BNO08xRptGravity.cpp +++ b/source/report/BNO08xRptGravity.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptGravity.hpp" +#include "BNO08xRptGravity.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptGravity::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.gravity; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptIGyroRV.cpp b/source/report/BNO08xRptIGyroRV.cpp index 658835f..dc4f186 100644 --- a/source/report/BNO08xRptIGyroRV.cpp +++ b/source/report/BNO08xRptIGyroRV.cpp @@ -14,6 +14,9 @@ void BNO08xRptIGyroRV::update_data(sh2_SensorValue_t* sensor_val) data = sensor_val->un.gyroIntegratedRV; data_vel = sensor_val->un.gyroIntegratedRV; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptLinearAcceleration.cpp b/source/report/BNO08xRptLinearAcceleration.cpp index af8bf4f..34ab20d 100644 --- a/source/report/BNO08xRptLinearAcceleration.cpp +++ b/source/report/BNO08xRptLinearAcceleration.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptLinearAcceleration.hpp" +#include "BNO08xRptLinearAcceleration.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptLinearAcceleration::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.linearAcceleration; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptRV.cpp b/source/report/BNO08xRptRV.cpp index 147420b..a8a3493 100644 --- a/source/report/BNO08xRptRV.cpp +++ b/source/report/BNO08xRptRV.cpp @@ -13,4 +13,7 @@ void BNO08xRptRV::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.rotationVector; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } \ No newline at end of file diff --git a/source/report/BNO08xRptRVGeomag.cpp b/source/report/BNO08xRptRVGeomag.cpp index 8e7f108..f781611 100644 --- a/source/report/BNO08xRptRVGeomag.cpp +++ b/source/report/BNO08xRptRVGeomag.cpp @@ -13,4 +13,7 @@ void BNO08xRptRVGeomag::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.geoMagRotationVector; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } \ No newline at end of file diff --git a/source/report/BNO08xRptRawMEMSAccelerometer.cpp b/source/report/BNO08xRptRawMEMSAccelerometer.cpp index 5ea0b47..3e244b1 100644 --- a/source/report/BNO08xRptRawMEMSAccelerometer.cpp +++ b/source/report/BNO08xRptRawMEMSAccelerometer.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptRawMEMSAccelerometer.hpp" +#include "BNO08xRptRawMEMSAccelerometer.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptRawMEMSAccelerometer::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.rawAccelerometer; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptRawMEMSGyro.cpp b/source/report/BNO08xRptRawMEMSGyro.cpp index 21b1625..249bf70 100644 --- a/source/report/BNO08xRptRawMEMSGyro.cpp +++ b/source/report/BNO08xRptRawMEMSGyro.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptRawMEMSGyro.hpp" +#include "BNO08xRptRawMEMSGyro.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptRawMEMSGyro::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.rawGyroscope; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptRawMEMSMagnetometer.cpp b/source/report/BNO08xRptRawMEMSMagnetometer.cpp index 1e14b11..d976aaf 100644 --- a/source/report/BNO08xRptRawMEMSMagnetometer.cpp +++ b/source/report/BNO08xRptRawMEMSMagnetometer.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptRawMEMSMagnetometer.hpp" +#include "BNO08xRptRawMEMSMagnetometer.hpp" #include "BNO08x.hpp" /** @@ -13,6 +13,9 @@ void BNO08xRptRawMEMSMagnetometer::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.rawMagnetometer; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptStepCounter.cpp b/source/report/BNO08xRptStepCounter.cpp index 95fb091..c8e94e1 100644 --- a/source/report/BNO08xRptStepCounter.cpp +++ b/source/report/BNO08xRptStepCounter.cpp @@ -24,6 +24,9 @@ void BNO08xRptStepCounter::update_data(sh2_SensorValue_t* sensor_val) prev_steps = data.steps; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptUncalGyro.cpp b/source/report/BNO08xRptUncalGyro.cpp index df58712..dfb997c 100644 --- a/source/report/BNO08xRptUncalGyro.cpp +++ b/source/report/BNO08xRptUncalGyro.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptUncalGyro.hpp" +#include "BNO08xRptUncalGyro.hpp" #include "BNO08x.hpp" /** @@ -14,6 +14,9 @@ void BNO08xRptUncalGyro::update_data(sh2_SensorValue_t* sensor_val) data = sensor_val->un.gyroscopeUncal; bias_data = sensor_val->un.gyroscopeUncal; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xRptUncalMagnetometer.cpp b/source/report/BNO08xRptUncalMagnetometer.cpp index b41a0e4..2435c63 100644 --- a/source/report/BNO08xRptUncalMagnetometer.cpp +++ b/source/report/BNO08xRptUncalMagnetometer.cpp @@ -1,4 +1,4 @@ -#include "BNO08xRptUncalMagnetometer.hpp" +#include "BNO08xRptUncalMagnetometer.hpp" #include "BNO08x.hpp" /** @@ -14,6 +14,9 @@ void BNO08xRptUncalMagnetometer::update_data(sh2_SensorValue_t* sensor_val) data = sensor_val->un.magneticFieldUncal; bias_data = sensor_val->un.magneticFieldUncal; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xShakeDetector.cpp b/source/report/BNO08xShakeDetector.cpp index f8d3524..69b9fe1 100644 --- a/source/report/BNO08xShakeDetector.cpp +++ b/source/report/BNO08xShakeDetector.cpp @@ -13,6 +13,9 @@ void BNO08xShakeDetector::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.shakeDetector; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xStabilityClassifier.cpp b/source/report/BNO08xStabilityClassifier.cpp index a3650f0..7adb8b8 100644 --- a/source/report/BNO08xStabilityClassifier.cpp +++ b/source/report/BNO08xStabilityClassifier.cpp @@ -13,6 +13,9 @@ void BNO08xStabilityClassifier::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.stabilityClassifier; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /** diff --git a/source/report/BNO08xTapDetector.cpp b/source/report/BNO08xTapDetector.cpp index 7b40580..9bf131e 100644 --- a/source/report/BNO08xTapDetector.cpp +++ b/source/report/BNO08xTapDetector.cpp @@ -29,6 +29,9 @@ void BNO08xTapDetector::update_data(sh2_SensorValue_t* sensor_val) imu->lock_user_data(); data = sensor_val->un.tapDetector; imu->unlock_user_data(); + + if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) + signal_data_available(); } /**