diff --git a/include/BNO08x.hpp b/include/BNO08x.hpp index 9b98bc0..13d6e6d 100644 --- a/include/BNO08x.hpp +++ b/include/BNO08x.hpp @@ -21,7 +21,7 @@ #include #include -//macros +// macros #define CHECK_TASKS_RUNNING(evt_grp_task_flow, running_bit) ((xEventGroupGetBits(evt_grp_task_flow) & (running_bit)) != 0) /// @brief SHTP protocol channels @@ -43,6 +43,17 @@ enum class IMUAccuracy HIGH }; +/// @brief Reason for previous IMU reset (returned by get_reset_reason()) +enum class IMUResetReason +{ + UNDEFINED, ///< Undefined reset reason, this should never occur and is an error. + POR, ///< Previous reset was due to power on reset. + INT_RST, ///< Previous reset was due to internal reset. + WTD, ///< Previous reset was due to watchdog timer. + EXT_RST, ///< Previous reset was due to external reset. + OTHER ///< Previous reset was due to power other reason. +}; + /// @brief IMU configuration settings passed into constructor typedef struct bno08x_config_t { @@ -63,14 +74,14 @@ typedef struct bno08x_config_t bno08x_config_t(bool install_isr_service = true) : spi_peripheral((spi_host_device_t) CONFIG_ESP32_BNO08x_SPI_HOST) , io_mosi(static_cast(CONFIG_ESP32_BNO08X_GPIO_DI)) // default: 23 - , io_miso(static_cast( CONFIG_ESP32_BNO08X_GPIO_SDA)) // default: 19 - , io_sclk(static_cast( CONFIG_ESP32_BNO08X_GPIO_SCL)) // default: 18 + , io_miso(static_cast(CONFIG_ESP32_BNO08X_GPIO_SDA)) // default: 19 + , io_sclk(static_cast(CONFIG_ESP32_BNO08X_GPIO_SCL)) // default: 18 , io_cs(static_cast(CONFIG_ESP32_BNO08X_GPIO_CS)) // default: 33 , io_int(static_cast(CONFIG_ESP32_BNO08X_GPIO_HINT)) // default: 26 , io_rst(static_cast(CONFIG_ESP32_BNO08X_GPIO_RST)) // default: 32 - , io_wake(static_cast( CONFIG_ESP32_BNO08X_GPIO_WAKE)) // default: -1 (unused) + , io_wake(static_cast(CONFIG_ESP32_BNO08X_GPIO_WAKE)) // default: -1 (unused) , sclk_speed(static_cast(CONFIG_ESP32_BNO08X_SCL_SPEED_HZ)) // default: 2MHz - , install_isr_service(install_isr_service) // default: true + , install_isr_service(install_isr_service) // default: true { } @@ -96,12 +107,12 @@ class BNO08x { public: BNO08x(bno08x_config_t imu_config = bno08x_config_t()); - ~BNO08x(); + ~BNO08x(); bool initialize(); bool hard_reset(); bool soft_reset(); - uint8_t get_reset_reason(); + IMUResetReason get_reset_reason(); bool mode_sleep(); bool mode_on(); @@ -255,36 +266,36 @@ class BNO08x // Record IDs from figure 29, page 29 reference manual // These are used to read the metadata for each sensor type static const constexpr uint16_t FRS_RECORD_ID_ACCELEROMETER = - 0xE302; ///< Accelerometer record ID, to be passed in metadata functions like get_Q1() + 0xE302U; ///< Accelerometer record ID, to be passed in metadata functions like get_Q1() static const constexpr uint16_t FRS_RECORD_ID_GYROSCOPE_CALIBRATED = - 0xE306; ///< Calirated gyroscope record ID, to be passed in metadata functions like get_Q1() + 0xE306U; ///< Calirated gyroscope record ID, to be passed in metadata functions like get_Q1() static const constexpr uint16_t FRS_RECORD_ID_MAGNETIC_FIELD_CALIBRATED = - 0xE309; ///< Calibrated magnetometer record ID, to be passed in metadata functions like get_Q1() + 0xE309U; ///< Calibrated magnetometer record ID, to be passed in metadata functions like get_Q1() static const constexpr uint16_t FRS_RECORD_ID_ROTATION_VECTOR = - 0xE30B; ///< Rotation vector record ID, to be passed in metadata functions like get_Q1() + 0xE30BU; ///< Rotation vector record ID, to be passed in metadata functions like get_Q1() // Activity classifier bits - static const constexpr uint16_t ACTIVITY_CLASSIFIER_UNKNOWN_EN = (1 << 0); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_IN_VEHICLE_EN = (1 << 1); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_BICYCLE_EN = (1 << 2); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_FOOT_EN = (1 << 3); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_STILL_EN = (1 << 4); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_TILTING_EN = (1 << 5); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_WALKING_EN = (1 << 6); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_RUNNING_EN = (1 << 7); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_STAIRS_EN = (1 << 8); - static const constexpr uint16_t ACTIVITY_CLASSIFIER_ALL_EN = 0x1F; + static const constexpr uint16_t ACTIVITY_CLASSIFIER_UNKNOWN_EN = (1U << 0U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_IN_VEHICLE_EN = (1U << 1U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_BICYCLE_EN = (1U << 2U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_FOOT_EN = (1U << 3U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_STILL_EN = (1U << 4U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_TILTING_EN = (1U << 5U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_WALKING_EN = (1U << 6U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_RUNNING_EN = (1U << 7U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_STAIRS_EN = (1U << 8U); + static const constexpr uint16_t ACTIVITY_CLASSIFIER_ALL_EN = 0x1FU; - static const constexpr uint8_t TARE_AXIS_ALL = 0x07; ///< Tare all axes (used with tare now command) - static const constexpr uint8_t TARE_AXIS_Z = 0x04; ///< Tar yaw axis only (used with tare now command) + static const constexpr uint8_t TARE_AXIS_ALL = 0x07U; ///< Tare all axes (used with tare now command) + static const constexpr uint8_t TARE_AXIS_Z = 0x04U; ///< Tar yaw axis only (used with tare now command) // Which rotation vector to tare, BNO08x saves them seperately - static const constexpr uint8_t TARE_ROTATION_VECTOR = 0; ///(CONFIG_ESP32_BNO08X_SPI_QUEUE_SZ); // set max allowable queued SPI transactions + imu_spi_config.queue_size = static_cast(CONFIG_ESP32_BNO08X_SPI_QUEUE_SZ); // set max allowable queued SPI transactions - return ESP_OK; + return ESP_OK; } -esp_err_t BNO08x::initialize_gpio() +esp_err_t BNO08x::init_gpio_inputs() { esp_err_t ret = ESP_OK; - /*GPIO config for pins not controlled by SPI peripheral*/ + // configure input(s) (HINT) + gpio_config_t inputs_config; + inputs_config.pin_bit_mask = (1ULL << imu_config.io_int); + inputs_config.mode = GPIO_MODE_INPUT; + inputs_config.pull_up_en = GPIO_PULLUP_DISABLE; + inputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE; + inputs_config.intr_type = GPIO_INTR_NEGEDGE; + + ret = gpio_config(&inputs_config); + + if (ret != ESP_OK) + ESP_LOGE(TAG, "Initialization failed, failed to configure HINT gpio."); + else + init_status.gpio_inputs = true; // set gpio_inputs to initialized such that deconstructor knows to clean them up + + return ret; +} + +esp_err_t BNO08x::init_gpio_outputs() +{ + esp_err_t ret = ESP_OK; // configure output(s) (CS, RST, and WAKE) gpio_config_t outputs_config; @@ -171,26 +199,26 @@ esp_err_t BNO08x::initialize_gpio() ret = gpio_config(&outputs_config); if (ret != ESP_OK) - { - ESP_LOGE(TAG, "Failed to configure CS, RST, and WAKE (if used) gpio."); - return ret; - } + ESP_LOGE(TAG, "Initialization failed, failed to configure CS, RST, and WAKE (if used) gpio."); + else + init_status.gpio_outputs = true; // set gpio_inputs to initialized such that deconstructor knows to clean them up - //configure input(s) (HINT) - gpio_config_t inputs_config; - inputs_config.pin_bit_mask = (1ULL << imu_config.io_int); - inputs_config.mode = GPIO_MODE_INPUT; - inputs_config.pull_up_en = GPIO_PULLUP_DISABLE; - inputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE; - inputs_config.intr_type = GPIO_INTR_NEGEDGE; + return ret; +} - ret = gpio_config(&inputs_config); +esp_err_t BNO08x::init_gpio() +{ + esp_err_t ret = ESP_OK; + /*GPIO config for pins not controlled by SPI peripheral*/ + + ret = init_gpio_outputs(); + if (ret != ESP_OK) + return ret; + + ret = init_gpio_inputs(); if (ret != ESP_OK) - { - ESP_LOGE(TAG, "Failed to configure HINT gpio."); return ret; - } gpio_set_level(imu_config.io_cs, 1); gpio_set_level(imu_config.io_rst, 1); @@ -201,7 +229,7 @@ esp_err_t BNO08x::initialize_gpio() return ret; } -esp_err_t BNO08x::initialize_hint_isr() +esp_err_t BNO08x::init_hint_isr() { esp_err_t ret = ESP_OK; @@ -211,23 +239,32 @@ esp_err_t BNO08x::initialize_hint_isr() if (ret != ESP_OK) { - ESP_LOGE(TAG, "Failed to install global ISR service."); + ESP_LOGE(TAG, "Initialization failed, failed to install global ISR service."); return ret; } + else + { + init_status.isr_service = true; // set isr service to initialized such that deconstructor knows to clean it up (this will be ignored if + // imu_config.install_isr_service == false) + } ret = gpio_isr_handler_add(imu_config.io_int, hint_handler, (void*) this); if (ret != ESP_OK) { - ESP_LOGE(TAG, "Add hint_handler ISR."); + ESP_LOGE(TAG, "Initialization failed, failed to add hint_handler ISR."); return ret; } + else + { + init_status.isr_handler = true; // set isr handler to initialized such that deconstructor knows to clean it up + } gpio_intr_disable(imu_config.io_int); // disable interrupts initially before reset return ret; } -esp_err_t BNO08x::initialize_spi() +esp_err_t BNO08x::init_spi() { esp_err_t ret = ESP_OK; uint8_t tx_buffer[50] = {0}; // for dummy transaction to stabilize SPI peripheral @@ -236,17 +273,25 @@ esp_err_t BNO08x::initialize_spi() ret = spi_bus_initialize(imu_config.spi_peripheral, &bus_config, SPI_DMA_CH_AUTO); if (ret != ESP_OK) { - ESP_LOGE(TAG, "SPI bus failed to initialize."); + ESP_LOGE(TAG, "Initialization failed, SPI bus failed to initialize."); return ret; } + else + { + init_status.spi_bus = true; + } // add the imu device to the bus ret = spi_bus_add_device(imu_config.spi_peripheral, &imu_spi_config, &spi_hdl); if (ret != ESP_OK) { - ESP_LOGE(TAG, "Failed to add device to SPI bus."); + ESP_LOGE(TAG, "Initialization failed, failed to add device to SPI bus."); return ret; } + else + { + init_status.spi_device = true; + } // do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.) spi_transaction.length = 8; @@ -259,6 +304,122 @@ esp_err_t BNO08x::initialize_spi() return ret; } +esp_err_t BNO08x::deinit_gpio() +{ + esp_err_t ret = ESP_OK; + + if (init_status.gpio_inputs) + { + ret = deinit_gpio_inputs(); + if (ret != ESP_OK) + return ret; + } + + if (init_status.gpio_outputs) + { + ret = deinit_gpio_outputs(); + if (ret != ESP_OK) + return ret; + } + + return ret; +} + +esp_err_t BNO08x::deinit_gpio_inputs() +{ + esp_err_t ret = ESP_OK; + + ret = gpio_reset_pin(imu_config.io_int); + if (ret != ESP_OK) + ESP_LOGE(TAG, "Deconstruction failed, could reset gpio HINT pin to default state."); + + return ret; +} + +esp_err_t BNO08x::deinit_gpio_outputs() +{ + esp_err_t ret = ESP_OK; + + if (imu_config.io_wake != GPIO_NUM_NC) + { + ret = gpio_reset_pin(imu_config.io_wake); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could reset gpio WAKE pin to default state."); + return ret; + } + } + + ret = gpio_reset_pin(imu_config.io_cs); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could reset gpio CS pin to default state."); + return ret; + } + + ret = gpio_reset_pin(imu_config.io_rst); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could reset gpio RST pin to default state."); + return ret; + } + + return ret; +} + +esp_err_t BNO08x::deinit_hint_isr() +{ + esp_err_t ret = ESP_OK; + + if (init_status.isr_handler) + { + ret = gpio_isr_handler_remove(imu_config.io_int); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could not remove hint ISR handler."); + return ret; + } + } + + if (init_status.isr_service) + { + // only remove the ISR service if it was requested to be installed by user + if (imu_config.install_isr_service) + { + gpio_uninstall_isr_service(); + } + } + + return ret; +} + +esp_err_t BNO08x::deinit_spi() +{ + esp_err_t ret = ESP_OK; + + if (init_status.spi_device) + { + ret = spi_bus_remove_device(spi_hdl); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could not remove spi device."); + return ret; + } + } + + if (init_status.spi_bus) + { + ret = spi_bus_free(imu_config.spi_peripheral); + if (ret != ESP_OK) + { + ESP_LOGE(TAG, "Deconstruction failed, could free SPI peripheral."); + return ret; + } + } + + return ret; +} + /** * @brief Waits for data to be received over SPI, or HOST_INT_TIMEOUT_MS to elapse. * @@ -446,7 +607,7 @@ bool BNO08x::soft_reset() * @return The reason for the most recent recent reset ( 1 = POR (power on reset), 2 = internal reset, 3 = watchdog * timer, 4 = external reset 5 = other) */ -uint8_t BNO08x::get_reset_reason() +IMUResetReason BNO08x::get_reset_reason() { uint32_t reset_reason = 0; @@ -464,7 +625,7 @@ uint8_t BNO08x::get_reset_reason() ESP_LOGE(TAG, "Failed to receive product ID report."); } - return reset_reason; + return static_cast(reset_reason); } /** @@ -3002,18 +3163,31 @@ esp_err_t BNO08x::launch_tasks() 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) - task_created = xTaskCreate(&spi_task_trampoline, "bno08x_spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task - if (task_created != pdTRUE) { - ESP_LOGE(TAG, "Tasks failed to launch."); + ESP_LOGE(TAG, "Initialization failed, data_proc_task failed to launch."); return ESP_ERR_INVALID_STATE; } else { - return ESP_OK; + init_status.data_proc_task = true; + init_status.task_count++; } + + task_created = xTaskCreate(&spi_task_trampoline, "bno08x_spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task + + if (task_created != pdTRUE) + { + ESP_LOGE(TAG, "Initialization failed, spi_task failed to launch."); + return ESP_ERR_INVALID_STATE; + } + else + { + init_status.spi_task = true; + init_status.task_count++; + } + + return ESP_OK; } esp_err_t BNO08x::kill_all_tasks() @@ -3024,17 +3198,24 @@ esp_err_t BNO08x::kill_all_tasks() xEventGroupClearBits( evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT); // clear task running bit in task flow event group to request deletion of tasks - xTaskNotifyGive(spi_task_hdl); // notify spi task for self deletion - xQueueSend(queue_rx_data, &dummy_packet, 0); // send a dummy packet to wake up data_proc task for self-deletion - for (uint8_t i = 0; i < TASK_CNT; i++) - if (xSemaphoreTake(sem_kill_tasks, TASK_DELETE_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE) - kill_count++; - - if (kill_count != TASK_CNT) + if (init_status.task_count != 0) { - ESP_LOGE(TAG, "Task deletion timed out in deconstructor call."); - return ESP_ERR_TIMEOUT; + if (init_status.spi_task) + xTaskNotifyGive(spi_task_hdl); // notify spi task for self deletion + + if (init_status.data_proc_task) + xQueueSend(queue_rx_data, &dummy_packet, 0); // send a dummy packet to wake up data_proc task for self-deletion + + for (uint8_t i = 0; i < init_status.task_count; i++) + if (xSemaphoreTake(sem_kill_tasks, TASK_DELETE_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE) + kill_count++; + + if (kill_count != init_status.task_count) + { + ESP_LOGE(TAG, "Task deletion timed out in deconstructor call."); + return ESP_ERR_TIMEOUT; + } } return ESP_OK; diff --git a/tests/BNO08xTestHelper.cpp b/tests/BNO08xTestHelper.cpp index 24f64cc..243f3b7 100644 --- a/tests/BNO08xTestHelper.cpp +++ b/tests/BNO08xTestHelper.cpp @@ -1 +1,13 @@ -#include "BNO08xTestHelper.hpp" \ No newline at end of file +#include "BNO08xTestHelper.hpp" + +BNO08x* BNO08xTestHelper::test_imu = nullptr; + +void BNO08xTestHelper::set_test_imu(BNO08x *imu) +{ + test_imu = imu; +} + +BNO08x* BNO08xTestHelper::get_test_imu() +{ + return test_imu; +} \ No newline at end of file diff --git a/tests/BNO08xTestHelper.hpp b/tests/BNO08xTestHelper.hpp index ddc2b63..d116bf4 100644 --- a/tests/BNO08xTestHelper.hpp +++ b/tests/BNO08xTestHelper.hpp @@ -5,5 +5,9 @@ class BNO08xTestHelper { - + public: + static void set_test_imu(BNO08x *imu); + static BNO08x* get_test_imu(); + private: + static BNO08x *test_imu; }; \ No newline at end of file diff --git a/tests/BNO08xTestSuite.cpp b/tests/BNO08xTestSuite.cpp index 31c1031..2652b07 100644 --- a/tests/BNO08xTestSuite.cpp +++ b/tests/BNO08xTestSuite.cpp @@ -2,5 +2,8 @@ void BNO08xTestSuite::run_tests() { - + BNO08x imu; + + BNO08xTestHelper::set_test_imu(&imu); + } \ No newline at end of file