diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 897f17a..df46521 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -32,50 +32,102 @@ class BNO08x { public: + inline static sh2_SensorConfig default_sensor_cfg = {.changeSensitivityEnabled = false, /// cb_fxn); - void print_product_ids(); + void print_product_ids(); private: /// @brief Holds info about which functionality has been successfully initialized (used by deconstructor during cleanup). typedef struct bno08x_init_status_t { - bool gpio_outputs; ///< True if GPIO outputs have been initialized. - bool gpio_inputs; ///< True if GPIO inputs have been initialized. - bool isr_service; ///< True if global ISR service has been initialized. - bool isr_handler; ///< True if HINT ISR handler has been initialized. - uint8_t task_count; ///< How many successfully initialized tasks (max of TSK_CNT) - bool data_proc_task; ///< True if xTaskCreate has been called successfully for data_proc_task. - bool spi_task; ///< True if xTaskCreate has been called successfully for spi_task. - bool spi_bus; ///< True if spi_bus_initialize() has been called successfully. - bool spi_device; ///< True if spi_bus_add_device() has been called successfully. - bool sh2_HAL; ///< True if sh2_open() has been called successfully. + bool gpio_outputs; ///< True if GPIO outputs have been initialized. + bool gpio_inputs; ///< True if GPIO inputs have been initialized. + bool isr_service; ///< True if global ISR service has been initialized. + bool isr_handler; ///< True if HINT ISR handler has been initialized. + bool data_proc_task; ///< True if xTaskCreate has been called successfully for data_proc_task. + bool sh2_HAL_service_task; ///< True if xTaskCreate has been called successfully for sh2_HAL_service_task. + bool spi_bus; ///< True if spi_bus_initialize() has been called successfully. + bool spi_device; ///< True if spi_bus_add_device() has been called successfully. + bool sh2_HAL; ///< True if sh2_open() has been called successfully. bno08x_init_status_t() : gpio_outputs(false) , gpio_inputs(false) , isr_service(false) , isr_handler(false) - , task_count(0) , data_proc_task(false) - , spi_task(false) , spi_bus(false) , spi_device(false) { } } bno08x_init_status_t; + /// @brief Holds data returned from sensor reports. + typedef struct bno08x_data_t + { + sh2_Accelerometer_t gravity; + sh2_Accelerometer_t linear_acceleration; + } bno08x_data_t; + + typedef struct bno08x_usr_report_periods_t + { + uint32_t gravity; + uint32_t linear_accelerometer; + } bno08x_usr_report_periods_t; + + bno08x_data_t data; ///< Holds all data returned from enabled reports. + bno08x_usr_report_periods_t user_report_periods; ///< Holds periods for reports enabled by user (0 == disabled report) + + // data processing task + TaskHandle_t data_proc_task_hdl; ///> cb_list; // Vector for storing any call-back functions added with register_cb() @@ -102,8 +160,7 @@ class BNO08x bno08x_init_status_t init_status; ///data_proc_task(); // launch data processing task task from object +} + +/** + * @brief Task responsible for handling sensor events sent by SH2 HAL. + * + * @return void, nothing to return + */ +void BNO08x::data_proc_task() +{ + sh2_SensorEvent_t sensor_evt; + sh2_SensorValue_t sensor_val; + + while (1) + { + if (xQueueReceive(queue_rx_sensor_event, &sensor_evt, portMAX_DELAY) == pdTRUE) + { + if (sh2_decodeSensorEvent(&sensor_val, &sensor_evt) != SH2_ERR) + handle_sensor_report(&sensor_val); + } + } +} + +void BNO08x::sh2_HAL_service_task_trampoline(void* arg) +{ + BNO08x* imu = (BNO08x*) arg; // cast argument received by xTaskCreate ("this" pointer to imu object created by constructor call) + imu->sh2_HAL_service_task(); // launch data processing task task from object +} + +void BNO08x::sh2_HAL_service_task() +{ + while (1) + { + xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_HINT_ASSERTED_BIT, pdFALSE, pdFALSE, portMAX_DELAY); + + if (reset_occurred) + { + reset_occurred = false; + re_enable_reports(); + } + + xSemaphoreTake(sh2_HAL_lock, portMAX_DELAY); + sh2_service(); + xSemaphoreGive(sh2_HAL_lock); + } +} + +void BNO08x::handle_sensor_report(sh2_SensorValue_t* sensor_val) +{ + switch (sensor_val->sensorId) + { + case SH2_GRAVITY: + update_gravity_data(sensor_val); + ESP_LOGW(TAG, "grav: %.3lf %.3lf %.3lf", sensor_val->un.gravity.x, sensor_val->un.gravity.y, sensor_val->un.gravity.z); + break; + + case SH2_LINEAR_ACCELERATION: + update_linear_accelerometer_data(sensor_val); + ESP_LOGW(TAG, "accl: %.3lf %.3lf %.3lf", sensor_val->un.linearAcceleration.x, sensor_val->un.linearAcceleration.y, + sensor_val->un.linearAcceleration.z); + break; + + default: + + break; + } +} + +/** + * @brief Updates gravity data from decoded sensor event. + * + * @param sensor_val The sh2_SensorValue_t struct used in sh2_decodeSensorEvent() call. + * + * @return void, nothing to return + */ +void BNO08x::update_gravity_data(sh2_SensorValue_t* sensor_val) +{ + data.gravity.x = sensor_val->un.gravity.x; + data.gravity.y = sensor_val->un.gravity.y; + data.gravity.z = sensor_val->un.gravity.z; +} + +/** + * @brief Updates linear accelerometer data from decoded sensor event. + * + * @param sensor_val The sh2_SensorValue_t struct used in sh2_decodeSensorEvent() call. + * + * @return void, nothing to return + */ +void BNO08x::update_linear_accelerometer_data(sh2_SensorValue_t* sensor_val) +{ + data.linear_acceleration.x = sensor_val->un.linearAcceleration.x; + data.linear_acceleration.y = sensor_val->un.linearAcceleration.y; + data.linear_acceleration.z = sensor_val->un.linearAcceleration.z; +} + /** * @brief Initializes required esp-idf SPI data structures with values from user passed bno08x_config_t struct. * @@ -330,6 +453,57 @@ esp_err_t BNO08x::init_hint_isr() return ret; } +/** + * @brief Initializes data_proc_task. + * + * @return ESP_OK if initialization was success. + */ +esp_err_t BNO08x::init_tasks() +{ + BaseType_t task_created = pdFALSE; + + vTaskDelay(100 / portTICK_PERIOD_MS); + + // launch data processing task + task_created = xTaskCreate( + &data_proc_task_trampoline, "bno08x_data_processing_task", CONFIG_ESP32_BNO08X_DATA_PROC_TASK_SZ, this, 7, &data_proc_task_hdl); + + if (task_created != pdTRUE) + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Initialization failed, data_proc_task failed to launch."); + #endif + // clang-format on + + return ESP_FAIL; + } + else + { + init_status.data_proc_task = true; + } + + // launch data processing task + task_created = xTaskCreate(&sh2_HAL_service_task_trampoline, "bno08x_sh2_HAL_service_task", 4095U, this, 7, &sh2_HAL_service_task_hdl); + + if (task_created != pdTRUE) + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Initialization failed, sh2_HAL_service_task failed to launch."); + #endif + // clang-format on + + return ESP_FAIL; + } + else + { + init_status.sh2_HAL_service_task = true; + } + + return ESP_OK; +} + /** * @brief Initializes SPI. * @@ -396,14 +570,30 @@ esp_err_t BNO08x::init_sh2_HAL() hard_reset(); if (sh2_open(&sh2_HAL, BNO08xSH2HAL::hal_cb, NULL) != SH2_OK) + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Initialization failed, sh2_open() call failed."); + #endif + // clang-format on + return ESP_FAIL; + } init_status.sh2_HAL = true; - memset(&product_ID, 0, sizeof(product_ID)); + memset(&product_IDs, 0, sizeof(product_IDs)); + + if (sh2_getProdIds(&product_IDs) != SH2_OK) + { + // clang-format off + #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS + ESP_LOGE(TAG, "Initialization failed, sh2_getProdIds() call failed."); + #endif + // clang-format on - if (sh2_getProdIds(&product_ID) != SH2_OK) return ESP_FAIL; + } // clang-format off #ifdef CONFIG_ESP32_BNO08x_LOG_STATEMENTS @@ -411,7 +601,8 @@ esp_err_t BNO08x::init_sh2_HAL() #endif // clang-format on - sh2_setSensorCallback(BNO08xSH2HAL::sensor_report_cb, NULL); + if (sh2_setSensorCallback(BNO08xSH2HAL::sensor_event_cb, NULL) != SH2_OK) + return ESP_FAIL; return ESP_OK; } @@ -593,6 +784,22 @@ esp_err_t BNO08x::deinit_spi() return ret; } +/** + * @brief Deinitializes tasks used by BNO08x driver. + * + * @return ESP_OK if deinitialization was success. + */ +esp_err_t BNO08x::deinit_tasks() +{ + if (init_status.data_proc_task) + vTaskDelete(data_proc_task_hdl); + + if (init_status.sh2_HAL_service_task) + vTaskDelete(sh2_HAL_service_task_hdl); + + return ESP_OK; +} + /** * @brief Deinitializes sh2 HAL. * @@ -627,6 +834,95 @@ void BNO08x::hard_reset() gpio_set_level(imu_config.io_rst, 1); // bring out of reset } +/** + * @brief Sends command to enable gravity reports. (See Ref. Manual 6.5.11) + * + * @param report_period_us The period/interval of the report in microseconds. + * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults. + * + * @return ESP_OK if report was successfully enabled. + */ +bool BNO08x::enable_gravity(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg) +{ + if (enable_report(SH2_GRAVITY, time_between_reports, sensor_cfg) != ESP_OK) + { + return false; + } + else + { + ESP_LOGE(TAG, "ENABLED"); + user_report_periods.gravity = time_between_reports; + xEventGroupSetBits(evt_grp_report_en, EVT_GRP_RPT_GRAVITY_BIT_EN); + return true; + } +} + +/** + * @brief Sends command to enable linear accelerometer reports. (See Ref. Manual 6.5.10) + * + * @param report_period_us The period/interval of the report in microseconds. + * @param sensor_cfg Sensor special configuration (optional), see default_sensor_cfg for defaults. + * + * @return ESP_OK if report was successfully enabled. + */ +bool BNO08x::enable_linear_accelerometer(uint32_t time_between_reports, sh2_SensorConfig_t sensor_cfg) +{ + if (enable_report(SH2_LINEAR_ACCELERATION, time_between_reports, sensor_cfg) != ESP_OK) + { + return false; + } + else + { + user_report_periods.linear_accelerometer = time_between_reports; + xEventGroupSetBits(evt_grp_report_en, EVT_GRP_RPT_LINEAR_ACCELEROMETER_BIT_EN); + return true; + } +} + +void BNO08x::get_gravity(float& x, float& y, float& z) +{ + x = data.gravity.x; + y = data.gravity.y; + z = data.gravity.z; +} + +float BNO08x::get_gravity_X() +{ + return data.gravity.x; +} + +float BNO08x::get_gravity_Y() +{ + return data.gravity.y; +} + +float BNO08x::get_gravity_Z() +{ + return data.gravity.z; +} + +void BNO08x::get_linear_accel(float& x, float& y, float& z) +{ + x = data.linear_acceleration.x; + y = data.linear_acceleration.y; + z = data.linear_acceleration.z; +} + +float BNO08x::get_linear_accel_X() +{ + return data.linear_acceleration.x; +} + +float BNO08x::get_linear_accel_Y() +{ + return data.linear_acceleration.y; +} + +float BNO08x::get_linear_accel_Z() +{ + return data.linear_acceleration.z; +} + /** * @brief Waits for HINT pin assertion or HOST_INT_TIMEOUT_DEFAULT_MS to elapse. * @@ -645,6 +941,59 @@ 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) +{ + + static sh2_SensorConfig_t config; + + xSemaphoreTake(sh2_HAL_lock, portMAX_DELAY); + // These sensor options are disabled or not used in most cases + config.changeSensitivityEnabled = false; + config.wakeupEnabled = false; + config.changeSensitivityRelative = false; + config.alwaysOnEnabled = false; + config.changeSensitivity = 0; + config.batchInterval_us = 0; + config.sensorSpecific = 0; + + config.reportInterval_us = time_between_reports; + + if (sh2_setSensorConfig(sensor_ID, &config) != SH2_OK) + { + xSemaphoreGive(sh2_HAL_lock); + return ESP_FAIL; + } + else + { + xSemaphoreGive(sh2_HAL_lock); + return ESP_OK; + } +} + +esp_err_t BNO08x::re_enable_reports() +{ + EventBits_t report_en_bits = xEventGroupGetBits(evt_grp_report_en); + + if (report_en_bits & EVT_GRP_RPT_GRAVITY_BIT_EN) + if (!enable_gravity(user_report_periods.gravity)) + return ESP_FAIL; + + if (report_en_bits & EVT_GRP_RPT_LINEAR_ACCELEROMETER_BIT_EN) + if (!enable_linear_accelerometer(user_report_periods.linear_accelerometer)) + return ESP_FAIL; + + return ESP_OK; +} + /** * @brief Registers a callback to execute when new data from a report is received. * @@ -664,7 +1013,7 @@ void BNO08x::register_cb(std::function cb_fxn) */ void BNO08x::print_product_ids() { - for (int i = 0; i < product_ID.numEntries; i++) + for (int i = 0; i < product_IDs.numEntries; i++) { ESP_LOGI(TAG, "Product ID %d Info: \n\r" @@ -675,8 +1024,8 @@ void BNO08x::print_product_ids() " SW Build Number: 0x%" PRIx32 "\n\r" " SW Version Patch: 0x%" PRIx16 "\n\r" " ---------------------------\n\r", - i, product_ID.entry->swPartNumber, product_ID.entry->swVersionMajor, product_ID.entry->swVersionMinor, - product_ID.entry->swBuildNumber, product_ID.entry->swVersionPatch); + i, product_IDs.entry->swPartNumber, product_IDs.entry->swVersionMajor, product_IDs.entry->swVersionMinor, + product_IDs.entry->swBuildNumber, product_IDs.entry->swVersionPatch); } } diff --git a/source/BNO08xSH2HAL.cpp b/source/BNO08xSH2HAL.cpp index 717c0d6..0671af4 100644 --- a/source/BNO08xSH2HAL.cpp +++ b/source/BNO08xSH2HAL.cpp @@ -84,9 +84,9 @@ void BNO08xSH2HAL::hal_cb(void* cookie, sh2_AsyncEvent_t* pEvent) imu->reset_occurred = true; } -void BNO08xSH2HAL::sensor_report_cb(void* cookie, sh2_SensorEvent_t* event) +void BNO08xSH2HAL::sensor_event_cb(void* cookie, sh2_SensorEvent_t* event) { - sh2_decodeSensorEvent(imu->sensor_report_val, event); + xQueueSend(imu->queue_rx_sensor_event, event, 0); } void BNO08xSH2HAL::hardware_reset()