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

idk, fix for initializing forever + formatting that im only allowing cuz its not a real project

This commit is contained in:
franchioping 2026-03-16 14:23:00 +00:00
parent d78f6e5c47
commit ae2dd0bef1
3 changed files with 70 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build/

10
CMakeLists.txt
View File

@ -1,3 +1,13 @@
set(COMPONENT_WRITE_STRIP_PEDANTIC_FLAGS 1)
idf_component_register(SRC_DIRS "source" "source/report" "SH2" idf_component_register(SRC_DIRS "source" "source/report" "SH2"
INCLUDE_DIRS "." "include" "include/report" "include/callback" "SH2" INCLUDE_DIRS "." "include" "include/report" "include/callback" "SH2"
REQUIRES driver esp_timer cmock) REQUIRES driver esp_timer cmock)
target_compile_options(${COMPONENT_LIB} PRIVATE
"-Wno-gnu-array-member-paren-init"
"-Wno-parentheses-equality"
"-Wno-template-in-declaration-name"
"-w"
)

125
source/BNO08x.cpp
View File

@ -359,7 +359,7 @@ void BNO08x::handle_sensor_report(sh2_SensorValue_t* sensor_val)
if (sync_ctx.cb_list.size() != 0) if (sync_ctx.cb_list.size() != 0)
if (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE) if (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE)
{ {
// clang-format off // clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "Callback queue full, callback execution for report missed."); ESP_LOGE(TAG, "Callback queue full, callback execution for report missed.");
#endif #endif
@ -637,13 +637,8 @@ esp_err_t BNO08x::init_tasks()
xEventGroupSetBits(sync_ctx.evt_grp_task, EVT_GRP_BNO08x_TASKS_RUNNING); xEventGroupSetBits(sync_ctx.evt_grp_task, EVT_GRP_BNO08x_TASKS_RUNNING);
// launch data processing task 6 // launch data processing task 6
task_created = xTaskCreatePinnedToCore( task_created = xTaskCreatePinnedToCore(&data_proc_task_trampoline, "bno08x_data_processing_task", DATA_PROC_TASK_SZ, this,
&data_proc_task_trampoline, "bno08x_data_processing_task", DATA_PROC_TASK_PRIORITY, &data_proc_task_hdl, DATA_PROC_TASK_AFFINITY);
DATA_PROC_TASK_SZ,
this,
DATA_PROC_TASK_PRIORITY,
&data_proc_task_hdl,
DATA_PROC_TASK_AFFINITY);
if (task_created != pdTRUE) if (task_created != pdTRUE)
{ {
@ -661,12 +656,8 @@ esp_err_t BNO08x::init_tasks()
} }
// launch cb task 5 // launch cb task 5
task_created = xTaskCreatePinnedToCore(&cb_task_trampoline, "bno08x_cb_task", task_created = xTaskCreatePinnedToCore(
CB_TASK_SZ, &cb_task_trampoline, "bno08x_cb_task", CB_TASK_SZ, this, CB_TASK_PRIORITY, &cb_task_hdl, CB_TASK_AFFINITY);
this,
CB_TASK_PRIORITY,
&cb_task_hdl,
CB_TASK_AFFINITY);
if (task_created != pdTRUE) if (task_created != pdTRUE)
{ {
@ -684,12 +675,9 @@ esp_err_t BNO08x::init_tasks()
} }
// launch sh2 hal service task 7 // launch sh2 hal service task 7
task_created = xTaskCreatePinnedToCore(&sh2_HAL_service_task_trampoline, "bno08x_sh2_HAL_service_task", task_created =
SH2_HAL_SERVICE_TASK_SZ, xTaskCreatePinnedToCore(&sh2_HAL_service_task_trampoline, "bno08x_sh2_HAL_service_task", SH2_HAL_SERVICE_TASK_SZ,
this, this, SH2_HAL_SERVICE_TASK_PRIORITY, &sh2_HAL_service_task_hdl, SH2_HAL_SERVICE_TASK_AFFINITY);
SH2_HAL_SERVICE_TASK_PRIORITY,
&sh2_HAL_service_task_hdl,
SH2_HAL_SERVICE_TASK_AFFINITY);
if (task_created != pdTRUE) if (task_created != pdTRUE)
{ {
@ -1468,22 +1456,21 @@ bool BNO08x::dynamic_calibration_data_clear_ram()
/** /**
* @brief Example calibration routine using dynamic/ME calibration commands. * @brief Example calibration routine using dynamic/ME calibration commands.
* *
* Routine does the following:alignas * Routine does the following:alignas
* *
* 1) disables all enabled reports * 1) disables all enabled reports
* 2) sends a command to enable dynamic/motion engine calibration for all possible options (SH2_CAL_ACCEL | SH2_CAL_GYRO | SH2_CAL_MAG | SH2_CAL_PLANAR) * 2) sends a command to enable dynamic/motion engine calibration for all possible options (SH2_CAL_ACCEL | SH2_CAL_GYRO |
* 3) enables game rotation vector reports and calibrated magnetic field reports * SH2_CAL_MAG | SH2_CAL_PLANAR) 3) enables game rotation vector reports and calibrated magnetic field reports 4) moving window
* 4) moving window average for accuracy received through reports * average for accuracy received through reports 5) deems calibration accuracy threshold met when magf accuracy avg is >=2 (MED)
* 5) deems calibration accuracy threshold met when magf accuracy avg is >=2 (MED) and quat accuracy avg >=3 (HIGH) for longer than 5 seconds * and quat accuracy avg >=3 (HIGH) for longer than 5 seconds 6) sends command to save dynamic calibration data 7) disables all
* 6) sends command to save dynamic calibration data * enabled reports
* 7) disables all enabled reports *
*
* Note the DCD commands don't have to be used this way, this is just an example, * Note the DCD commands don't have to be used this way, this is just an example,
* but the dynamic_calibration_autosave_enable() allows calibration to be run and * but the dynamic_calibration_autosave_enable() allows calibration to be run and
* saved constantly even while data is used for other operations. * saved constantly even while data is used for other operations.
* *
* @return True if calibration routine succeeded. * @return True if calibration routine succeeded.
*/ */
bool BNO08x::dynamic_calibration_run_routine() bool BNO08x::dynamic_calibration_run_routine()
{ {
@ -1689,25 +1676,26 @@ bool BNO08x::dynamic_calibration_run_routine()
bool BNO08x::dynamic_calibration_data_delete() bool BNO08x::dynamic_calibration_data_delete()
{ {
// 1. Reset hub (using hard_reset) // 1. Reset hub (using hard_reset)
if (!hard_reset()) { if (!hard_reset())
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS {
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "delete_calibration_data(): failed to hard reset hub"); ESP_LOGE(TAG, "delete_calibration_data(): failed to hard reset hub");
#endif #endif
return false; return false;
} }
// 2. Delete flash copy of DCD via FRS // 2. Delete flash copy of DCD via FRS
// Deleting FRS record: use sh2_setFrs with nullptr and 0 words // Deleting FRS record: use sh2_setFrs with nullptr and 0 words
if(!write_frs(BNO08xFrsID::DYNAMIC_CALIBRATION, nullptr, 0U)) if (!write_frs(BNO08xFrsID::DYNAMIC_CALIBRATION, nullptr, 0U))
return false; return false;
// 3. Issue Clear DCD and Reset Command (atomic clear DCD from RAM and reset) // 3. Issue Clear DCD and Reset Command (atomic clear DCD from RAM and reset)
if(!dynamic_calibration_data_clear_ram()) if (!dynamic_calibration_data_clear_ram())
return false; return false;
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGI(TAG, "delete_calibration_data(): calibration data cleared successfully"); ESP_LOGI(TAG, "delete_calibration_data(): calibration data cleared successfully");
#endif #endif
return true; return true;
} }
@ -1733,7 +1721,7 @@ bool BNO08x::get_frs(BNO08xFrsID frs_ID, uint32_t (&data)[16], uint16_t& rx_data
unlock_sh2_HAL(); unlock_sh2_HAL();
if (op_success != SH2_OK) if (op_success != SH2_OK)
{ {
// clang-format off // clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "get_frs(): Failed to retrieve FRS record for ID: %s, op_success: %li", BNO08xFrsID_to_str(frs_ID), static_cast<int32_t>(op_success)); ESP_LOGE(TAG, "get_frs(): Failed to retrieve FRS record for ID: %s, op_success: %li", BNO08xFrsID_to_str(frs_ID), static_cast<int32_t>(op_success));
@ -1840,6 +1828,7 @@ void BNO08x::toggle_reset()
vTaskDelay(HARD_RESET_DELAY_MS); // 10ns min, set to larger delay to let things stabilize(Anton) vTaskDelay(HARD_RESET_DELAY_MS); // 10ns min, set to larger delay to let things stabilize(Anton)
gpio_intr_enable(imu_config.io_int); // enable interrupts before bringing out of reset gpio_intr_enable(imu_config.io_int); // enable interrupts before bringing out of reset
gpio_set_level(imu_config.io_rst, 1); // bring out of reset gpio_set_level(imu_config.io_rst, 1); // bring out of reset
vTaskDelay(HARD_RESET_DELAY_MS); // 10ns min, set to larger delay to let things stabilize(Anton)
} }
/** /**
@ -1968,41 +1957,45 @@ void BNO08x::print_product_ids()
void BNO08x::print_system_orientation() void BNO08x::print_system_orientation()
{ {
float w, x, y, z; float w, x, y, z;
if (get_system_orientation(w, x, y, z)) { if (get_system_orientation(w, x, y, z))
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS {
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGI(TAG, "Mounting orientation (float): W: %.6f X: %.6f Y: %.6f Z: %.6f", w, x, y, z); ESP_LOGI(TAG, "Mounting orientation (float): W: %.6f X: %.6f Y: %.6f Z: %.6f", w, x, y, z);
#endif #endif
} else { }
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS else
{
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "Failed to get mounting orientation"); ESP_LOGE(TAG, "Failed to get mounting orientation");
#endif #endif
} }
} }
// Internal private function, converts a 32-bit signed Q30 fixed-point value to float // Internal private function, converts a 32-bit signed Q30 fixed-point value to float
static inline float q30_to_float(int32_t q) static inline float q30_to_float(int32_t q)
{ {
return ((float)q) / (float)(1UL << 30); return ((float) q) / (float) (1UL << 30);
} }
// Internal private function, converts a float to 32-bit signed Q30 fixed-point value // Internal private function, converts a float to 32-bit signed Q30 fixed-point value
static inline int32_t float_to_q30(float f) static inline int32_t float_to_q30(float f)
{ {
if (f > 1.0f) f = 1.0f; if (f > 1.0f)
if (f < -1.0f) f = -1.0f; f = 1.0f;
return (int32_t)(f * (float)(1UL << 30)); if (f < -1.0f)
f = -1.0f;
return (int32_t) (f * (float) (1UL << 30));
} }
/** /**
* @brief Sets the system orientation of the BNO08x device and persist it in flash (FRS). * @brief Sets the system orientation of the BNO08x device and persist it in flash (FRS).
* *
* @note Datasheet Figure 4.3 for reference of physical mounting position relative to mapping quaternion. * @note Datasheet Figure 4.3 for reference of physical mounting position relative to mapping quaternion.
* @note Use SQRT2OVER2 as a constant for sqrt(2)/2 * @note Use SQRT2OVER2 as a constant for sqrt(2)/2
* @note that a reset is required to apply changes. * @note that a reset is required to apply changes.
* @note This configuration seems only to work if reports are already enabled. * @note This configuration seems only to work if reports are already enabled.
* e.g. set .rpt.rv.enable(PERIOD) prior this call * e.g. set .rpt.rv.enable(PERIOD) prior this call
* *
* @param Qw Real component of mapping quaternion. * @param Qw Real component of mapping quaternion.
* @param Qx X (i) component of mapping quaternion. * @param Qx X (i) component of mapping quaternion.
* @param Qy Y (j) component of mapping quaternion. * @param Qy Y (j) component of mapping quaternion.
@ -2011,21 +2004,21 @@ static inline int32_t float_to_q30(float f)
bool BNO08x::set_system_orientation(float Qw, float Qx, float Qy, float Qz) bool BNO08x::set_system_orientation(float Qw, float Qx, float Qy, float Qz)
{ {
uint32_t orientation_raw[4] = { uint32_t orientation_raw[4] = {
static_cast<uint32_t>(float_to_q30(Qx)), // X component static_cast<uint32_t>(float_to_q30(Qx)), // X component
static_cast<uint32_t>(float_to_q30(Qy)), // Y component static_cast<uint32_t>(float_to_q30(Qy)), // Y component
static_cast<uint32_t>(float_to_q30(Qz)), // Z component static_cast<uint32_t>(float_to_q30(Qz)), // Z component
static_cast<uint32_t>(float_to_q30(Qw)) // W component static_cast<uint32_t>(float_to_q30(Qw)) // W component
}; };
if(!write_frs(BNO08xFrsID::SYSTEM_ORIENTATION, orientation_raw, sizeof(orientation_raw)/sizeof(uint32_t))) if (!write_frs(BNO08xFrsID::SYSTEM_ORIENTATION, orientation_raw, sizeof(orientation_raw) / sizeof(uint32_t)))
return false; return false;
return true; return true;
} }
/** /**
* @brief Retrieves the system orientation of the BNO08x device and converts to float. * @brief Retrieves the system orientation of the BNO08x device and converts to float.
* *
* @param Qw Reference to save real component of mapping quaternion. * @param Qw Reference to save real component of mapping quaternion.
* @param Qx Reference to save X (i) component of mapping quaternion. * @param Qx Reference to save X (i) component of mapping quaternion.
* @param Qy Reference to save Y (j) component of mapping quaternion. * @param Qy Reference to save Y (j) component of mapping quaternion.
@ -2036,10 +2029,10 @@ bool BNO08x::get_system_orientation(float& Qw, float& Qx, float& Qy, float& Qz)
uint16_t words_rxd = 0U; uint16_t words_rxd = 0U;
uint32_t raw[16] = {0}; uint32_t raw[16] = {0};
if(!get_frs(BNO08xFrsID::SYSTEM_ORIENTATION, raw, words_rxd)) if (!get_frs(BNO08xFrsID::SYSTEM_ORIENTATION, raw, words_rxd))
return false; return false;
if(words_rxd < 4U) if (words_rxd < 4U)
{ {
// clang-format off // clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS