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

8
Kconfig.projbuild
View File

@ -71,6 +71,14 @@ menu "esp32_BNO08x"
help help
SPI clock speed in Hz, default 2MHz. 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 endmenu #SPI Configuration
config ESP32_BNO08X_DATA_PROC_TASK_SZ config ESP32_BNO08X_DATA_PROC_TASK_SZ

74
include/BNO08x.hpp
View File

@ -21,6 +21,9 @@
#include <functional> #include <functional>
#include <vector> #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 /// @brief SHTP protocol channels
enum channels_t 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_rst; /// Reset pin (connects to BNO08x RST pin)
gpio_num_t io_wake; ///<Wake pin (optional, connects to BNO08x P0) gpio_num_t io_wake; ///<Wake pin (optional, connects to BNO08x P0)
uint32_t sclk_speed; ///<Desired SPI SCLK speed in Hz (max 3MHz) 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. /// @brief Default IMU configuration settings constructor.
/// To modify default GPIO pins, run "idf.py menuconfig" esp32_BNO08x->GPIO Configuration. /// To modify default GPIO pins, run "idf.py menuconfig" esp32_BNO08x->GPIO Configuration.
/// Alternatively, edit the default values in "Kconfig.projbuild" /// 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) : spi_peripheral((spi_host_device_t) CONFIG_ESP32_BNO08x_SPI_HOST)
, io_mosi((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_DI) // default: 23 , io_mosi(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_DI)) // default: 23
, io_miso((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_SDA) // default: 19 , io_miso(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_SDA)) // default: 19
, io_sclk((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_SCL) // default: 18 , io_sclk(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_SCL)) // default: 18
, io_cs((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_CS) // default: 33 , io_cs(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_CS)) // default: 33
, io_int((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_HINT) // default: 26 , io_int(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_HINT)) // default: 26
, io_rst((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_RST) // default: 32 , io_rst(static_cast<gpio_num_t>(CONFIG_ESP32_BNO08X_GPIO_RST)) // default: 32
, io_wake((gpio_num_t) CONFIG_ESP32_BNO08X_GPIO_WAKE) // default: -1 (unused) , io_wake(static_cast<gpio_num_t>( CONFIG_ESP32_BNO08X_GPIO_WAKE)) // default: -1 (unused)
, sclk_speed((uint32_t) CONFIG_ESP32_BNO08X_SCL_SPEED_HZ) // default: 2MHz , 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 /// @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, 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) : spi_peripheral(spi_peripheral)
, io_mosi(io_mosi) , io_mosi(io_mosi)
, io_miso(io_miso) , io_miso(io_miso)
@ -82,6 +87,7 @@ typedef struct bno08x_config_t
, io_rst(io_rst) , io_rst(io_rst)
, io_wake(io_wake) , io_wake(io_wake)
, sclk_speed(sclk_speed) , sclk_speed(sclk_speed)
, install_isr_service(install_isr_service)
{ {
} }
} bno08x_config_t; } bno08x_config_t;
@ -305,6 +311,11 @@ class BNO08x
uint16_t length; ///< Packet length in bytes. uint16_t length; ///< Packet length in bytes.
} bno08x_tx_packet_t; } 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_rx_done();
bool wait_for_tx_done(); bool wait_for_tx_done();
bool wait_for_data(); bool wait_for_data();
@ -329,9 +340,25 @@ class BNO08x
void print_header(bno08x_rx_packet_t* packet); void print_header(bno08x_rx_packet_t* packet);
void print_packet(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 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. 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_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_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. 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, 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) 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 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 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 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 = 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) 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 // evt_grp_spi bits
static const constexpr EventBits_t EVT_GRP_SPI_RX_DONE_BIT = 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. (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. (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. (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 // 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. 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_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. 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 = 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_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 | EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT | EVT_GRP_RPT_GYRO_ROTATION_VECTOR_BIT | EVT_GRP_RPT_ACCELEROMETER_BIT |

451
source/BNO08x.cpp
View File

@ -1,7 +1,5 @@
#include "BNO08x.hpp" #include "BNO08x.hpp"
bool BNO08x::isr_service_installed = {false};
/** /**
* @brief BNO08x imu constructor. * @brief BNO08x imu constructor.
* *
@ -12,136 +10,44 @@ bool BNO08x::isr_service_installed = {false};
* @return void, nothing to return * @return void, nothing to return
*/ */
BNO08x::BNO08x(bno08x_config_t imu_config) 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_report_en(xEventGroupCreate())
, evt_grp_task_flow(xEventGroupCreate())
, queue_rx_data(xQueueCreate(1, sizeof(bno08x_rx_packet_t))) , queue_rx_data(xQueueCreate(1, sizeof(bno08x_rx_packet_t)))
, queue_tx_data(xQueueCreate(1, sizeof(bno08x_tx_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_frs_read_data(xQueueCreate(1, RX_DATA_LENGTH * sizeof(uint8_t)))
, queue_reset_reason(xQueueCreate(1, sizeof(uint32_t))) , queue_reset_reason(xQueueCreate(1, sizeof(uint32_t)))
, imu_config(imu_config) , imu_config(imu_config)
, calibration_status(1) , 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()
BNO08x::~BNO08x() {
{ // disable interrupts before beginning so we can ensure SPI task doesn't attempt to run
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
gpio_intr_disable(imu_config.io_int); gpio_intr_disable(imu_config.io_int);
//delete tasks // delete tasks if they have been created
kill_tasks = true; if (spi_task_hdl != NULL && data_proc_task_hdl != NULL)
xTaskNotifyGive(spi_task_hdl); //notify spi task for self deletion ESP_ERROR_CHECK(kill_all_tasks());
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++) // delete queues
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
vQueueDelete(queue_rx_data); vQueueDelete(queue_rx_data);
vQueueDelete(queue_tx_data); vQueueDelete(queue_tx_data);
vQueueDelete(queue_frs_read_data); vQueueDelete(queue_frs_read_data);
vQueueDelete(queue_reset_reason); vQueueDelete(queue_reset_reason);
//delete event groups // delete event groups
vEventGroupDelete(evt_grp_spi); vEventGroupDelete(evt_grp_spi);
vEventGroupDelete(evt_grp_report_en); vEventGroupDelete(evt_grp_report_en);
vEventGroupDelete(evt_grp_task_flow);
//clear callback list // clear callback list
cb_list.clear(); cb_list.clear();
}
}
/** /**
* @brief Initializes BNO08x sensor * @brief Initializes BNO08x sensor
@ -153,13 +59,27 @@ BNO08x::BNO08x(bno08x_config_t imu_config)
*/ */
bool BNO08x::initialize() bool BNO08x::initialize()
{ {
// launch tasks //initialize configuration arguments
data_proc_task_hdl = NULL; if(initialize_config_args() != ESP_OK)
spi_task_hdl = NULL; return false;
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 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()) if (!hard_reset())
return false; return false;
@ -172,6 +92,173 @@ bool BNO08x::initialize()
return false; 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. * @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 // 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)) 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"); ESP_LOGI(TAG, "int asserted");
#endif #endif
success = true; 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)) 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 // 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)) HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{ {
// only return true if packet is valid // 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."); ESP_LOGI(TAG, "Valid packet received.");
#endif #endif
success = true; success = true;
} }
@ -249,7 +336,7 @@ bool BNO08x::wait_for_data()
ESP_LOGE(TAG, "Interrupt to host device never asserted."); 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; return success;
} }
@ -266,11 +353,11 @@ bool BNO08x::wait_for_tx_done()
if (xEventGroupGetBits(evt_grp_report_en) == 0) if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts 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."); ESP_LOGI(TAG, "Packet sent successfully.");
#endif #endif
return true; 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 = (((uint16_t) packet.header[1]) << 8) | ((uint16_t) packet.header[0]);
packet.length &= ~(1 << 15); // Clear the MSbit 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); ESP_LOGW(TAG, "packet rx length: %d", packet.length);
#endif #endif
if (packet.length == 0) if (packet.length == 0)
return false; 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 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) 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]); 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 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 // 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) 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"); 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 return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found // report is found
} }
else if (packet->header[2] == CHANNEL_CONTROL) 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"); ESP_LOGI(TAG, "RX'd packet, channel control");
#endif #endif
return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc. return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc.
} }
else if (packet->header[2] == CHANNEL_GYRO) 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"); 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 return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found // report is found
@ -2809,10 +2896,10 @@ void BNO08x::spi_task_trampoline(void* arg)
*/ */
void BNO08x::spi_task() 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 prev_time = esp_timer_get_time();
static uint64_t current_time = 0; static uint64_t current_time = 0;
#endif #endif
bno08x_tx_packet_t tx_packet; 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) ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // block until notified by ISR (hint_handler)
if(kill_tasks) if (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
break; {
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS #ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
current_time = esp_timer_get_time(); current_time = esp_timer_get_time();
ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time)); ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time));
prev_time = current_time; prev_time = current_time;
#endif #endif
if (xQueueReceive(queue_tx_data, &tx_packet, 0)) // check for queued packet to be sent, non blocking if (xQueueReceive(queue_tx_data, &tx_packet, 0)) // check for queued packet to be sent, non blocking
send_packet(&tx_packet); // send packet send_packet(&tx_packet); // send packet
else else
receive_packet(); // receive packet receive_packet(); // receive packet
} }
else
{
// 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); vTaskDelete(NULL);
} }
@ -2867,30 +2960,86 @@ void BNO08x::data_proc_task()
{ {
bno08x_rx_packet_t packet; 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 (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
{ {
if(kill_tasks)
break;
if (parse_packet(&packet) != 0) // check if packet is valid if (parse_packet(&packet) != 0) // check if packet is valid
{ {
// execute any registered callbacks // execute any registered callbacks
for (auto& cb_fxn : cb_list) for (auto& cb_fxn : cb_list)
cb_fxn(); 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 else
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET); // indicated invalid packet to wait_for_data() {
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET_BIT); // indicated invalid packet to wait_for_data()
}
}
else
{
// 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); 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. * @brief HINT interrupt service routine, handles falling edge of BNO08x HINT pin.
* *