From e9fbbb5c88f2b5e90ee627e58950bd49389624e3 Mon Sep 17 00:00:00 2001 From: flyinggorilla Date: Sun, 25 May 2025 16:58:44 +0200 Subject: [PATCH] added system orientation and DCD flash erasing --- include/BNO08x.hpp | 8 +++ source/BNO08x.cpp | 124 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 132 insertions(+) diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index f7f73ca..98f3064 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -55,6 +55,13 @@ class BNO08x bool dynamic_calibration_clear(); bool dynamic_calibration_run_routine(); + bool delete_calibration_data(); + + constexpr static float SQRT2OVER2 = 0.7071067811865476f; // sqrt(2)/2, used for setting system orientation + bool set_system_orientation(float w, float x, float y, float z); + bool get_system_orientation(float &w, float &x, float &y, float &z); + + bool get_frs(uint16_t frs_ID, uint32_t (&data)[16], uint16_t& rx_data_sz); sh2_ProductIds_t get_product_IDs(); @@ -63,6 +70,7 @@ class BNO08x bool register_cb(std::function cb_fxn); void print_product_ids(); + void print_system_orientation(); // enum helper fxns static const char* activity_to_str(BNO08xActivity activity); diff --git a/source/BNO08x.cpp b/source/BNO08x.cpp index 7e1dd68..0ad53e2 100644 --- a/source/BNO08x.cpp +++ b/source/BNO08x.cpp @@ -1681,6 +1681,47 @@ bool BNO08x::dynamic_calibration_run_routine() return true; } +bool BNO08x::delete_calibration_data() +{ + // 1. Reset hub (using hard_reset) + if (!hard_reset()) { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "delete_calibration_data(): failed to hard reset hub"); + #endif + return false; + } + + // 2. Delete flash copy of DCD via FRS + int op_success = SH2_ERR; + lock_sh2_HAL(); + // Deleting FRS record: use sh2_setFrs with nullptr and 0 words + op_success = sh2_setFrs(DYNAMIC_CALIBRATION, nullptr, 0U); + unlock_sh2_HAL(); + if (op_success != SH2_OK) { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "delete_calibration_data(): failed to delete DCD FRS record, op_success: %li", (int32_t)op_success); + #endif + return false; + } + + // 3. Issue Clear DCD and Reset Command (atomic clear DCD from RAM and reset) + lock_sh2_HAL(); + op_success = sh2_clearDcdAndReset(); + unlock_sh2_HAL(); + if (op_success != SH2_OK) { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "delete_calibration_data(): failed to clear DCD and reset, op_success: %li", (int32_t)op_success); + #endif + return false; + } + + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGI(TAG, "delete_calibration_data(): calibration data cleared successfully"); + #endif + + return true; +} + /** * @brief Retrieves a record from flash record system (if your goal is to retrieve meta data use the * BNO08xRpt:get_meta_data() method instead) @@ -1885,6 +1926,89 @@ void BNO08x::print_product_ids() } } + +// Converts a 32-bit signed Q30 fixed-point value to float +static inline float q30_to_float(int32_t q) +{ + return ((float)q) / (float)(1UL << 30); +} + +// Converts a float to 32-bit signed Q30 fixed-point value +static inline int32_t float_to_q30(float f) +{ + if (f > 1.0f) f = 1.0f; + if (f < -1.0f) f = -1.0f; + return (int32_t)(f * (float)(1UL << 30)); +} + + +void BNO08x::print_system_orientation() +{ + float w, x, y, z; + if (get_system_orientation(w, x, y, z)) { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGI(TAG, "Mounting orientation (float): W: %.6f X: %.6f Y: %.6f Z: %.6f", w, x, y, z); + #endif + } else { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Failed to get mounting orientation"); + #endif + } +} + +/** + * @brief Sets the system orientation of the BNO08x device and persist it in flash (FRS). + * use SQRT2OVER2 as a constant for sqrt(2)/2 + * see Datasheet Figure 4.3 for reference + * Note that a reset is required to apply changes. + * Note also that this configuration seems only to work if reports are already enabled. + * e.g. set .rpt.rv.enable(true) prior this call + */ +bool BNO08x::set_system_orientation(float w, float x, float y, float z) +{ + uint32_t orientation_raw[4] = { + static_cast(float_to_q30(x)), // X component + static_cast(float_to_q30(y)), // Y component + static_cast(float_to_q30(z)), // Z component + static_cast(float_to_q30(w)) // W component + }; + + int op_success = SH2_ERR; + lock_sh2_HAL(); + op_success = sh2_setFrs(SYSTEM_ORIENTATION, (uint32_t*)&orientation_raw, 4); + unlock_sh2_HAL(); + + if (op_success != SH2_OK) + { + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Failed to set mounting orientation, op_success: %li", (int32_t)op_success); + #endif + return false; + } + return true; +} + +bool BNO08x::get_system_orientation(float& w, float& x, float& y, float& z) +{ + uint16_t words = 4; + uint32_t raw[4] = {0}; + + lock_sh2_HAL(); + int op_success = sh2_getFrs(SYSTEM_ORIENTATION, raw, &words); + unlock_sh2_HAL(); + + if (op_success != SH2_OK) { + return false; + } + + x = q30_to_float((int32_t)raw[0]); + y = q30_to_float((int32_t)raw[1]); + z = q30_to_float((int32_t)raw[2]); + w = q30_to_float((int32_t)raw[3]); + + return true; +} + /** * @brief Converts a BNO08xActivity enum to string. *