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"
INCLUDE_DIRS "." "include" "include/report" "include/callback" "SH2"
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"
)

95
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 (xQueueSend(queue_cb_report_id, &rpt_ID, 0) != pdTRUE)
{
// clang-format off
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "Callback queue full, callback execution for report missed.");
#endif
@ -637,13 +637,8 @@ esp_err_t BNO08x::init_tasks()
xEventGroupSetBits(sync_ctx.evt_grp_task, EVT_GRP_BNO08x_TASKS_RUNNING);
// launch data processing task 6
task_created = xTaskCreatePinnedToCore(
&data_proc_task_trampoline, "bno08x_data_processing_task",
DATA_PROC_TASK_SZ,
this,
DATA_PROC_TASK_PRIORITY,
&data_proc_task_hdl,
DATA_PROC_TASK_AFFINITY);
task_created = xTaskCreatePinnedToCore(&data_proc_task_trampoline, "bno08x_data_processing_task", DATA_PROC_TASK_SZ, this,
DATA_PROC_TASK_PRIORITY, &data_proc_task_hdl, DATA_PROC_TASK_AFFINITY);
if (task_created != pdTRUE)
{
@ -661,12 +656,8 @@ esp_err_t BNO08x::init_tasks()
}
// launch cb task 5
task_created = xTaskCreatePinnedToCore(&cb_task_trampoline, "bno08x_cb_task",
CB_TASK_SZ,
this,
CB_TASK_PRIORITY,
&cb_task_hdl,
CB_TASK_AFFINITY);
task_created = xTaskCreatePinnedToCore(
&cb_task_trampoline, "bno08x_cb_task", CB_TASK_SZ, this, CB_TASK_PRIORITY, &cb_task_hdl, CB_TASK_AFFINITY);
if (task_created != pdTRUE)
{
@ -684,12 +675,9 @@ esp_err_t BNO08x::init_tasks()
}
// launch sh2 hal service task 7
task_created = xTaskCreatePinnedToCore(&sh2_HAL_service_task_trampoline, "bno08x_sh2_HAL_service_task",
SH2_HAL_SERVICE_TASK_SZ,
this,
SH2_HAL_SERVICE_TASK_PRIORITY,
&sh2_HAL_service_task_hdl,
SH2_HAL_SERVICE_TASK_AFFINITY);
task_created =
xTaskCreatePinnedToCore(&sh2_HAL_service_task_trampoline, "bno08x_sh2_HAL_service_task", SH2_HAL_SERVICE_TASK_SZ,
this, SH2_HAL_SERVICE_TASK_PRIORITY, &sh2_HAL_service_task_hdl, SH2_HAL_SERVICE_TASK_AFFINITY);
if (task_created != pdTRUE)
{
@ -1472,12 +1460,11 @@ bool BNO08x::dynamic_calibration_data_clear_ram()
* Routine does the following:alignas
*
* 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)
* 3) enables game rotation vector reports and calibrated magnetic field reports
* 4) moving window average for accuracy received through reports
* 5) deems calibration accuracy threshold met when magf accuracy avg is >=2 (MED) and quat accuracy avg >=3 (HIGH) for longer than 5 seconds
* 6) sends command to save dynamic calibration data
* 7) 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) 3) enables game rotation vector reports and calibrated magnetic field reports 4) moving window
* average for accuracy received through reports 5) deems calibration accuracy threshold met when magf accuracy avg is >=2 (MED)
* and quat accuracy avg >=3 (HIGH) for longer than 5 seconds 6) sends command to save dynamic calibration data 7) disables all
* enabled reports
*
* 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
@ -1689,25 +1676,26 @@ bool BNO08x::dynamic_calibration_run_routine()
bool BNO08x::dynamic_calibration_data_delete()
{
// 1. Reset hub (using hard_reset)
if (!hard_reset()) {
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
if (!hard_reset())
{
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "delete_calibration_data(): failed to hard reset hub");
#endif
#endif
return false;
}
// 2. Delete flash copy of DCD via FRS
// 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;
// 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;
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGI(TAG, "delete_calibration_data(): calibration data cleared successfully");
#endif
#endif
return true;
}
@ -1840,6 +1828,7 @@ void BNO08x::toggle_reset()
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_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,30 +1957,34 @@ void BNO08x::print_product_ids()
void BNO08x::print_system_orientation()
{
float w, x, y, z;
if (get_system_orientation(w, x, y, z)) {
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
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
#endif
}
else
{
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS
ESP_LOGE(TAG, "Failed to get mounting orientation");
#endif
#endif
}
}
// Internal private function, 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);
return ((float) q) / (float) (1UL << 30);
}
// Internal private function, 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));
if (f > 1.0f)
f = 1.0f;
if (f < -1.0f)
f = -1.0f;
return (int32_t) (f * (float) (1UL << 30));
}
/**
@ -2011,13 +2004,13 @@ static inline int32_t float_to_q30(float f)
bool BNO08x::set_system_orientation(float Qw, float Qx, float Qy, float Qz)
{
uint32_t orientation_raw[4] = {
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(Qz)), // Z component
static_cast<uint32_t>(float_to_q30(Qw)) // W 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(Qz)), // Z 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 true;
@ -2036,10 +2029,10 @@ bool BNO08x::get_system_orientation(float& Qw, float& Qx, float& Qy, float& Qz)
uint16_t words_rxd = 0U;
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;
if(words_rxd < 4U)
if (words_rxd < 4U)
{
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS