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

initialization refactoring

This commit is contained in:
myles-parfeniuk 2024-11-13 22:42:21 -08:00
parent a33a53dea5
commit 343c406325
3 changed files with 371 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Kconfig.projbuild
View File

@ -71,6 +71,14 @@ menu "esp32_BNO08x"
help
SPI clock speed in Hz, default 2MHz.
config ESP32_BNO08X_SPI_QUEUE_SZ
int "Callback queue size."
range 1 100
default 5
help
Amount of SPI transactions that can be queued during operation.
This argument is loaded directly into the spi_device_interface_config_t struct used for SPI configuration.
endmenu #SPI Configuration
config ESP32_BNO08X_DATA_PROC_TASK_SZ
@ -82,9 +90,9 @@ menu "esp32_BNO08x"
Note that callbacks should remain as short as possible, pass the data out of
the callback with a queue or save it to different variables for longer operations.
config ESP32_BNO08x_DEBUG_STATEMENTS
bool "Print debug statements."
default "n"
help
Print the various debug statements scattered throughout the code when running.
config ESP32_BNO08x_DEBUG_STATEMENTS
bool "Print debug statements."
default "n"
help
Print the various debug statements scattered throughout the code when running.
endmenu

74
include/BNO08x.hpp
View File

@ -21,6 +21,9 @@
#include <functional>
#include <vector>
//macros
#define CHECK_TASKS_RUNNING(evt_grp_task_flow, running_bit) ((xEventGroupGetBits(evt_grp_task_flow) & (running_bit)) != 0)
/// @brief SHTP protocol channels
enum channels_t
{
@ -52,27 +55,29 @@ typedef struct bno08x_config_t
gpio_num_t io_rst; /// Reset pin (connects to BNO08x RST pin)
gpio_num_t io_wake; ///<Wake pin (optional, connects to BNO08x P0)
uint32_t sclk_speed; ///<Desired SPI SCLK speed in Hz (max 3MHz)
bool install_isr_service; ///<Indicates whether the ISR service for the HINT should be installed at IMU initialization, (if gpio_install_isr_service() is called before initialize() set this to false)
/// @brief Default IMU configuration settings constructor.
/// To modify default GPIO pins, run "idf.py menuconfig" esp32_BNO08x->GPIO Configuration.
/// Alternatively, edit the default values in "Kconfig.projbuild"
bno08x_config_t()
bno08x_config_t(bool install_isr_service = true)
: spi_peripheral((spi_host_device_t) CONFIG_ESP32_BNO08x_SPI_HOST)
, io_mosi((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_DI) // default: 23
, io_miso((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_SDA) // default: 19
, io_sclk((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_SCL) // default: 18
, io_cs((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_CS) // default: 33
, io_int((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_HINT) // default: 26
, io_rst((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_RST) // default: 32
, io_wake((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_WAKE) // default: -1 (unused)
, sclk_speed((uint32_t) CONFIG_ESP32_BNO08X_SCL_SPEED_HZ) // default: 2MHz
, io_mosi(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_DI)) // default: 23
, io_miso(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_SDA)) // default: 19
, io_sclk(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_SCL)) // default: 18
, io_cs(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_CS)) // default: 33
, io_int(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_HINT)) // default: 26
, io_rst(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_RST)) // default: 32
, io_wake(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_WAKE)) // default: -1 (unused)
, sclk_speed(static_cast<uint32_t>(CONFIG_ESP32_BNO08X_SCL_SPEED_HZ)) // default: 2MHz
, install_isr_service(install_isr_service) // default: true
{
}
/// @brief Overloaded IMU configuration settings constructor for custom pin settings
bno08x_config_t(spi_host_device_t spi_peripheral, gpio_num_t io_mosi, gpio_num_t io_miso, gpio_num_t io_sclk, gpio_num_t io_cs,
gpio_num_t io_int, gpio_num_t io_rst, gpio_num_t io_wake, uint32_t sclk_speed)
gpio_num_t io_int, gpio_num_t io_rst, gpio_num_t io_wake, uint32_t sclk_speed, bool install_isr_service = true)
: spi_peripheral(spi_peripheral)
, io_mosi(io_mosi)
, io_miso(io_miso)
@ -82,6 +87,7 @@ typedef struct bno08x_config_t
, io_rst(io_rst)
, io_wake(io_wake)
, sclk_speed(sclk_speed)
, install_isr_service(install_isr_service)
{
}
} bno08x_config_t;
@ -305,6 +311,11 @@ class BNO08x
uint16_t length; ///< Packet length in bytes.
} bno08x_tx_packet_t;
esp_err_t initialize_config_args();
esp_err_t initialize_gpio();
esp_err_t initialize_hint_isr();
esp_err_t initialize_spi();
bool wait_for_rx_done();
bool wait_for_tx_done();
bool wait_for_data();
@ -329,9 +340,25 @@ class BNO08x
void print_header(bno08x_rx_packet_t* packet);
void print_packet(bno08x_rx_packet_t* packet);
// spi task
TaskHandle_t spi_task_hdl; ///<spi_task() handle
static void spi_task_trampoline(void* arg);
void spi_task();
// data processing task
TaskHandle_t data_proc_task_hdl; ///<data_proc_task() task handle
static void data_proc_task_trampoline(void* arg);
void data_proc_task();
// task control
SemaphoreHandle_t sem_kill_tasks; ///<semaphore to count amount of killed tasks
esp_err_t launch_tasks();
esp_err_t kill_all_tasks();
EventGroupHandle_t
evt_grp_spi; ///<Event group for indicating when bno08x hint pin has triggered and when new data has been processed. Used by calls to sending or receiving functions.
EventGroupHandle_t evt_grp_report_en; ///<Event group for indicating which reports are currently enabled.
EventGroupHandle_t evt_grp_task_flow; ///<Event group for indicating when tasks should complete and self-delete (on deconstructor call)
QueueHandle_t queue_rx_data; ///<Packet queue used to send data received from bno08x from spi_task to data_proc_task.
QueueHandle_t queue_tx_data; ///<Packet queue used to send data to be sent over SPI from sending functions to spi_task.
@ -378,22 +405,7 @@ class BNO08x
uint16_t mems_raw_magf_X, mems_raw_magf_Y,
mems_raw_magf_Z; ///<Raw magnetometer (compass) readings from MEMS sensor (See SH-2 Ref. Manual 6.5.15)
// spi task
TaskHandle_t spi_task_hdl; ///<spi_task() handle
static void spi_task_trampoline(void* arg);
void spi_task();
// data processing task
TaskHandle_t data_proc_task_hdl; ///<data_proc_task() task handle
static void data_proc_task_trampoline(void* arg);
void data_proc_task();
bool kill_tasks; ///<indicates to spi_task and data_proc task that deconstructor wants them to self-delete
SemaphoreHandle_t sem_kill_tasks;
static void IRAM_ATTR hint_handler(void* arg);
static bool
isr_service_installed; ///<true of the isr service has been installed, only has to be done once regardless of how many devices are used
static const constexpr uint16_t RX_DATA_LENGTH = 300; ///<length buffer containing data received over spi
static const constexpr uint16_t MAX_METADATA_LENGTH = 9; ///<max length of metadata used in frs read operations
@ -401,16 +413,17 @@ class BNO08x
static const constexpr uint64_t HOST_INT_TIMEOUT_MS =
300ULL; ///<Max wait between HINT being asserted by BNO08x before transaction is considered failed (in miliseconds)
static const constexpr uint8_t TASK_CNT = 2;
static const constexpr uint8_t TASK_CNT = 2; ///<total amount of tasks utilized by BNO08x driver library
static const constexpr uint32_t SCLK_MAX_SPEED = 3000000UL;
// evt_grp_spi bits
static const constexpr EventBits_t EVT_GRP_SPI_RX_DONE_BIT =
(1 << 0); ///<When this bit is set it indicates a receive procedure has completed.
static const constexpr EventBits_t EVT_GRP_SPI_RX_VALID_PACKET =
static const constexpr EventBits_t EVT_GRP_SPI_RX_VALID_PACKET_BIT =
(1 << 1); ///< When this bit is set, it indicates a valid packet has been received and processed.
static const constexpr EventBits_t EVT_GRP_SPI_RX_INVALID_PACKET =
static const constexpr EventBits_t EVT_GRP_SPI_RX_INVALID_PACKET_BIT =
(1 << 2); ///<When this bit is set, it indicates an invalid packet has been received.
static const constexpr EventBits_t EVT_GRP_SPI_TX_DONE = (1 << 3); ///<When this bit is set, it indicates a queued packet has been sent.
static const constexpr EventBits_t EVT_GRP_SPI_TX_DONE_BIT = (1 << 3); ///<When this bit is set, it indicates a queued packet has been sent.
// evt_grp_report_en bits
static const constexpr EventBits_t EVT_GRP_RPT_ROTATION_VECTOR_BIT = (1 << 0); ///< When set, rotation vector reports are active.
@ -435,6 +448,9 @@ class BNO08x
static const constexpr EventBits_t EVT_GRP_RPT_RAW_GYRO_BIT = (1 << 16); ///< When set, raw gyro reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_RAW_MAGNETOMETER_BIT = (1 << 17); ///< When set, raw magnetometer reports are active.
// evt_grp_task_flow bits
static const constexpr EventBits_t EVT_GRP_TSK_FLW_RUNNING_BIT= (1 << 0); ///< When set, data_proc_task and spi_task are active, when 0 they are pending deletion or deleted.
static const constexpr EventBits_t EVT_GRP_RPT_ALL_BITS =
EVT_GRP_RPT_ROTATION_VECTOR_BIT | EVT_GRP_RPT_GAME_ROTATION_VECTOR_BIT | EVT_GRP_RPT_ARVR_S_ROTATION_VECTOR_BIT |
EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT | EVT_GRP_RPT_GYRO_ROTATION_VECTOR_BIT | EVT_GRP_RPT_ACCELEROMETER_BIT |

