From 2d20a89b072a664bf609edc6fc162e96e5bdea0a Mon Sep 17 00:00:00 2001 From: myles-parfeniuk Date: Tue, 26 Nov 2024 13:28:27 -0800 Subject: [PATCH] input report status field as accuracy --- include/BNO08x.hpp | 7 + include/BNO08x_global_types.hpp | 245 ++++++++++++++++-- include/report/BNO08xStabilityClassifier.hpp | 5 +- source/BNO08x.cpp | 182 +++++++++---- source/BNO08xRpt.cpp | 1 + .../report/BNO08xRptARVRStabilizedGameRV.cpp | 1 + source/report/BNO08xRptARVRStabilizedRV.cpp | 1 + source/report/BNO08xRptAcceleration.cpp | 1 + source/report/BNO08xRptActivityClassifier.cpp | 1 + source/report/BNO08xRptCalGyro.cpp | 1 + source/report/BNO08xRptCalMagnetometer.cpp | 1 + source/report/BNO08xRptGameRV.cpp | 1 + source/report/BNO08xRptGravity.cpp | 1 + source/report/BNO08xRptIGyroRV.cpp | 1 + source/report/BNO08xRptLinearAcceleration.cpp | 1 + source/report/BNO08xRptRV.cpp | 1 + source/report/BNO08xRptRVGeneric.cpp | 4 +- source/report/BNO08xRptRVGeomag.cpp | 1 + .../report/BNO08xRptRawMEMSAccelerometer.cpp | 3 +- source/report/BNO08xRptRawMEMSGyro.cpp | 1 + .../report/BNO08xRptRawMEMSMagnetometer.cpp | 1 + source/report/BNO08xRptStepCounter.cpp | 1 + source/report/BNO08xRptUncalGyro.cpp | 1 + source/report/BNO08xRptUncalMagnetometer.cpp | 1 + source/report/BNO08xShakeDetector.cpp | 1 + source/report/BNO08xStabilityClassifier.cpp | 20 +- source/report/BNO08xTapDetector.cpp | 1 + 27 files changed, 404 insertions(+), 82 deletions(-) diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 6662e48..e9b3d04 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -69,6 +69,12 @@ class BNO08x bool initialize(); bool hard_reset(); bool soft_reset(); + BNO08xResetReason get_reset_reason(); + + bool on(); + bool sleep(); + + bool get_frs(BNO08xFRSID frs_ID, uint32_t* data, uint16_t* rx_data_sz); bool data_available(); void register_cb(std::function cb_fxn); @@ -79,6 +85,7 @@ class BNO08x // enum helper fxns static const char* activity_to_str(BNO08xActivity activity); static const char* stability_to_str(BNO08xStability stability); + static const char* accuracy_to_str(BNO08xAccuracy accuracy); BNO08xRptAcceleration accelerometer; BNO08xRptLinearAcceleration linear_accelerometer; diff --git a/include/BNO08x_global_types.hpp b/include/BNO08x_global_types.hpp index 54640ed..9245625 100644 --- a/include/BNO08x_global_types.hpp +++ b/include/BNO08x_global_types.hpp @@ -35,6 +35,42 @@ enum class BNO08xResetReason OTHER ///< Previous reset was due to power other reason. }; +/// @brief Sensor accuracy returned from input reports, corresponds to status bits (see ref. manual 6.5.1) +enum class BNO08xAccuracy +{ + UNRELIABLE, + LOW, + MED, + HIGH, + UNDEFINED +}; +using IMUAccuracy = BNO08xAccuracy; // legacy version compatibility + +enum class BNO08xFRSID +{ + RawAccelerometer = 0xE301, + Accelerometer = 0xE302, + LinearAcceleration = 0xE303, + Gravity = 0xE304, + RawGyroscope = 0xE305, + CalGyro = 0xE306, + UncalGyro = 0xE307, + RawMagnetometer = 0xE308, + CalMagnetometer = 0xE309, + UncalMagnetometer = 0xE30A, + RV = 0xE30B, + RVGame = 0xE30C, + RVGeomagnetic = 0xE30D, + TapDetector = 0xE313, + StepCounter = 0xE315, + StabilityClassifier = 0xE317, + ShakeDetector = 0xE318, + ActivityClassifier = 0xE31C, + RVARVRStabilizedRotation = 0xE322, + RVARVRStabilizedGame = 0xE323, + RVGyroIntegrated = 0xE324, +}; + /// @brief BNO08xActivity Classifier enable bits passed to enable_activity_classifier() enum class BNO08xActivityEnable { @@ -134,49 +170,49 @@ typedef struct bno08x_quat_t float i; float j; float k; - float accuracy; + BNO08xAccuracy accuracy; + float rad_accuracy; bno08x_quat_t() : real(0.0f) , i(0.0f) , j(0.0f) , k(0.0f) - , accuracy(0.0f) + , accuracy(BNO08xAccuracy::UNDEFINED) + , rad_accuracy(0.0f) { } - // overloaded assignment operator to handle RV with accuracy + // overloaded assignment operator to handle RV with rad accuracy 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; + this->rad_accuracy = source.accuracy; return *this; } - // overloaded assignment operator to handle RV with w/o accuracy + // overloaded assignment operator to handle RV with w/o rad accuracy 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; + this->rad_accuracy = 0.0f; return *this; } // overloaded assignment operator to handle IRV report - - // overloaded assignment operator to handle RV with w/o accuracy bno08x_quat_t& operator=(const sh2_GyroIntegratedRV_t& source) { this->real = source.real; this->i = source.i; this->j = source.j; this->k = source.k; - this->accuracy = 0.0f; + this->rad_accuracy = 0.0f; return *this; } @@ -188,13 +224,15 @@ typedef struct bno08x_euler_angle_t float x; float y; float z; - float accuracy; + BNO08xAccuracy accuracy; + float rad_accuracy; bno08x_euler_angle_t() : x(0.0f) , y(0.0f) , z(0.0f) - , accuracy(0.0f) + , accuracy(BNO08xAccuracy::UNDEFINED) + , rad_accuracy(0.0f) { } @@ -204,6 +242,7 @@ typedef struct bno08x_euler_angle_t 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->rad_accuracy = source.rad_accuracy; this->accuracy = source.accuracy; return *this; } @@ -215,7 +254,7 @@ typedef struct bno08x_euler_angle_t x *= static_cast(value); y *= static_cast(value); z *= static_cast(value); - accuracy *= static_cast(value); + rad_accuracy *= static_cast(value); return *this; } @@ -261,11 +300,13 @@ typedef struct bno08x_magf_t float x; float y; float z; + BNO08xAccuracy accuracy; bno08x_magf_t() : x(0.0f) , y(0.0f) , z(0.0f) + , accuracy(BNO08xAccuracy::UNDEFINED) { } @@ -320,11 +361,13 @@ typedef struct bno08x_gyro_t float x; float y; float z; + BNO08xAccuracy accuracy; bno08x_gyro_t() : x(0.0f) , y(0.0f) , z(0.0f) + , accuracy(BNO08xAccuracy::UNDEFINED) { } @@ -380,12 +423,14 @@ typedef struct bno08x_activity_classifier_t bool lastPage; BNO08xActivity mostLikelyState; uint8_t confidence[10]; + BNO08xAccuracy accuracy; bno08x_activity_classifier_t() : page(0U) , lastPage(false) , mostLikelyState(BNO08xActivity::UNDEFINED) , confidence({}) + , accuracy(BNO08xAccuracy::UNDEFINED) { } @@ -410,12 +455,14 @@ typedef struct bno08x_tap_detector_t int8_t y_flag; int8_t z_flag; bool double_tap; + BNO08xAccuracy accuracy; bno08x_tap_detector_t() : x_flag(0) , y_flag(0) , z_flag(0) , double_tap(false) + , accuracy(BNO08xAccuracy::UNDEFINED) { } @@ -462,11 +509,13 @@ typedef struct bno08x_shake_detector_t uint8_t x_flag; uint8_t y_flag; uint8_t z_flag; + BNO08xAccuracy accuracy; bno08x_shake_detector_t() : x_flag(0U) , y_flag(0U) , z_flag(0U) + , accuracy(BNO08xAccuracy::UNDEFINED) { } @@ -501,6 +550,14 @@ typedef struct bno08x_sample_counts_t 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. + bno08x_sample_counts_t() + : offered(0UL) + , on(0UL) + , accepted(0UL) + , attempted(0UL) + { + } + // conversion from sh2_PersonalActivityClassifier_t bno08x_sample_counts_t& operator=(const sh2_Counts_t& source) { @@ -513,10 +570,164 @@ typedef struct bno08x_sample_counts_t } } 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; -typedef sh2_RawAccelerometer bno08x_raw_accel_t; -typedef sh2_RawMagnetometer_t bno08x_raw_magf_t; +/// @brief Struct to represent acceleration data from acceleration, linear acceleration, and gravity reports. +typedef struct bno08x_accel_t +{ + float x; + float y; + float z; + BNO08xAccuracy accuracy; + + bno08x_accel_t() + : x(0.0f) + , y(0.0f) + , z(0.0f) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_Accelerometer_t + bno08x_accel_t& operator=(const sh2_Accelerometer_t& source) + { + this->x = source.x; + this->y = source.y; + this->z = source.z; + return *this; + } +} bno08x_accel_t; + +/// @brief Struct to represent step counter data from step counter reports. +typedef struct bno08x_step_counter_t +{ + uint32_t latency; + uint16_t steps; + BNO08xAccuracy accuracy; + + bno08x_step_counter_t() + : latency(0UL) + , steps(0U) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_StepCounter_t + bno08x_step_counter_t& operator=(const sh2_StepCounter_t& source) + { + this->latency = source.latency; + this->steps = source.steps; + return *this; + } +} bno08x_step_counter_t; + +/// @brief Struct to represent raw mems gyro data from raw gyro reports (units in ADC counts). +typedef struct bno08x_raw_gyro_t +{ + int16_t x; + int16_t y; + int16_t z; + int16_t temperature; + uint32_t timestamp_us; + BNO08xAccuracy accuracy; + + bno08x_raw_gyro_t() + : x(0U) + , y(0U) + , z(0U) + , temperature(0U) + , timestamp_us(0UL) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_RawGyroscope_t + bno08x_raw_gyro_t& operator=(const sh2_RawGyroscope_t& source) + { + this->x = source.x; + this->y = source.y; + this->z = source.z; + this->temperature = source.temperature; + this->timestamp_us = source.timestamp; + return *this; + } +} bno08x_raw_gyro_t; + +/// @brief Struct to represent raw mems accelerometer data from raw accelerometer reports (units in ADC counts). +typedef struct bno08x_raw_accel_t +{ + int16_t x; + int16_t y; + int16_t z; + uint32_t timestamp_us; + BNO08xAccuracy accuracy; + + bno08x_raw_accel_t() + : x(0U) + , y(0U) + , z(0U) + , timestamp_us(0UL) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_RawAccelerometer_t + bno08x_raw_accel_t& operator=(const sh2_RawAccelerometer_t& source) + { + this->x = source.x; + this->y = source.y; + this->z = source.z; + this->timestamp_us = source.timestamp; + return *this; + } +} bno08x_raw_accel_t; + +/// @brief Struct to represent raw mems magnetometer data from raw magnetometer reports (units in ADC counts). +typedef struct bno08x_raw_magf_t +{ + int16_t x; + int16_t y; + int16_t z; + uint32_t timestamp_us; + BNO08xAccuracy accuracy; + + bno08x_raw_magf_t() + : x(0U) + , y(0U) + , z(0U) + , timestamp_us(0UL) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_RawMagnetometer_t + bno08x_raw_magf_t& operator=(const sh2_RawMagnetometer_t& source) + { + this->x = source.x; + this->y = source.y; + this->z = source.z; + this->timestamp_us = source.timestamp; + return *this; + } +} bno08x_raw_magf_t; + +/// @brief Struct to represent stability classifier data from stability classifier reports. +typedef struct bno08x_stability_classifier_t +{ + BNO08xStability stability; + BNO08xAccuracy accuracy; + + bno08x_stability_classifier_t() + : stability(BNO08xStability::UNDEFINED) + , accuracy(BNO08xAccuracy::UNDEFINED) + { + } + + // conversion from sh2_StabilityClassifier_t + bno08x_stability_classifier_t& operator=(const sh2_StabilityClassifier_t& source) + { + this->stability = static_cast(source.classification); + return *this; + } + +} bno08x_stability_classifier_t; typedef bno08x_config_t imu_config_t; // legacy version compatibility \ No newline at end of file diff --git a/include/report/BNO08xStabilityClassifier.hpp b/include/report/BNO08xStabilityClassifier.hpp index b380bf7..36ed1d0 100644 --- a/include/report/BNO08xStabilityClassifier.hpp +++ b/include/report/BNO08xStabilityClassifier.hpp @@ -13,10 +13,11 @@ class BNO08xStabilityClassifier : public BNO08xRpt { } - BNO08xStability get(); + bno08x_stability_classifier_t get(); + BNO08xStability get_stability(); private: void update_data(sh2_SensorValue_t* sensor_val) override; - sh2_StabilityClassifier_t data = {0U}; + bno08x_stability_classifier_t data; static const constexpr char* TAG = "BNO08xStabilityClassifier"; }; diff --git a/source/BNO08x.cpp b/source/BNO08x.cpp index 533440a..08b2d38 100644 --- a/source/BNO08x.cpp +++ b/source/BNO08x.cpp @@ -126,7 +126,7 @@ bool BNO08x::initialize() if (init_tasks() != ESP_OK) return false; - // clang-format off + // clang-format off #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS ESP_LOGI(TAG, "Successfully initialized...."); #endif @@ -354,21 +354,26 @@ void BNO08x::handle_sensor_report(sh2_SensorValue_t* sensor_val) { uint8_t rpt_ID = sensor_val->sensorId; - // update respective report with new data - usr_reports.at(rpt_ID)->update_data(sensor_val); - - // 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)) + // check if report exists within map + auto it = usr_reports.find(rpt_ID); + if (it != usr_reports.end()) { - if (cb_list.size() != 0) - if (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE) - { - // clang-format off + // update respective report with new data + it->second->update_data(sensor_val); + + // 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 - } + // clang-format on + } + } } } @@ -1032,44 +1037,28 @@ esp_err_t BNO08x::deinit_sh2_HAL() */ bool BNO08x::hard_reset() { - int op_success = SH2_ERR; - + // toggle reset gpio toggle_reset(); - // wait for reset and run service to dispatch callbacks + // wait for reset to be detected by SH2 HAL lib if (wait_for_reset() == ESP_OK) { + // run service to dispatch callbacks lock_sh2_HAL(); sh2_service(); unlock_sh2_HAL(); // get product ids and check reset reason - memset(&product_IDs, 0, sizeof(sh2_ProductIds_t)); - lock_sh2_HAL(); - op_success = sh2_getProdIds(&product_IDs); - unlock_sh2_HAL(); - - if (op_success == SH2_OK) + if (get_reset_reason() == BNO08xResetReason::EXT_RST) { - if (static_cast(product_IDs.entry[0].resetCause) == BNO08xResetReason::EXT_RST) - { - return true; - } - else - { - // clang-format off - #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS - ESP_LOGE(TAG, "Hard reset failure, incorrect reset reason returned: %d.", product_IDs.entry[0].resetCause); - #endif - // clang-format on - } + return true; } else { // clang-format off - #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS - ESP_LOGE(TAG, "Hard reset failure, failed to get prodIDs."); - #endif + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Hard reset failure, incorrect reset reason returned: %d.", product_IDs.entry[0].resetCause); + #endif // clang-format on } } @@ -1094,45 +1083,30 @@ bool BNO08x::soft_reset() { int op_success = SH2_ERR; + // send reset command lock_sh2_HAL(); op_success = sh2_devReset(); unlock_sh2_HAL(); if (op_success == SH2_OK) { - // wait for reset and run service to dispatch callbacks + // wait for reset to be detected by SH2 HAL lib if (wait_for_reset() == ESP_OK) { + // run service to dispatch callbacks lock_sh2_HAL(); sh2_service(); unlock_sh2_HAL(); - // get product ids and check reset reason - memset(&product_IDs, 0, sizeof(sh2_ProductIds_t)); - lock_sh2_HAL(); - op_success = sh2_getProdIds(&product_IDs); - unlock_sh2_HAL(); - - if (op_success == SH2_OK) + if (get_reset_reason() == BNO08xResetReason::EXT_RST) { - if (static_cast(product_IDs.entry[0].resetCause) == BNO08xResetReason::EXT_RST) - { - return true; - } - else - { - // clang-format off - #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS - ESP_LOGE(TAG, "Soft reset failure, incorrect reset reason returned."); - #endif - // clang-format on - } + return true; } else { // clang-format off #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS - ESP_LOGE(TAG, "Soft reset failure, failed to get prodIDs."); + ESP_LOGE(TAG, "Soft reset failure, incorrect reset reason returned."); #endif // clang-format on } @@ -1158,6 +1132,81 @@ bool BNO08x::soft_reset() return false; } +/** + * @brief Returns reason for previous reset via product ID report. + * + * @return Enum object containing reset reason, BNO08xResetReason::UNDEFINED if failure. + */ +BNO08xResetReason BNO08x::get_reset_reason() +{ + int op_success = SH2_ERR; + BNO08xResetReason rr = BNO08xResetReason::UNDEFINED; + + memset(&product_IDs, 0, sizeof(sh2_ProductIds_t)); + lock_sh2_HAL(); + op_success = sh2_getProdIds(&product_IDs); + unlock_sh2_HAL(); + + if (op_success == SH2_OK) + { + rr = static_cast(product_IDs.entry[0].resetCause); + } + else + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Get reset reason failure, failed to get prodIDs."); + #endif + // clang-format on + } + + return rr; +} + +/** + * @brief Places BNO08x device in on state by sending ON (2) command on "device" channel. + * + * @return True if on operation succeeded. + */ +bool BNO08x::on() +{ + int op_success = SH2_ERR; + + lock_sh2_HAL(); + op_success = sh2_devOn(); + unlock_sh2_HAL(); + + return (op_success == SH2_OK); +} + +/** + * @brief Places BNO08x device in sleep state by sending SLEEP (3) command on "device" channel. + * + * @return True if sleep operation succeeded. + */ +bool BNO08x::sleep() +{ + + int op_success = SH2_ERR; + + lock_sh2_HAL(); + op_success = sh2_devSleep(); + unlock_sh2_HAL(); + + return (op_success == SH2_OK); +} + +bool BNO08x::get_frs(BNO08xFRSID frs_ID, uint32_t* data, uint16_t* rx_data_sz) +{ + int op_success = SH2_ERR; + + lock_sh2_HAL(); + op_success = sh2_getFrs(static_cast(frs_ID), data, rx_data_sz); + unlock_sh2_HAL(); + + return (op_success == SH2_OK); +} + /** * @brief Waits for HINT pin assertion or HOST_INT_TIMEOUT_DEFAULT_MS to elapse. * @@ -1368,6 +1417,25 @@ const char* BNO08x::stability_to_str(BNO08xStability stability) } } +const char* BNO08x::accuracy_to_str(BNO08xAccuracy accuracy) +{ + switch (accuracy) + { + case BNO08xAccuracy::UNRELIABLE: + return "UNRELIABLE"; + case BNO08xAccuracy::LOW: + return "LOW"; + case BNO08xAccuracy::MED: + return "MED"; + case BNO08xAccuracy::HIGH: + return "HIGH"; + case BNO08xAccuracy::UNDEFINED: + return "UNDEFINED"; + default: + return "UNDEFINED"; + } +} + /** * @brief HINT interrupt service routine, handles falling edge of BNO08x HINT pin. * diff --git a/source/BNO08xRpt.cpp b/source/BNO08xRpt.cpp index 0f8bf8c..3248f7a 100644 --- a/source/BNO08xRpt.cpp +++ b/source/BNO08xRpt.cpp @@ -29,6 +29,7 @@ bool BNO08xRpt::enable(uint32_t time_between_reports, sh2_SensorConfig_t sensor_ { period_us = time_between_reports; // Update the period xEventGroupSetBits(imu->evt_grp_report_en, rpt_bit); // Set the event group bit + vTaskDelay(20UL / portTICK_PERIOD_MS); // delay a bit to allow command to execute return true; } } diff --git a/source/report/BNO08xRptARVRStabilizedGameRV.cpp b/source/report/BNO08xRptARVRStabilizedGameRV.cpp index 4ed2f07..ab36a18 100644 --- a/source/report/BNO08xRptARVRStabilizedGameRV.cpp +++ b/source/report/BNO08xRptARVRStabilizedGameRV.cpp @@ -12,6 +12,7 @@ void BNO08xRptARVRStabilizedGameRV::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.arvrStabilizedGRV; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptARVRStabilizedRV.cpp b/source/report/BNO08xRptARVRStabilizedRV.cpp index 757510e..425012e 100644 --- a/source/report/BNO08xRptARVRStabilizedRV.cpp +++ b/source/report/BNO08xRptARVRStabilizedRV.cpp @@ -12,6 +12,7 @@ void BNO08xRptARVRStabilizedRV::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.arvrStabilizedRV; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptAcceleration.cpp b/source/report/BNO08xRptAcceleration.cpp index c6b7b0e..4cf8b90 100644 --- a/source/report/BNO08xRptAcceleration.cpp +++ b/source/report/BNO08xRptAcceleration.cpp @@ -12,6 +12,7 @@ void BNO08xRptAcceleration::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.accelerometer; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptActivityClassifier.cpp b/source/report/BNO08xRptActivityClassifier.cpp index e8eb6ae..33a0e44 100644 --- a/source/report/BNO08xRptActivityClassifier.cpp +++ b/source/report/BNO08xRptActivityClassifier.cpp @@ -28,6 +28,7 @@ void BNO08xRptActivityClassifier::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.personalActivityClassifier; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptCalGyro.cpp b/source/report/BNO08xRptCalGyro.cpp index fb5054d..4d4eca4 100644 --- a/source/report/BNO08xRptCalGyro.cpp +++ b/source/report/BNO08xRptCalGyro.cpp @@ -12,6 +12,7 @@ void BNO08xRptCalGyro::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.gyroscope; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptCalMagnetometer.cpp b/source/report/BNO08xRptCalMagnetometer.cpp index b800312..c20117d 100644 --- a/source/report/BNO08xRptCalMagnetometer.cpp +++ b/source/report/BNO08xRptCalMagnetometer.cpp @@ -12,6 +12,7 @@ void BNO08xRptCalMagnetometer::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.magneticField; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptGameRV.cpp b/source/report/BNO08xRptGameRV.cpp index a2d53b9..bb83814 100644 --- a/source/report/BNO08xRptGameRV.cpp +++ b/source/report/BNO08xRptGameRV.cpp @@ -12,6 +12,7 @@ void BNO08xRptGameRV::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.gameRotationVector; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptGravity.cpp b/source/report/BNO08xRptGravity.cpp index 2325af1..cb8beac 100644 --- a/source/report/BNO08xRptGravity.cpp +++ b/source/report/BNO08xRptGravity.cpp @@ -12,6 +12,7 @@ void BNO08xRptGravity::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.gravity; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptIGyroRV.cpp b/source/report/BNO08xRptIGyroRV.cpp index dc4f186..893f6b4 100644 --- a/source/report/BNO08xRptIGyroRV.cpp +++ b/source/report/BNO08xRptIGyroRV.cpp @@ -12,6 +12,7 @@ void BNO08xRptIGyroRV::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.gyroIntegratedRV; + data.accuracy = static_cast(sensor_val->status); data_vel = sensor_val->un.gyroIntegratedRV; imu->unlock_user_data(); diff --git a/source/report/BNO08xRptLinearAcceleration.cpp b/source/report/BNO08xRptLinearAcceleration.cpp index 34ab20d..612795b 100644 --- a/source/report/BNO08xRptLinearAcceleration.cpp +++ b/source/report/BNO08xRptLinearAcceleration.cpp @@ -12,6 +12,7 @@ void BNO08xRptLinearAcceleration::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.linearAcceleration; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptRV.cpp b/source/report/BNO08xRptRV.cpp index 911e673..d231978 100644 --- a/source/report/BNO08xRptRV.cpp +++ b/source/report/BNO08xRptRV.cpp @@ -12,6 +12,7 @@ void BNO08xRptRV::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.rotationVector; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptRVGeneric.cpp b/source/report/BNO08xRptRVGeneric.cpp index 5ebdb5f..70495e8 100644 --- a/source/report/BNO08xRptRVGeneric.cpp +++ b/source/report/BNO08xRptRVGeneric.cpp @@ -2,9 +2,9 @@ #include "BNO08x.hpp" /** - * @brief Grabs most recent rotation vector data in form of unit quaternion, accuracy units in radians (if available, else constant 0.0f). + * @brief Grabs most recent rotation vector data in form of unit quaternion, rad accuracy units in radians (if available, else constant 0.0f). * - * The following RV reports have accuracy data: + * The following RV reports have rad accuracy data: * * - rotation vector * - geomagnetic rotation vector diff --git a/source/report/BNO08xRptRVGeomag.cpp b/source/report/BNO08xRptRVGeomag.cpp index 71f3bfa..e3dbf3e 100644 --- a/source/report/BNO08xRptRVGeomag.cpp +++ b/source/report/BNO08xRptRVGeomag.cpp @@ -12,6 +12,7 @@ void BNO08xRptRVGeomag::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.geoMagRotationVector; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptRawMEMSAccelerometer.cpp b/source/report/BNO08xRptRawMEMSAccelerometer.cpp index 3e244b1..d10d749 100644 --- a/source/report/BNO08xRptRawMEMSAccelerometer.cpp +++ b/source/report/BNO08xRptRawMEMSAccelerometer.cpp @@ -12,8 +12,9 @@ void BNO08xRptRawMEMSAccelerometer::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.rawAccelerometer; + data.accuracy = static_cast(sensor_val->status); 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 249bf70..ac3280e 100644 --- a/source/report/BNO08xRptRawMEMSGyro.cpp +++ b/source/report/BNO08xRptRawMEMSGyro.cpp @@ -12,6 +12,7 @@ void BNO08xRptRawMEMSGyro::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.rawGyroscope; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptRawMEMSMagnetometer.cpp b/source/report/BNO08xRptRawMEMSMagnetometer.cpp index d976aaf..ce7c3ce 100644 --- a/source/report/BNO08xRptRawMEMSMagnetometer.cpp +++ b/source/report/BNO08xRptRawMEMSMagnetometer.cpp @@ -12,6 +12,7 @@ void BNO08xRptRawMEMSMagnetometer::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.rawMagnetometer; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptStepCounter.cpp b/source/report/BNO08xRptStepCounter.cpp index c8e94e1..9fabfff 100644 --- a/source/report/BNO08xRptStepCounter.cpp +++ b/source/report/BNO08xRptStepCounter.cpp @@ -23,6 +23,7 @@ void BNO08xRptStepCounter::update_data(sh2_SensorValue_t* sensor_val) } prev_steps = data.steps; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xRptUncalGyro.cpp b/source/report/BNO08xRptUncalGyro.cpp index dfb997c..1702634 100644 --- a/source/report/BNO08xRptUncalGyro.cpp +++ b/source/report/BNO08xRptUncalGyro.cpp @@ -12,6 +12,7 @@ void BNO08xRptUncalGyro::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.gyroscopeUncal; + data.accuracy = static_cast(sensor_val->status); bias_data = sensor_val->un.gyroscopeUncal; imu->unlock_user_data(); diff --git a/source/report/BNO08xRptUncalMagnetometer.cpp b/source/report/BNO08xRptUncalMagnetometer.cpp index 2435c63..1d8abf3 100644 --- a/source/report/BNO08xRptUncalMagnetometer.cpp +++ b/source/report/BNO08xRptUncalMagnetometer.cpp @@ -12,6 +12,7 @@ void BNO08xRptUncalMagnetometer::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.magneticFieldUncal; + data.accuracy = static_cast(sensor_val->status); bias_data = sensor_val->un.magneticFieldUncal; imu->unlock_user_data(); diff --git a/source/report/BNO08xShakeDetector.cpp b/source/report/BNO08xShakeDetector.cpp index 69b9fe1..5897cd0 100644 --- a/source/report/BNO08xShakeDetector.cpp +++ b/source/report/BNO08xShakeDetector.cpp @@ -12,6 +12,7 @@ void BNO08xShakeDetector::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.shakeDetector; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) diff --git a/source/report/BNO08xStabilityClassifier.cpp b/source/report/BNO08xStabilityClassifier.cpp index 7adb8b8..26eeda7 100644 --- a/source/report/BNO08xStabilityClassifier.cpp +++ b/source/report/BNO08xStabilityClassifier.cpp @@ -12,6 +12,7 @@ void BNO08xStabilityClassifier::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.stabilityClassifier; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en)) @@ -23,10 +24,23 @@ void BNO08xStabilityClassifier::update_data(sh2_SensorValue_t* sensor_val) * * @return BNO08xStability enum object with detected state. */ -BNO08xStability BNO08xStabilityClassifier::get() +bno08x_stability_classifier_t BNO08xStabilityClassifier::get() { imu->lock_user_data(); - BNO08xStability rqdata = static_cast(data.classification); + bno08x_stability_classifier_t rqdata = data; imu->unlock_user_data(); return rqdata; -} \ No newline at end of file +} + +/** + * @brief Grabs most recent stability classifier reading (excludes accuracy) + * + * @return BNO08xStability enum object with detected state. + */ +BNO08xStability BNO08xStabilityClassifier::get_stability() +{ + imu->lock_user_data(); + BNO08xStability rqdata = data.stability; + imu->unlock_user_data(); + return rqdata; +} diff --git a/source/report/BNO08xTapDetector.cpp b/source/report/BNO08xTapDetector.cpp index 9bf131e..b13feb6 100644 --- a/source/report/BNO08xTapDetector.cpp +++ b/source/report/BNO08xTapDetector.cpp @@ -28,6 +28,7 @@ void BNO08xTapDetector::update_data(sh2_SensorValue_t* sensor_val) { imu->lock_user_data(); data = sensor_val->un.tapDetector; + data.accuracy = static_cast(sensor_val->status); imu->unlock_user_data(); if (rpt_bit & xEventGroupGetBits(imu->evt_grp_report_en))