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

deconstructor + refactoring

This commit is contained in:
myles-parfeniuk 2024-11-14 01:36:28 -08:00
parent 343c406325
commit 991c447f05
5 changed files with 431 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

260
include/BNO08x.hpp
View File

@ -21,7 +21,7 @@
#include <functional> #include <functional>
#include <vector> #include <vector>
//macros // macros
#define CHECK_TASKS_RUNNING(evt_grp_task_flow, running_bit) ((xEventGroupGetBits(evt_grp_task_flow) & (running_bit)) != 0) #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
@ -43,6 +43,17 @@ enum class IMUAccuracy
HIGH 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 /// @brief IMU configuration settings passed into constructor
typedef struct bno08x_config_t typedef struct bno08x_config_t
{ {
@ -63,14 +74,14 @@ typedef struct bno08x_config_t
bno08x_config_t(bool install_isr_service = true) 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(static_cast<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(static_cast<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(static_cast<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(static_cast<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(static_cast<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(static_cast<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(static_cast<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(static_cast<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 , install_isr_service(install_isr_service) // default: true
{ {
} }
@ -96,12 +107,12 @@ class BNO08x
{ {
public: public:
BNO08x(bno08x_config_t imu_config = bno08x_config_t()); BNO08x(bno08x_config_t imu_config = bno08x_config_t());
~BNO08x(); ~BNO08x();
bool initialize(); bool initialize();
bool hard_reset(); bool hard_reset();
bool soft_reset(); bool soft_reset();
uint8_t get_reset_reason(); IMUResetReason get_reset_reason();
bool mode_sleep(); bool mode_sleep();
bool mode_on(); bool mode_on();
@ -255,36 +266,36 @@ class BNO08x
// Record IDs from figure 29, page 29 reference manual // Record IDs from figure 29, page 29 reference manual
// These are used to read the metadata for each sensor type // These are used to read the metadata for each sensor type
static const constexpr uint16_t FRS_RECORD_ID_ACCELEROMETER = 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 = 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 = 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 = 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 // Activity classifier bits
static const constexpr uint16_t ACTIVITY_CLASSIFIER_UNKNOWN_EN = (1 << 0); static const constexpr uint16_t ACTIVITY_CLASSIFIER_UNKNOWN_EN = (1U << 0U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_IN_VEHICLE_EN = (1 << 1); static const constexpr uint16_t ACTIVITY_CLASSIFIER_IN_VEHICLE_EN = (1U << 1U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_BICYCLE_EN = (1 << 2); static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_BICYCLE_EN = (1U << 2U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_FOOT_EN = (1 << 3); static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_FOOT_EN = (1U << 3U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_STILL_EN = (1 << 4); static const constexpr uint16_t ACTIVITY_CLASSIFIER_STILL_EN = (1U << 4U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_TILTING_EN = (1 << 5); static const constexpr uint16_t ACTIVITY_CLASSIFIER_TILTING_EN = (1U << 5U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_WALKING_EN = (1 << 6); static const constexpr uint16_t ACTIVITY_CLASSIFIER_WALKING_EN = (1U << 6U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_RUNNING_EN = (1 << 7); static const constexpr uint16_t ACTIVITY_CLASSIFIER_RUNNING_EN = (1U << 7U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_STAIRS_EN = (1 << 8); static const constexpr uint16_t ACTIVITY_CLASSIFIER_ON_STAIRS_EN = (1U << 8U);
static const constexpr uint16_t ACTIVITY_CLASSIFIER_ALL_EN = 0x1F; 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_ALL = 0x07U; ///< 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_Z = 0x04U; ///< Tar yaw axis only (used with tare now command)
// Which rotation vector to tare, BNO08x saves them seperately // Which rotation vector to tare, BNO08x saves them seperately
static const constexpr uint8_t TARE_ROTATION_VECTOR = 0; ///<Tare rotation vector static const constexpr uint8_t TARE_ROTATION_VECTOR = 0U; ///<Tare rotation vector
static const constexpr uint8_t TARE_GAME_ROTATION_VECTOR = 1; ///<Tare game rotation vector static const constexpr uint8_t TARE_GAME_ROTATION_VECTOR = 1U; ///<Tare game rotation vector
static const constexpr uint8_t TARE_GEOMAGNETIC_ROTATION_VECTOR = 2; ///< tare geomagnetic rotation vector static const constexpr uint8_t TARE_GEOMAGNETIC_ROTATION_VECTOR = 2U; ///< tare geomagnetic rotation vector
static const constexpr uint8_t TARE_GYRO_INTEGRATED_ROTATION_VECTOR = 3; ///<Tare gyro integrated rotation vector static const constexpr uint8_t TARE_GYRO_INTEGRATED_ROTATION_VECTOR = 3U; ///<Tare gyro integrated rotation vector
static const constexpr uint8_t TARE_ARVR_STABILIZED_ROTATION_VECTOR = 4; ///< Tare ARVR stabilized rotation vector static const constexpr uint8_t TARE_ARVR_STABILIZED_ROTATION_VECTOR = 4U; ///< Tare ARVR stabilized rotation vector
static const constexpr uint8_t TARE_ARVR_STABILIZED_GAME_ROTATION_VECTOR = 5; ///<Tare ARVR stabilized game rotation vector static const constexpr uint8_t TARE_ARVR_STABILIZED_GAME_ROTATION_VECTOR = 5U; ///<Tare ARVR stabilized game rotation vector
static const constexpr int16_t ROTATION_VECTOR_Q1 = 14; ///< Rotation vector Q point (See SH-2 Ref. Manual 6.5.18) static const constexpr int16_t ROTATION_VECTOR_Q1 = 14; ///< Rotation vector Q point (See SH-2 Ref. Manual 6.5.18)
static const constexpr int16_t ROTATION_VECTOR_ACCURACY_Q1 = 12; ///< Rotation vector accuracy estimate Q point (See SH-2 Ref. Manual 6.5.18) static const constexpr int16_t ROTATION_VECTOR_ACCURACY_Q1 = 12; ///< Rotation vector accuracy estimate Q point (See SH-2 Ref. Manual 6.5.18)
@ -296,6 +307,7 @@ class BNO08x
static const constexpr int16_t GRAVITY_Q1 = 8; ///< Gravity Q point (See SH-2 Ref. Manual 6.5.11) static const constexpr int16_t GRAVITY_Q1 = 8; ///< Gravity Q point (See SH-2 Ref. Manual 6.5.11)
private: private:
/// @brief Holds data that is received over spi. /// @brief Holds data that is received over spi.
typedef struct bno08x_rx_packet_t typedef struct bno08x_rx_packet_t
{ {
@ -311,10 +323,45 @@ 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(); /// @brief Holds info about which functionality has been successfully initialized (used by deconstructor during cleanup).
esp_err_t initialize_gpio(); typedef struct bno08x_init_status_t
esp_err_t initialize_hint_isr(); {
esp_err_t initialize_spi(); 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.
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;
esp_err_t init_config_args();
esp_err_t init_gpio();
esp_err_t init_gpio_inputs();
esp_err_t init_gpio_outputs();
esp_err_t init_hint_isr();
esp_err_t init_spi();
esp_err_t deinit_gpio();
esp_err_t deinit_gpio_inputs();
esp_err_t deinit_gpio_outputs();
esp_err_t deinit_hint_isr();
esp_err_t deinit_spi();
bool wait_for_rx_done(); bool wait_for_rx_done();
bool wait_for_tx_done(); bool wait_for_tx_done();
@ -348,12 +395,12 @@ class BNO08x
// data processing task // data processing task
TaskHandle_t data_proc_task_hdl; ///<data_proc_task() task handle TaskHandle_t data_proc_task_hdl; ///<data_proc_task() task handle
static void data_proc_task_trampoline(void* arg); static void data_proc_task_trampoline(void* arg);
void data_proc_task(); void data_proc_task();
// task control // task control
SemaphoreHandle_t sem_kill_tasks; ///<semaphore to count amount of killed tasks SemaphoreHandle_t sem_kill_tasks; ///<semaphore to count amount of killed tasks
esp_err_t launch_tasks(); esp_err_t launch_tasks();
esp_err_t kill_all_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.
@ -374,6 +421,7 @@ class BNO08x
spi_device_interface_config_t imu_spi_config{}; ///<SPI slave device settings spi_device_interface_config_t imu_spi_config{}; ///<SPI slave device settings
spi_device_handle_t spi_hdl{}; ///<SPI device handle spi_device_handle_t spi_hdl{}; ///<SPI device handle
spi_transaction_t spi_transaction{}; ///<SPI transaction handle spi_transaction_t spi_transaction{}; ///<SPI transaction handle
bno08x_init_status_t init_status; ///<Initialization status of various functionality, used by deconstructor during cleanup, set during initialization.
// These are the raw sensor values (without Q applied) pulled from the user requested Input Report // These are the raw sensor values (without Q applied) pulled from the user requested Input Report
uint32_t time_stamp; ///<Report timestamp (see datasheet 1.3.5.3) uint32_t time_stamp; ///<Report timestamp (see datasheet 1.3.5.3)
@ -407,49 +455,51 @@ class BNO08x
static void IRAM_ATTR hint_handler(void* arg); static void IRAM_ATTR hint_handler(void* arg);
static const constexpr uint16_t RX_DATA_LENGTH = 300; ///<length buffer containing data received over spi static const constexpr uint8_t TASK_CNT = 2U; ///<Total amount of tasks utilized by BNO08x driver library
static const constexpr uint16_t MAX_METADATA_LENGTH = 9; ///<max length of metadata used in frs read operations
static const constexpr uint16_t RX_DATA_LENGTH = 300U; ///<length buffer containing data received over spi
static const constexpr uint16_t MAX_METADATA_LENGTH = 9U; ///<max length of metadata used in frs read operations
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; ///<total amount of tasks utilized by BNO08x driver library static const constexpr uint32_t SCLK_MAX_SPEED = 3000000UL; ///<Max SPI SCLK speed BNO08x is capable of
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. (1U << 0U); ///<When this bit is set it indicates a receive procedure has completed.
static const constexpr EventBits_t EVT_GRP_SPI_RX_VALID_PACKET_BIT = 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. (1U << 1U); ///< 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_BIT = 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. (1U << 2U); ///<When this bit is set, it indicates an invalid packet has been received.
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. 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.
static const constexpr EventBits_t EVT_GRP_RPT_GAME_ROTATION_VECTOR_BIT = (1 << 1); ///< When set, game rotation vector reports are active. static const constexpr EventBits_t EVT_GRP_RPT_GAME_ROTATION_VECTOR_BIT = (1 << 1); ///< When set, game rotation vector reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_ARVR_S_ROTATION_VECTOR_BIT = static const constexpr EventBits_t EVT_GRP_RPT_ARVR_S_ROTATION_VECTOR_BIT =
(1 << 2); ///< When set, ARVR stabilized rotation vector reports are active. (1U << 2U); ///< When set, ARVR stabilized rotation vector reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT = static const constexpr EventBits_t EVT_GRP_RPT_ARVR_S_GAME_ROTATION_VECTOR_BIT =
(1 << 3); ///< When set, ARVR stabilized game rotation vector reports are active. (1U << 3U); ///< When set, ARVR stabilized game rotation vector reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_GYRO_ROTATION_VECTOR_BIT = static const constexpr EventBits_t EVT_GRP_RPT_GYRO_ROTATION_VECTOR_BIT =
(1 << 4); ///< When set, gyro integrator rotation vector reports are active. (1U << 4U); ///< When set, gyro integrator rotation vector reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_ACCELEROMETER_BIT = (1 << 5); ///< When set, accelerometer reports are active. static const constexpr EventBits_t EVT_GRP_RPT_ACCELEROMETER_BIT = (1U << 5U); ///< When set, accelerometer reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_LINEAR_ACCELEROMETER_BIT = (1 << 6); ///< When set, linear accelerometer reports are active. static const constexpr EventBits_t EVT_GRP_RPT_LINEAR_ACCELEROMETER_BIT = (1U << 6U); ///< When set, linear accelerometer reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_GRAVITY_BIT = (1 << 7); ///< When set, gravity reports are active. static const constexpr EventBits_t EVT_GRP_RPT_GRAVITY_BIT = (1U << 7U); ///< When set, gravity reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_GYRO_BIT = (1 << 8); ///< When set, gyro reports are active. static const constexpr EventBits_t EVT_GRP_RPT_GYRO_BIT = (1U << 8U); ///< When set, gyro reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_GYRO_UNCALIBRATED_BIT = (1 << 9); ///< When set, uncalibrated gyro reports are active. static const constexpr EventBits_t EVT_GRP_RPT_GYRO_UNCALIBRATED_BIT = (1U << 9U); ///< When set, uncalibrated gyro reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_MAGNETOMETER_BIT = (1 << 10); ///< When set, magnetometer reports are active. static const constexpr EventBits_t EVT_GRP_RPT_MAGNETOMETER_BIT = (1U << 10U); ///< When set, magnetometer reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_TAP_DETECTOR_BIT = (1 << 11); ///< When set, tap detector reports are active. static const constexpr EventBits_t EVT_GRP_RPT_TAP_DETECTOR_BIT = (1U << 11U); ///< When set, tap detector reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_STEP_COUNTER_BIT = (1 << 12); ///< When set, step counter reports are active. static const constexpr EventBits_t EVT_GRP_RPT_STEP_COUNTER_BIT = (1U << 12U); ///< When set, step counter reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_STABILITY_CLASSIFIER_BIT = (1 << 13); ///< When set, stability classifier reports are active. static const constexpr EventBits_t EVT_GRP_RPT_STABILITY_CLASSIFIER_BIT = (1U << 13U); ///< When set, stability classifier reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_ACTIVITY_CLASSIFIER_BIT = (1 << 14); ///< When set, activity classifier reports are active. static const constexpr EventBits_t EVT_GRP_RPT_ACTIVITY_CLASSIFIER_BIT = (1U << 14U); ///< When set, activity classifier reports are active.
static const constexpr EventBits_t EVT_GRP_RPT_RAW_ACCELEROMETER_BIT = (1 << 15); ///< When set, raw accelerometer reports are active. static const constexpr EventBits_t EVT_GRP_RPT_RAW_ACCELEROMETER_BIT = (1U << 15U); ///< When set, raw accelerometer 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_GYRO_BIT = (1U << 16U); ///< 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 = (1U << 17U); ///< When set, raw magnetometer reports are active.
// evt_grp_task_flow bits // 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_TSK_FLW_RUNNING_BIT =
(1U << 0U); ///< 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 |
@ -459,60 +509,60 @@ class BNO08x
EVT_GRP_RPT_ACTIVITY_CLASSIFIER_BIT | EVT_GRP_RPT_RAW_ACCELEROMETER_BIT | EVT_GRP_RPT_RAW_GYRO_BIT | EVT_GRP_RPT_RAW_MAGNETOMETER_BIT; EVT_GRP_RPT_ACTIVITY_CLASSIFIER_BIT | EVT_GRP_RPT_RAW_ACCELEROMETER_BIT | EVT_GRP_RPT_RAW_GYRO_BIT | EVT_GRP_RPT_RAW_MAGNETOMETER_BIT;
// Higher level calibration commands, used by queue_calibrate_command // Higher level calibration commands, used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_ACCEL = 0; ///<Calibrate accelerometer command used by queue_calibrate_command static const constexpr uint8_t CALIBRATE_ACCEL = 0U; ///<Calibrate accelerometer command used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_GYRO = 1; ///<Calibrate gyro command used by queue_calibrate_command static const constexpr uint8_t CALIBRATE_GYRO = 1U; ///<Calibrate gyro command used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_MAG = 2; ///<Calibrate magnetometer command used by queue_calibrate_command static const constexpr uint8_t CALIBRATE_MAG = 2U; ///<Calibrate magnetometer command used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_PLANAR_ACCEL = 3; ///<Calibrate planar acceleration command used by queue_calibrate_command static const constexpr uint8_t CALIBRATE_PLANAR_ACCEL = 3U; ///<Calibrate planar acceleration command used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_ACCEL_GYRO_MAG = static const constexpr uint8_t CALIBRATE_ACCEL_GYRO_MAG =
4; ///<Calibrate accelerometer, gyro, & magnetometer command used by queue_calibrate_command 4U; ///<Calibrate accelerometer, gyro, & magnetometer command used by queue_calibrate_command
static const constexpr uint8_t CALIBRATE_STOP = 5; ///<Stop calibration command used by queue_calibrate_command static const constexpr uint8_t CALIBRATE_STOP = 5U; ///<Stop calibration command used by queue_calibrate_command
// Command IDs (see Ref. Manual 6.4) // Command IDs (see Ref. Manual 6.4)
static const constexpr uint8_t COMMAND_ERRORS = 1; static const constexpr uint8_t COMMAND_ERRORS = 1U;
static const constexpr uint8_t COMMAND_COUNTER = 2; static const constexpr uint8_t COMMAND_COUNTER = 2U;
static const constexpr uint8_t COMMAND_TARE = 3; ///<Command and response to tare command (See Sh2 Ref. Manual 6.4.4) static const constexpr uint8_t COMMAND_TARE = 3U; ///<Command and response to tare command (See Sh2 Ref. Manual 6.4.4)
static const constexpr uint8_t COMMAND_INITIALIZE = 4; ///<Reinitialize sensor hub components See (SH2 Ref. Manual 6.4.5) static const constexpr uint8_t COMMAND_INITIALIZE = 4U; ///<Reinitialize sensor hub components See (SH2 Ref. Manual 6.4.5)
static const constexpr uint8_t COMMAND_DCD = 6; ///<Save DCD command (See SH2 Ref. Manual 6.4.7) static const constexpr uint8_t COMMAND_DCD = 6U; ///<Save DCD command (See SH2 Ref. Manual 6.4.7)
static const constexpr uint8_t COMMAND_ME_CALIBRATE = 7; ///<Command and response to configure ME calibration (See SH2 Ref. Manual 6.4.7) static const constexpr uint8_t COMMAND_ME_CALIBRATE = 7U; ///<Command and response to configure ME calibration (See SH2 Ref. Manual 6.4.7)
static const constexpr uint8_t COMMAND_DCD_PERIOD_SAVE = 9; ///<Configure DCD periodic saving (See SH2 Ref. Manual 6.4) static const constexpr uint8_t COMMAND_DCD_PERIOD_SAVE = 9U; ///<Configure DCD periodic saving (See SH2 Ref. Manual 6.4)
static const constexpr uint8_t COMMAND_OSCILLATOR = 10; ///<Retrieve oscillator type command (See SH2 Ref. Manual 6.4) static const constexpr uint8_t COMMAND_OSCILLATOR = 10U; ///<Retrieve oscillator type command (See SH2 Ref. Manual 6.4)
static const constexpr uint8_t COMMAND_CLEAR_DCD = 11; ///<Clear DCD & Reset command (See SH2 Ref. Manual 6.4) static const constexpr uint8_t COMMAND_CLEAR_DCD = 11U; ///<Clear DCD & Reset command (See SH2 Ref. Manual 6.4)
// SHTP channel 2 control report IDs, used in communication with sensor (See Ref. Manual 6.2) // SHTP channel 2 control report IDs, used in communication with sensor (See Ref. Manual 6.2)
static const constexpr uint8_t SHTP_REPORT_COMMAND_RESPONSE = 0xF1; ///< See SH2 Ref. Manual 6.3.9 static const constexpr uint8_t SHTP_REPORT_COMMAND_RESPONSE = 0xF1U; ///< See SH2 Ref. Manual 6.3.9
static const constexpr uint8_t SHTP_REPORT_COMMAND_REQUEST = 0xF2; ///< See SH2 Ref. Manual 6.3.8 static const constexpr uint8_t SHTP_REPORT_COMMAND_REQUEST = 0xF2U; ///< See SH2 Ref. Manual 6.3.8
static const constexpr uint8_t SHTP_REPORT_FRS_READ_RESPONSE = 0xF3; ///< See SH2 Ref. Manual 6.3.7 static const constexpr uint8_t SHTP_REPORT_FRS_READ_RESPONSE = 0xF3U; ///< See SH2 Ref. Manual 6.3.7
static const constexpr uint8_t SHTP_REPORT_FRS_READ_REQUEST = 0xF4; ///< See SH2 Ref. Manual 6.3.6 static const constexpr uint8_t SHTP_REPORT_FRS_READ_REQUEST = 0xF4U; ///< See SH2 Ref. Manual 6.3.6
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8; ///< See SH2 Ref. Manual 6.3.2 static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8U; ///< See SH2 Ref. Manual 6.3.2
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9; ///< See SH2 Ref. Manual 6.3.1 static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9U; ///< See SH2 Ref. Manual 6.3.1
static const constexpr uint8_t SHTP_REPORT_BASE_TIMESTAMP = 0xFB; ///< See SH2 Ref. Manual 7.2.1 static const constexpr uint8_t SHTP_REPORT_BASE_TIMESTAMP = 0xFBU; ///< See SH2 Ref. Manual 7.2.1
static const constexpr uint8_t SHTP_REPORT_SET_FEATURE_COMMAND = 0xFD; ///< See SH2 Ref. Manual 6.5.4 static const constexpr uint8_t SHTP_REPORT_SET_FEATURE_COMMAND = 0xFDU; ///< See SH2 Ref. Manual 6.5.4
// Sensor report IDs, used when enabling and reading BNO08x reports // Sensor report IDs, used when enabling and reading BNO08x reports
static const constexpr uint8_t SENSOR_REPORT_ID_ACCELEROMETER = 0x01; ///< See SH2 Ref. Manual 6.5.9 static const constexpr uint8_t SENSOR_REPORT_ID_ACCELEROMETER = 0x01U; ///< See SH2 Ref. Manual 6.5.9
static const constexpr uint8_t SENSOR_REPORT_ID_GYROSCOPE = 0x02; ///< See SH2 Ref. Manual 6.5.13 static const constexpr uint8_t SENSOR_REPORT_ID_GYROSCOPE = 0x02U; ///< See SH2 Ref. Manual 6.5.13
static const constexpr uint8_t SENSOR_REPORT_ID_MAGNETIC_FIELD = 0x03; ///< See SH2 Ref. Manual 6.5.16 static const constexpr uint8_t SENSOR_REPORT_ID_MAGNETIC_FIELD = 0x03U; ///< See SH2 Ref. Manual 6.5.16
static const constexpr uint8_t SENSOR_REPORT_ID_LINEAR_ACCELERATION = 0x04; ///< See SH2 Ref. Manual 6.5.10 static const constexpr uint8_t SENSOR_REPORT_ID_LINEAR_ACCELERATION = 0x04U; ///< See SH2 Ref. Manual 6.5.10
static const constexpr uint8_t SENSOR_REPORT_ID_ROTATION_VECTOR = 0x05; ///< See SH2 Ref. Manual 6.5.18 static const constexpr uint8_t SENSOR_REPORT_ID_ROTATION_VECTOR = 0x05U; ///< See SH2 Ref. Manual 6.5.18
static const constexpr uint8_t SENSOR_REPORT_ID_GRAVITY = 0x06; ///< See SH2 Ref. Manual 6.5.11 static const constexpr uint8_t SENSOR_REPORT_ID_GRAVITY = 0x06U; ///< See SH2 Ref. Manual 6.5.11
static const constexpr uint8_t SENSOR_REPORT_ID_UNCALIBRATED_GYRO = 0x07; ///< See SH2 Ref. Manual 6.5.14 static const constexpr uint8_t SENSOR_REPORT_ID_UNCALIBRATED_GYRO = 0x07U; ///< See SH2 Ref. Manual 6.5.14
static const constexpr uint8_t SENSOR_REPORT_ID_GAME_ROTATION_VECTOR = 0x08; ///< See SH2 Ref. Manual 6.5.19 static const constexpr uint8_t SENSOR_REPORT_ID_GAME_ROTATION_VECTOR = 0x08U; ///< See SH2 Ref. Manual 6.5.19
static const constexpr uint8_t SENSOR_REPORT_ID_GEOMAGNETIC_ROTATION_VECTOR = 0x09; ///< See SH2 Ref. Manual 6.5.20 static const constexpr uint8_t SENSOR_REPORT_ID_GEOMAGNETIC_ROTATION_VECTOR = 0x09U; ///< See SH2 Ref. Manual 6.5.20
static const constexpr uint8_t SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR = 0x2A; ///< See SH2 Ref. Manual 6.5.44 static const constexpr uint8_t SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR = 0x2AU; ///< See SH2 Ref. Manual 6.5.44
static const constexpr uint8_t SENSOR_REPORT_ID_TAP_DETECTOR = 0x10; ///< See SH2 Ref. Manual 6.5.27 static const constexpr uint8_t SENSOR_REPORT_ID_TAP_DETECTOR = 0x10U; ///< See SH2 Ref. Manual 6.5.27
static const constexpr uint8_t SENSOR_REPORT_ID_STEP_COUNTER = 0x11; ///< See SH2 Ref. Manual 6.5.29 static const constexpr uint8_t SENSOR_REPORT_ID_STEP_COUNTER = 0x11U; ///< See SH2 Ref. Manual 6.5.29
static const constexpr uint8_t SENSOR_REPORT_ID_STABILITY_CLASSIFIER = 0x13; ///< See SH2 Ref. Manual 6.5.31 static const constexpr uint8_t SENSOR_REPORT_ID_STABILITY_CLASSIFIER = 0x13U; ///< See SH2 Ref. Manual 6.5.31
static const constexpr uint8_t SENSOR_REPORT_ID_RAW_ACCELEROMETER = 0x14; ///< See SH2 Ref. Manual 6.5.8 static const constexpr uint8_t SENSOR_REPORT_ID_RAW_ACCELEROMETER = 0x14U; ///< See SH2 Ref. Manual 6.5.8
static const constexpr uint8_t SENSOR_REPORT_ID_RAW_GYROSCOPE = 0x15; ///< See SH2 Ref. Manual 6.5.12 static const constexpr uint8_t SENSOR_REPORT_ID_RAW_GYROSCOPE = 0x15U; ///< See SH2 Ref. Manual 6.5.12
static const constexpr uint8_t SENSOR_REPORT_ID_RAW_MAGNETOMETER = 0x16; ///< See SH2 Ref. Manual 6.5.15 static const constexpr uint8_t SENSOR_REPORT_ID_RAW_MAGNETOMETER = 0x16U; ///< See SH2 Ref. Manual 6.5.15
static const constexpr uint8_t SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER = 0x1E; ///< See SH2 Ref. Manual 6.5.36 static const constexpr uint8_t SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER = 0x1EU; ///< See SH2 Ref. Manual 6.5.36
static const constexpr uint8_t SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR = 0x28; ///< See SH2 Ref. Manual 6.5.42 static const constexpr uint8_t SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR = 0x28U; ///< See SH2 Ref. Manual 6.5.42
static const constexpr uint8_t SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR = 0x29; ///< See SH2 Ref. Manual 6.5.43 static const constexpr uint8_t SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR = 0x29U; ///< See SH2 Ref. Manual 6.5.43
// Tare commands used by queue_tare_command // Tare commands used by queue_tare_command
static const constexpr uint8_t TARE_NOW = 0; ///< See SH2 Ref. Manual 6.4.4.1 static const constexpr uint8_t TARE_NOW = 0U; ///< See SH2 Ref. Manual 6.4.4.1
static const constexpr uint8_t TARE_PERSIST = 1; ///< See SH2 Ref. Manual 6.4.4.2 static const constexpr uint8_t TARE_PERSIST = 1U; ///< See SH2 Ref. Manual 6.4.4.2
static const constexpr uint8_t TARE_SET_REORIENTATION = 2; ///< See SH2 Ref. Manual 6.4.4.3 static const constexpr uint8_t TARE_SET_REORIENTATION = 2U; ///< See SH2 Ref. Manual 6.4.4.3
static const constexpr char* TAG = "BNO08x"; ///< Class tag used for serial print statements static const constexpr char* TAG = "BNO08x"; ///< Class tag used for serial print statements
}; };

327
source/BNO08x.cpp
View File

@ -22,7 +22,6 @@ BNO08x::BNO08x(bno08x_config_t imu_config)
, imu_config(imu_config) , imu_config(imu_config)
, calibration_status(1) , calibration_status(1)
{ {
} }
BNO08x::~BNO08x() BNO08x::~BNO08x()
@ -30,9 +29,17 @@ BNO08x::~BNO08x()
// disable interrupts before beginning so we can ensure SPI task doesn't attempt to run // 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 if they have been created // delete any tasks if they have been created
if (spi_task_hdl != NULL && data_proc_task_hdl != NULL) ESP_ERROR_CHECK(kill_all_tasks());
ESP_ERROR_CHECK(kill_all_tasks());
// deinitialize spi if has been initialized
ESP_ERROR_CHECK(deinit_spi());
// deinitialize hint ISR if it has been initialized
ESP_ERROR_CHECK(deinit_hint_isr());
// deinitialize GPIO if they have been initialized
ESP_ERROR_CHECK(deinit_gpio());
// delete queues // delete queues
vQueueDelete(queue_rx_data); vQueueDelete(queue_rx_data);
@ -59,20 +66,20 @@ BNO08x::~BNO08x()
*/ */
bool BNO08x::initialize() bool BNO08x::initialize()
{ {
//initialize configuration arguments // initialize configuration arguments
if(initialize_config_args() != ESP_OK) if (init_config_args() != ESP_OK)
return false; return false;
//initialize GPIO // initialize GPIO
if(initialize_gpio() != ESP_OK) if (init_gpio() != ESP_OK)
return false; return false;
// intialize HINT ISR // initialize HINT ISR
if (initialize_hint_isr() != ESP_OK) if (init_hint_isr() != ESP_OK)
return false; return false;
// initialize SPI // initialize SPI
if (initialize_spi() != ESP_OK) if (init_spi() != ESP_OK)
return false; return false;
// launch tasks // launch tasks
@ -83,45 +90,46 @@ bool BNO08x::initialize()
if (!hard_reset()) if (!hard_reset())
return false; return false;
if (get_reset_reason() != 0) if (get_reset_reason() == IMUResetReason::UNDEFINED)
{ {
ESP_LOGI(TAG, "Successfully initialized...."); ESP_LOGE(TAG, "Initialization failed, undefined reset reason returned after reset.");
return true; return false;
} }
return false; ESP_LOGI(TAG, "Successfully initialized....");
return true;
} }
esp_err_t BNO08x::initialize_config_args() esp_err_t BNO08x::init_config_args()
{ {
if((imu_config.io_cs == GPIO_NUM_NC)) if ((imu_config.io_cs == GPIO_NUM_NC))
{ {
ESP_LOGE(TAG, "CS GPIO cannot be unassigned."); ESP_LOGE(TAG, "Initialization failed, CS GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if((imu_config.io_miso == GPIO_NUM_NC)) if ((imu_config.io_miso == GPIO_NUM_NC))
{ {
ESP_LOGE(TAG, "MISO GPIO cannot be unassigned."); ESP_LOGE(TAG, "Initialization failed, MISO GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if((imu_config.io_mosi == GPIO_NUM_NC)) if ((imu_config.io_mosi == GPIO_NUM_NC))
{ {
ESP_LOGE(TAG, "MOSI GPIO cannot be unassigned."); ESP_LOGE(TAG, "Initialization failed, MOSI GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if((imu_config.io_sclk == GPIO_NUM_NC)) if ((imu_config.io_sclk == GPIO_NUM_NC))
{ {
ESP_LOGE(TAG, "SCLK GPIO cannot be unassigned."); ESP_LOGE(TAG, "Initialization failed, SCLK GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if((imu_config.io_rst == GPIO_NUM_NC)) if ((imu_config.io_rst == GPIO_NUM_NC))
{ {
ESP_LOGE(TAG, "RST GPIO cannot be unassigned."); ESP_LOGE(TAG, "RST GPIO cannot be unassigned.");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
// SPI bus config // SPI bus config
@ -146,16 +154,36 @@ esp_err_t BNO08x::initialize_config_args()
imu_spi_config.command_bits = 0; // 0 command 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 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 // 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 imu_spi_config.queue_size = static_cast<int>(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; 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) // configure output(s) (CS, RST, and WAKE)
gpio_config_t outputs_config; gpio_config_t outputs_config;
@ -171,26 +199,26 @@ esp_err_t BNO08x::initialize_gpio()
ret = gpio_config(&outputs_config); ret = gpio_config(&outputs_config);
if (ret != ESP_OK) if (ret != ESP_OK)
{ ESP_LOGE(TAG, "Initialization failed, failed to configure CS, RST, and WAKE (if used) gpio.");
ESP_LOGE(TAG, "Failed to configure CS, RST, and WAKE (if used) gpio."); else
return ret; init_status.gpio_outputs = true; // set gpio_inputs to initialized such that deconstructor knows to clean them up
}
//configure input(s) (HINT) return ret;
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); 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) if (ret != ESP_OK)
{
ESP_LOGE(TAG, "Failed to configure HINT gpio.");
return ret; return ret;
}
gpio_set_level(imu_config.io_cs, 1); gpio_set_level(imu_config.io_cs, 1);
gpio_set_level(imu_config.io_rst, 1); gpio_set_level(imu_config.io_rst, 1);
@ -201,7 +229,7 @@ esp_err_t BNO08x::initialize_gpio()
return ret; return ret;
} }
esp_err_t BNO08x::initialize_hint_isr() esp_err_t BNO08x::init_hint_isr()
{ {
esp_err_t ret = ESP_OK; esp_err_t ret = ESP_OK;
@ -211,23 +239,32 @@ esp_err_t BNO08x::initialize_hint_isr()
if (ret != ESP_OK) 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; 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); ret = gpio_isr_handler_add(imu_config.io_int, hint_handler, (void*) this);
if (ret != ESP_OK) if (ret != ESP_OK)
{ {
ESP_LOGE(TAG, "Add hint_handler ISR."); ESP_LOGE(TAG, "Initialization failed, failed to add hint_handler ISR.");
return ret; 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 gpio_intr_disable(imu_config.io_int); // disable interrupts initially before reset
return ret; return ret;
} }
esp_err_t BNO08x::initialize_spi() esp_err_t BNO08x::init_spi()
{ {
esp_err_t ret = ESP_OK; esp_err_t ret = ESP_OK;
uint8_t tx_buffer[50] = {0}; // for dummy transaction to stabilize SPI peripheral 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); ret = spi_bus_initialize(imu_config.spi_peripheral, &bus_config, SPI_DMA_CH_AUTO);
if (ret != ESP_OK) if (ret != ESP_OK)
{ {
ESP_LOGE(TAG, "SPI bus failed to initialize."); ESP_LOGE(TAG, "Initialization failed, SPI bus failed to initialize.");
return ret; return ret;
} }
else
{
init_status.spi_bus = true;
}
// add the imu device to the bus // add the imu device to the bus
ret = spi_bus_add_device(imu_config.spi_peripheral, &imu_spi_config, &spi_hdl); ret = spi_bus_add_device(imu_config.spi_peripheral, &imu_spi_config, &spi_hdl);
if (ret != ESP_OK) 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; return ret;
} }
else
{
init_status.spi_device = true;
}
// do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.) // do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.)
spi_transaction.length = 8; spi_transaction.length = 8;
@ -259,6 +304,122 @@ esp_err_t BNO08x::initialize_spi()
return ret; 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. * @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 * @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) * timer, 4 = external reset 5 = other)
*/ */
uint8_t BNO08x::get_reset_reason() IMUResetReason BNO08x::get_reset_reason()
{ {
uint32_t reset_reason = 0; uint32_t reset_reason = 0;
@ -464,7 +625,7 @@ uint8_t BNO08x::get_reset_reason()
ESP_LOGE(TAG, "Failed to receive product ID report."); ESP_LOGE(TAG, "Failed to receive product ID report.");
} }
return reset_reason; return static_cast<IMUResetReason>(reset_reason);
} }
/** /**
@ -3002,18 +3163,31 @@ esp_err_t BNO08x::launch_tasks()
task_created = xTaskCreate( task_created = xTaskCreate(
&data_proc_task_trampoline, "bno08x_data_processing_task", CONFIG_ESP32_BNO08X_DATA_PROC_TASK_SZ, this, 7, &data_proc_task_hdl); &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) 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; return ESP_ERR_INVALID_STATE;
} }
else 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() esp_err_t BNO08x::kill_all_tasks()
@ -3024,17 +3198,24 @@ esp_err_t BNO08x::kill_all_tasks()
xEventGroupClearBits( 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 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 (init_status.task_count != 0)
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."); if (init_status.spi_task)
return ESP_ERR_TIMEOUT; 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; return ESP_OK;

14
tests/BNO08xTestHelper.cpp
View File

@ -1 +1,13 @@
#include "BNO08xTestHelper.hpp" #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;
}

6
tests/BNO08xTestHelper.hpp
View File

@ -5,5 +5,9 @@
class BNO08xTestHelper class BNO08xTestHelper
{ {
public:
static void set_test_imu(BNO08x *imu);
static BNO08x* get_test_imu();
private:
static BNO08x *test_imu;
}; };

5
tests/BNO08xTestSuite.cpp
View File

@ -2,5 +2,8 @@
void BNO08xTestSuite::run_tests() void BNO08xTestSuite::run_tests()
{ {
BNO08x imu;
BNO08xTestHelper::set_test_imu(&imu);
} }