471
source/BNO08x.cpp
View File

@ -1,7 +1,5 @@
#include "BNO08x.hpp"
bool BNO08x::isr_service_installed = {false};
/**
* @brief BNO08x imu constructor.
*
@ -12,136 +10,44 @@ bool BNO08x::isr_service_installed = {false};
* @return void, nothing to return
*/
BNO08x::BNO08x(bno08x_config_t imu_config)
: evt_grp_spi(xEventGroupCreate())
: spi_task_hdl(NULL)
, data_proc_task_hdl(NULL)
, evt_grp_spi(xEventGroupCreate())
, evt_grp_report_en(xEventGroupCreate())
, evt_grp_task_flow(xEventGroupCreate())
, queue_rx_data(xQueueCreate(1, sizeof(bno08x_rx_packet_t)))
, queue_tx_data(xQueueCreate(1, sizeof(bno08x_tx_packet_t)))
, queue_frs_read_data(xQueueCreate(1, RX_DATA_LENGTH * sizeof(uint8_t)))
, queue_reset_reason(xQueueCreate(1, sizeof(uint32_t)))
, imu_config(imu_config)
, calibration_status(1)
, kill_tasks(false)
{
uint8_t tx_buffer[50] = {0};
// SPI bus config
bus_config.mosi_io_num = imu_config.io_mosi; // assign mosi gpio pin
bus_config.miso_io_num = imu_config.io_miso; // assign miso gpio pin
bus_config.sclk_io_num = imu_config.io_sclk; // assign sclk gpio pin
bus_config.quadhd_io_num = -1; // hold signal gpio (not used)
bus_config.quadwp_io_num = -1; // write protect signal gpio (not used)
// SPI slave device specific config
imu_spi_config.mode = 0x3; // set mode to 3 as per BNO08x datasheet (CPHA second edge, CPOL bus high when idle)
if (imu_config.sclk_speed > 3000000UL) // max sclk speed of 3MHz for BNO08x
{
ESP_LOGE(TAG, "Max clock speed exceeded, %ld overwritten with 3MHz", imu_config.sclk_speed);
imu_config.sclk_speed = 3000000UL;
}
imu_spi_config.clock_source = SPI_CLK_SRC_DEFAULT;
imu_spi_config.clock_speed_hz = imu_config.sclk_speed; // assign SCLK speed
imu_spi_config.address_bits = 0; // 0 address bits, not using this system
imu_spi_config.command_bits = 0; // 0 command bits, not using this system
imu_spi_config.spics_io_num = -1; // due to esp32 silicon issue, chip select cannot be used with full-duplex mode
// driver, it must be handled via calls to gpio pins
imu_spi_config.queue_size = 5; // only allow for 5 queued transactions at a time
// SPI non-driver-controlled GPIO config
// configure outputs
gpio_config_t outputs_config;
if (imu_config.io_wake != GPIO_NUM_NC)
outputs_config.pin_bit_mask =
(1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst) | (1ULL << imu_config.io_wake); // configure CS, RST, and wake gpio pins
else
outputs_config.pin_bit_mask = (1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst);
outputs_config.mode = GPIO_MODE_OUTPUT;
outputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE;
outputs_config.pull_up_en = GPIO_PULLUP_DISABLE;
outputs_config.intr_type = GPIO_INTR_DISABLE;
gpio_config(&outputs_config);
gpio_set_level(imu_config.io_cs, 1);
gpio_set_level(imu_config.io_rst, 1);
if (imu_config.io_wake != GPIO_NUM_NC)
gpio_set_level(imu_config.io_wake, 1);
// configure input (HINT pin)
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;
gpio_config(&inputs_config);
// check if GPIO ISR service has been installed (only has to be done once regardless of SPI slaves being used)
if (!isr_service_installed)
{
gpio_install_isr_service(0); // install isr service
isr_service_installed = true;
}
ESP_ERROR_CHECK(gpio_isr_handler_add(imu_config.io_int, hint_handler, (void*) this));
gpio_intr_disable(imu_config.io_int); // disable interrupts initially before reset
// initialize the spi peripheral
spi_bus_initialize(imu_config.spi_peripheral, &bus_config, SPI_DMA_CH_AUTO);
// add the imu device to the bus
spi_bus_add_device(imu_config.spi_peripheral, &imu_spi_config, &spi_hdl);
// do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.)
spi_transaction.length = 8;
spi_transaction.rxlength = 0;
spi_transaction.tx_buffer = tx_buffer;
spi_transaction.rx_buffer = NULL;
spi_transaction.flags = 0;
spi_device_polling_transmit(spi_hdl, &spi_transaction); // send data packet
}
BNO08x::~BNO08x()
{
static const constexpr uint8_t TASK_DELETE_TIMEOUT_MS = 10;
bno08x_rx_packet_t dummy_packet;
uint8_t kill_count = 0;
//disable interrupts before beginning so we can ensure SPI task doesn't attempt to run
BNO08x::~BNO08x()
{
// disable interrupts before beginning so we can ensure SPI task doesn't attempt to run
gpio_intr_disable(imu_config.io_int);
//delete tasks
kill_tasks = true;
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
// delete tasks if they have been created
if (spi_task_hdl != NULL && data_proc_task_hdl != NULL)
ESP_ERROR_CHECK(kill_all_tasks());
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)
{
ESP_LOGE(TAG, "Task deletion timedout in deconstructor call.");
ESP_ERROR_CHECK(ESP_ERR_TIMEOUT);
}
//delete queues
// delete queues
vQueueDelete(queue_rx_data);
vQueueDelete(queue_tx_data);
vQueueDelete(queue_frs_read_data);
vQueueDelete(queue_reset_reason);
//delete event groups
// delete event groups
vEventGroupDelete(evt_grp_spi);
vEventGroupDelete(evt_grp_report_en);
vEventGroupDelete(evt_grp_task_flow);
//clear callback list
// clear callback list
cb_list.clear();
}
}
/**
* @brief Initializes BNO08x sensor
@ -153,13 +59,27 @@ BNO08x::BNO08x(bno08x_config_t imu_config)
*/
bool BNO08x::initialize()
{
// launch tasks
data_proc_task_hdl = NULL;
spi_task_hdl = NULL;
xTaskCreate(&data_proc_task_trampoline, "bno08x_data_processing_task", CONFIG_ESP32_BNO08X_DATA_PROC_TASK_SZ, this, 7,
&data_proc_task_hdl); // launch data processing task
xTaskCreate(&spi_task_trampoline, "bno08x_spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task
//initialize configuration arguments
if(initialize_config_args() != ESP_OK)
return false;
//initialize GPIO
if(initialize_gpio() != ESP_OK)
return false;
// intialize HINT ISR
if (initialize_hint_isr() != ESP_OK)
return false;
// initialize SPI
if (initialize_spi() != ESP_OK)
return false;
// launch tasks
if (launch_tasks() != ESP_OK)
return false;
// reset BNO08x
if (!hard_reset())
return false;
@ -172,6 +92,173 @@ bool BNO08x::initialize()
return false;
}
esp_err_t BNO08x::initialize_config_args()
{
if((imu_config.io_cs == GPIO_NUM_NC))
{
ESP_LOGE(TAG, "CS GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG;
}
if((imu_config.io_miso == GPIO_NUM_NC))
{
ESP_LOGE(TAG, "MISO GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG;
}
if((imu_config.io_mosi == GPIO_NUM_NC))
{
ESP_LOGE(TAG, "MOSI GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG;
}
if((imu_config.io_sclk == GPIO_NUM_NC))
{
ESP_LOGE(TAG, "SCLK GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG;
}
if((imu_config.io_rst == GPIO_NUM_NC))
{
ESP_LOGE(TAG, "RST GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG;
}
// SPI bus config
bus_config.mosi_io_num = imu_config.io_mosi; // assign mosi gpio pin
bus_config.miso_io_num = imu_config.io_miso; // assign miso gpio pin
bus_config.sclk_io_num = imu_config.io_sclk; // assign sclk gpio pin
bus_config.quadhd_io_num = -1; // hold signal gpio (not used)
bus_config.quadwp_io_num = -1; // write protect signal gpio (not used)
// SPI slave device specific config
imu_spi_config.mode = 0x3; // set mode to 3 as per BNO08x datasheet (CPHA second edge, CPOL bus high when idle)
if (imu_config.sclk_speed > SCLK_MAX_SPEED) // max sclk speed of 3MHz for BNO08x
{
ESP_LOGE(TAG, "Max SPI clock speed exceeded, %ld overwritten with 3MHz", imu_config.sclk_speed);
imu_config.sclk_speed = SCLK_MAX_SPEED;
}
imu_spi_config.clock_source = SPI_CLK_SRC_DEFAULT;
imu_spi_config.clock_speed_hz = imu_config.sclk_speed; // assign SCLK speed
imu_spi_config.address_bits = 0; // 0 address bits, not using this system
imu_spi_config.command_bits = 0; // 0 command bits, not using this system
imu_spi_config.spics_io_num = -1; // due to esp32 silicon issue, chip select cannot be used with full-duplex mode
// driver, it must be handled via calls to gpio pins
imu_spi_config.queue_size = static_cast<int>(CONFIG_ESP32_BNO08X_SPI_QUEUE_SZ); // set max allowable queued SPI transactions
return ESP_OK;
}
esp_err_t BNO08x::initialize_gpio()
{
esp_err_t ret = ESP_OK;
/*GPIO config for pins not controlled by SPI peripheral*/
// configure output(s) (CS, RST, and WAKE)
gpio_config_t outputs_config;
outputs_config.pin_bit_mask = (imu_config.io_wake != GPIO_NUM_NC)
? ((1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst) | (1ULL << imu_config.io_wake))
: ((1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst));
outputs_config.mode = GPIO_MODE_OUTPUT;
outputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE;
outputs_config.pull_up_en = GPIO_PULLUP_DISABLE;
outputs_config.intr_type = GPIO_INTR_DISABLE;
ret = gpio_config(&outputs_config);
if (ret != ESP_OK)
{
ESP_LOGE(TAG, "Failed to configure CS, RST, and WAKE (if used) gpio.");
return ret;
}
//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, "Failed to configure HINT gpio.");
return ret;
}
gpio_set_level(imu_config.io_cs, 1);
gpio_set_level(imu_config.io_rst, 1);
if (imu_config.io_wake != GPIO_NUM_NC)
gpio_set_level(imu_config.io_wake, 1);
return ret;
}
esp_err_t BNO08x::initialize_hint_isr()
{
esp_err_t ret = ESP_OK;
// check if installation of ISR service has been requested by user (default is true)
if (imu_config.install_isr_service)
ret = gpio_install_isr_service(0); // install isr service
if (ret != ESP_OK)
{
ESP_LOGE(TAG, "Failed to install global ISR service.");
return ret;
}
ret = gpio_isr_handler_add(imu_config.io_int, hint_handler, (void*) this);
if (ret != ESP_OK)
{
ESP_LOGE(TAG, "Add hint_handler ISR.");
return ret;
}
gpio_intr_disable(imu_config.io_int); // disable interrupts initially before reset
return ret;
}
esp_err_t BNO08x::initialize_spi()
{
esp_err_t ret = ESP_OK;
uint8_t tx_buffer[50] = {0}; // for dummy transaction to stabilize SPI peripheral
// initialize the spi peripheral
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.");
return ret;
}
// 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.");
return ret;
}
// do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.)
spi_transaction.length = 8;
spi_transaction.rxlength = 0;
spi_transaction.tx_buffer = tx_buffer;
spi_transaction.rx_buffer = NULL;
spi_transaction.flags = 0;
spi_device_polling_transmit(spi_hdl, &spi_transaction); // send data packet
return ret;
}
/**
* @brief Waits for data to be received over SPI, or HOST_INT_TIMEOUT_MS to elapse.
*
@ -192,9 +279,9 @@ bool BNO08x::wait_for_rx_done()
// wait until an interrupt has been asserted and data received or timeout has occured
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "int asserted");
#endif
#endif
success = true;
}
@ -226,15 +313,15 @@ bool BNO08x::wait_for_data()
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
// wait until processing is done, this should never go to timeout; however, it will be set slightly after EVT_GRP_SPI_RX_DONE_BIT
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET | EVT_GRP_SPI_RX_INVALID_PACKET, pdFALSE, pdFALSE,
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET_BIT | EVT_GRP_SPI_RX_INVALID_PACKET_BIT, pdFALSE, pdFALSE,
HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
// only return true if packet is valid
if (xEventGroupGetBits(evt_grp_spi) & EVT_GRP_SPI_RX_VALID_PACKET)
if (xEventGroupGetBits(evt_grp_spi) & EVT_GRP_SPI_RX_VALID_PACKET_BIT)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Valid packet received.");
#endif
#endif
success = true;
}
@ -249,7 +336,7 @@ bool BNO08x::wait_for_data()
ESP_LOGE(TAG, "Interrupt to host device never asserted.");
}
xEventGroupClearBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET | EVT_GRP_SPI_RX_INVALID_PACKET);
xEventGroupClearBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET_BIT | EVT_GRP_SPI_RX_INVALID_PACKET_BIT);
return success;
}
@ -266,11 +353,11 @@ bool BNO08x::wait_for_tx_done()
if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Packet sent successfully.");
#endif
#endif
return true;
}
@ -455,9 +542,9 @@ bool BNO08x::receive_packet()
packet.length = (((uint16_t) packet.header[1]) << 8) | ((uint16_t) packet.header[0]);
packet.length &= ~(1 << 15); // Clear the MSbit
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "packet rx length: %d", packet.length);
#endif
#endif
if (packet.length == 0)
return false;
@ -584,7 +671,7 @@ void BNO08x::send_packet(bno08x_tx_packet_t* packet)
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE);
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE_BIT);
}
/**
@ -918,9 +1005,9 @@ void BNO08x::register_cb(std::function<void()> cb_fxn)
*/
uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "SHTP Header RX'd: 0x%X 0x%X 0x%X 0x%X", packet->header[0], packet->header[1], packet->header[2], packet->header[3]);
#endif
#endif
if (packet->body[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) // check to see that product ID matches what it should
{
@ -935,26 +1022,26 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
// Check to see if this packet is a sensor reporting its data to us
if (packet->header[2] == CHANNEL_REPORTS && packet->body[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel report");
#endif
#endif
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
}
else if (packet->header[2] == CHANNEL_CONTROL)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel control");
#endif
#endif
return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc.
}
else if (packet->header[2] == CHANNEL_GYRO)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
#endif
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
@ -2809,10 +2896,10 @@ void BNO08x::spi_task_trampoline(void* arg)
*/
void BNO08x::spi_task()
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
static uint64_t prev_time = esp_timer_get_time();
static uint64_t current_time = 0;
#endif
#endif
bno08x_tx_packet_t tx_packet;
@ -2825,22 +2912,28 @@ void BNO08x::spi_task()
ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // block until notified by ISR (hint_handler)
if(kill_tasks)
break;
if (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
current_time = esp_timer_get_time();
ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time));
prev_time = current_time;
#endif
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
current_time = esp_timer_get_time();
ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time));
prev_time = current_time;
#endif
if (xQueueReceive(queue_tx_data, &tx_packet, 0)) // check for queued packet to be sent, non blocking
send_packet(&tx_packet); // send packet
if (xQueueReceive(queue_tx_data, &tx_packet, 0)) // check for queued packet to be sent, non blocking
send_packet(&tx_packet); // send packet
else
receive_packet(); // receive packet
}
else
receive_packet(); // receive packet
{
// exit loop, deconstructor has requested task be deleted
break;
}
}
xSemaphoreGive(sem_kill_tasks); //signal to deconstructor deletion is completed
xSemaphoreGive(sem_kill_tasks); // signal to deconstructor deletion is completed
vTaskDelete(NULL);
}
@ -2867,30 +2960,86 @@ void BNO08x::data_proc_task()
{
bno08x_rx_packet_t packet;
while (1)
while (1) // receive packet from spi_task()
{
if (xQueueReceive(queue_rx_data, &packet, portMAX_DELAY)) // receive packet from spi_task()
if (xQueueReceive(queue_rx_data, &packet, portMAX_DELAY) == pdTRUE)
{
if(kill_tasks)
break;
if (parse_packet(&packet) != 0) // check if packet is valid
if (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
{
// execute any registered callbacks
for (auto& cb_fxn : cb_list)
cb_fxn();
if (parse_packet(&packet) != 0) // check if packet is valid
{
// execute any registered callbacks
for (auto& cb_fxn : cb_list)
cb_fxn();
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET); // indicate valid packet to wait_for_data()
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET_BIT); // indicate valid packet to wait_for_data()
}
else
{
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET_BIT); // indicated invalid packet to wait_for_data()
}
}
else
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET); // indicated invalid packet to wait_for_data()
{
// exit loop, deconstructor has requested task be deleted
break;
}
}
}
xSemaphoreGive(sem_kill_tasks); //signal to deconstructor deletion is completed
// self delete task
xSemaphoreGive(sem_kill_tasks); // signal to deconstructor task deletion is completed
vTaskDelete(NULL);
}
esp_err_t BNO08x::launch_tasks()
{
BaseType_t task_created = pdFALSE;
xEventGroupSetBits(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT); // set task flow to running
// 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)
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.");
return ESP_ERR_INVALID_STATE;
}
else
{
return ESP_OK;
}
}
esp_err_t BNO08x::kill_all_tasks()
{
static const constexpr uint8_t TASK_DELETE_TIMEOUT_MS = 10;
uint8_t kill_count = 0;
bno08x_rx_packet_t dummy_packet;
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)
{
ESP_LOGE(TAG, "Task deletion timed out in deconstructor call.");
return ESP_ERR_TIMEOUT;
}
return ESP_OK;
}
/**
* @brief HINT interrupt service routine, handles falling edge of BNO08x HINT pin.
*