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

more robust report handling, major refactoring

This commit is contained in:
myles-parfeniuk 2024-11-15 21:35:11 -08:00
parent 58639579ed
commit be5f8789b6
4 changed files with 597 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
include/BNO08x.hpp
View File

@ -8,9 +8,6 @@
#include <vector>
// esp-idf includes
#include <driver/gpio.h>
#include <driver/spi_common.h>
#include <driver/spi_master.h>
#include <esp_log.h>
#include <esp_rom_gpio.h>
#include <esp_timer.h>
@ -21,85 +18,8 @@
#include <freertos/semphr.h>
#include <rom/ets_sys.h>
/// @brief SHTP protocol channels
enum channels_t
{
CHANNEL_COMMAND,
CHANNEL_EXECUTABLE,
CHANNEL_CONTROL,
CHANNEL_REPORTS,
CHANNEL_WAKE_REPORTS,
CHANNEL_GYRO
};
/// @brief Sensor accuracy returned during sensor calibration
enum class IMUAccuracy
{
LOW = 1,
MED,
HIGH,
UNDEFINED
};
/// @brief Reason for previous IMU reset (returned by get_reset_reason())
enum class IMUResetReason
{
UNDEFINED, ///< Undefined reset reason, this should never occur and is an error.
POR, ///< Previous reset was due to power on reset.
INT_RST, ///< Previous reset was due to internal reset.
WTD, ///< Previous reset was due to watchdog timer.
EXT_RST, ///< Previous reset was due to external reset.
OTHER ///< Previous reset was due to power other reason.
};
/// @brief IMU configuration settings passed into constructor
typedef struct bno08x_config_t
{
spi_host_device_t spi_peripheral; ///<SPI peripheral to be used
gpio_num_t io_mosi; ///<MOSI GPIO pin (connects to BNO08x DI pin)
gpio_num_t io_miso; ///<MISO GPIO pin (connects to BNO08x SDA pin)
gpio_num_t io_sclk; ///<SCLK pin (connects to BNO08x SCL pin)
gpio_num_t io_cs; /// Chip select pin (connects to BNO08x CS pin)
gpio_num_t io_int; /// Host interrupt pin (connects to BNO08x INT 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)
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(bool install_isr_service = true)
: 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_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, bool install_isr_service = true)
: spi_peripheral(spi_peripheral)
, io_mosi(io_mosi)
, io_miso(io_miso)
, io_sclk(io_sclk)
, io_cs(io_cs)
, io_int(io_int)
, io_rst(io_rst)
, io_wake(io_wake)
, sclk_speed(sclk_speed)
, install_isr_service(install_isr_service)
{
}
} bno08x_config_t;
// in-house includes
#include "BNO08x_global_types.hpp"
class BNO08x
{
@ -308,6 +228,17 @@ class BNO08x
static const constexpr int16_t GRAVITY_Q1 = 8; ///< Gravity Q point (See SH-2 Ref. Manual 6.5.11)
private:
/// @brief SHTP protocol channels
enum channels_t
{
CHANNEL_COMMAND,
CHANNEL_EXECUTABLE,
CHANNEL_CONTROL,
CHANNEL_REPORTS,
CHANNEL_WAKE_REPORTS,
CHANNEL_GYRO
};
/// @brief Holds data that is received over spi.
typedef struct bno08x_rx_packet_t
{
@ -380,12 +311,34 @@ class BNO08x
void queue_tare_command(uint8_t command, uint8_t axis = TARE_AXIS_ALL, uint8_t rotation_vector_basis = TARE_ROTATION_VECTOR);
void queue_request_product_id_command();
uint16_t parse_packet(bno08x_rx_packet_t* packet);
//functions to parse packets received from bno08x
uint16_t parse_packet(bno08x_rx_packet_t* packet, bool& notify_users);
uint16_t parse_product_id_report(bno08x_rx_packet_t* packet);
uint16_t parse_frs_read_response_report(bno08x_rx_packet_t* packet);
uint16_t parse_feature_get_response_report(bno08x_rx_packet_t* packet);
uint16_t parse_input_report(bno08x_rx_packet_t* packet);
void parse_input_report_data(bno08x_rx_packet_t* packet, uint16_t* data, uint16_t data_length);
uint16_t parse_gyro_report(bno08x_rx_packet_t* packet);
uint16_t parse_command_report(bno08x_rx_packet_t* packet);
//functions to update data returned to user
void update_accelerometer_data(uint16_t* data, uint8_t status);
void update_lin_accelerometer_data(uint16_t* data, uint8_t status);
void update_gyro_data(uint16_t* data, uint8_t status);
void update_uncalib_gyro_data(uint16_t* data, uint8_t status);
void update_magf_data(uint16_t* data, uint8_t status);
void update_gravity_data(uint16_t* data, uint8_t status);
void update_rotation_vector_data(uint16_t* data, uint8_t status);
void update_step_counter_data(uint16_t* data);
void update_raw_accelerometer_data(uint16_t* data);
void update_raw_gyro_data(uint16_t* data);
void update_raw_magf_data(uint16_t* data);
void update_tap_detector_data(bno08x_rx_packet_t* packet);
void update_stability_classifier_data(bno08x_rx_packet_t* packet);
void update_personal_activity_classifier_data(bno08x_rx_packet_t* packet);
void update_command_data(bno08x_rx_packet_t* packet);
void update_integrated_gyro_rotation_vector_data(bno08x_rx_packet_t* packet);
// for debug
void print_header(bno08x_rx_packet_t* packet);
void print_packet(bno08x_rx_packet_t* packet);

74
include/BNO08x_global_types.hpp Normal file
View File

@ -0,0 +1,74 @@
#pragma once
#include <driver/gpio.h>
#include <driver/spi_common.h>
#include <driver/spi_master.h>
/// @brief Sensor accuracy returned during sensor calibration
enum class IMUAccuracy
{
LOW = 1,
MED,
HIGH,
UNDEFINED
};
/// @brief Reason for previous IMU reset (returned by get_reset_reason())
enum class IMUResetReason
{
UNDEFINED, ///< Undefined reset reason, this should never occur and is an error.
POR, ///< Previous reset was due to power on reset.
INT_RST, ///< Previous reset was due to internal reset.
WTD, ///< Previous reset was due to watchdog timer.
EXT_RST, ///< Previous reset was due to external reset.
OTHER ///< Previous reset was due to power other reason.
};
/// @brief IMU configuration settings passed into constructor
typedef struct bno08x_config_t
{
spi_host_device_t spi_peripheral; ///<SPI peripheral to be used
gpio_num_t io_mosi; ///<MOSI GPIO pin (connects to BNO08x DI pin)
gpio_num_t io_miso; ///<MISO GPIO pin (connects to BNO08x SDA pin)
gpio_num_t io_sclk; ///<SCLK pin (connects to BNO08x SCL pin)
gpio_num_t io_cs; /// Chip select pin (connects to BNO08x CS pin)
gpio_num_t io_int; /// Host interrupt pin (connects to BNO08x INT 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)
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(bool install_isr_service = true)
: 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_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, bool install_isr_service = true)
: spi_peripheral(spi_peripheral)
, io_mosi(io_mosi)
, io_miso(io_miso)
, io_sclk(io_sclk)
, io_cs(io_cs)
, io_int(io_int)
, io_rst(io_rst)
, io_wake(io_wake)
, sclk_speed(sclk_speed)
, install_isr_service(install_isr_service)
{
}
} bno08x_config_t;

30
include/BNO08x_macros.hpp
View File

@ -23,14 +23,14 @@
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->header[1]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->header[0])))
#define PARSE_PACKET_TIMESTAMP(packet_ptr) \
(UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[4]) << 24UL, 3UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[3]) << 16UL, 2UL) | \
(UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[4]) << 24UL, 3UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[3]) << 16UL, 2UL) | \
UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[2]) << 8UL, 1UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[1]), 0UL))
// product id report parsing
#define PARSE_PRODUCT_ID_REPORT_RESET_REASON(packet_ptr) UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[1]), 0UL)
#define PARSE_PRODUCT_ID_REPORT_SW_PART_NO(packet_ptr) \
(UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[7]) << 24UL, 3UL) | \
(UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[7]) << 24UL, 3UL) | \
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[6]) << 16UL, 2UL) | \
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[5]) << 8UL, 1UL) | \
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_ptr->body[4]), 0UL))
@ -48,25 +48,27 @@
#define PARSE_PRODUCT_ID_REPORT_SW_VERSION_MINOR(packet_ptr) UINT32_MSK_BYTE(static_cast<uint32_t>(packet->body[3]), 0UL)
// input report parsing
#define PARSE_INPUT_REPORT_RAW_QUAT_I(packet) \
// gyro report parsing
#define PARSE_GYRO_REPORT_RAW_QUAT_I(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[1]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[0])))
#define PARSE_INPUT_REPORT_RAW_QUAT_J(packet) \
#define PARSE_GYRO_REPORT_RAW_QUAT_J(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[3]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[2])))
#define PARSE_INPUT_REPORT_RAW_QUAT_K(packet) \
#define PARSE_GYRO_REPORT_RAW_QUAT_K(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[5]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[4])))
#define PARSE_INPUT_REPORT_RAW_QUAT_REAL(packet) \
#define PARSE_GYRO_REPORT_RAW_QUAT_REAL(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[7]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[6])))
#define PARSE_INPUT_REPORT_RAW_GYRO_VEL_X(packet) \
#define PARSE_GYRO_REPORT_RAW_GYRO_VEL_X(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[9]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[8])))
#define PARSE_INPUT_REPORT_RAW_GYRO_VEL_Y(packet) \
#define PARSE_GYRO_REPORT_RAW_GYRO_VEL_Y(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[11]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[10])))
#define PARSE_INPUT_REPORT_RAW_GYRO_VEL_Z(packet) \
#define PARSE_GYRO_REPORT_RAW_GYRO_VEL_Z(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[13]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[12])))
// input report parsing
#define PARSE_INPUT_REPORT_STATUS_BITS(packet) (packet->body[5 + 2] & 0x03U)
#define PARSE_INPUT_REPORT_REPORT_ID(packet) UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[5]))
#define PARSE_INPUT_REPORT_DATA_1(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[5 + 5]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[5 + 4])))
#define PARSE_INPUT_REPORT_DATA_2(packet) \
@ -80,6 +82,12 @@
#define PARSE_INPUT_REPORT_DATA_6(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->body[5 + 15]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->body[5 + 14])))
#define IS_ROTATION_VECTOR_REPORT(packet) \
((packet)->body[5] == SENSOR_REPORT_ID_ROTATION_VECTOR || (packet)->body[5] == SENSOR_REPORT_ID_GAME_ROTATION_VECTOR || \
(packet)->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR || \
(packet)->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR)
// frs read response report parsing
#define PARSE_FRS_READ_RESPONSE_REPORT_RECORD_ID(packet_body) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet_body[13]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet_body[12])))
#define PARSE_FRS_READ_RESPONSE_REPORT_DATA_1(packet_body) \
@ -87,4 +95,4 @@
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[5]) << 8UL, 1UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[4]), 0UL))
#define PARSE_FRS_READ_RESPONSE_REPORT_DATA_2(packet_body) \
(UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[11]) << 24UL, 3UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[10]) << 16UL, 2UL) | \
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[9]) << 8UL, 1UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[8]), 0UL))
UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[9]) << 8UL, 1UL) | UINT32_MSK_BYTE(static_cast<uint32_t>(packet_body[8]), 0UL))

590
source/BNO08x.cpp
View File

@ -836,7 +836,6 @@ void BNO08x::disable_report(uint8_t report_ID, const EventBits_t report_evt_grp_
Lets flush this response as it's currently detected as an invalid packet, we don't care about it.
It might be wise to detect the response for the respective report being disabled, but is probably not necessary.
*/
flush_rx_packets(3);
// no reports enabled, disable hint to avoid wasting processing time
if ((xEventGroupGetBits(evt_grp_report_en)) == 0)
@ -1234,10 +1233,13 @@ void BNO08x::register_cb(std::function<void()> cb_fxn)
* @brief Parses a packet received from bno08x, updating any data according to received reports.
*
* @param packet The packet to be parsed.
* @param notify_users Bool reference that is set to true if users should be notified of new data through callbacks/polling, false if packet is valid
* but users don't need to be notified.
* @return 0 if invalid packet.
*/
uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet, bool& notify_users)
{
notify_users = true;
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
@ -1252,7 +1254,8 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
break;
case SHTP_REPORT_GET_FEATURE_RESPONSE:
notify_users = false;
return parse_feature_get_response_report(packet);
break;
case SHTP_REPORT_FRS_READ_RESPONSE:
@ -1300,10 +1303,34 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
// clang-format on
// this will update the rawAccelX, etc variables depending on which feature report is found
return parse_input_report(packet);
return parse_gyro_report(packet);
break;
case CHANNEL_COMMAND:
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel command");
#endif
// clang-format on
// todo add proper handling
notify_users = false;
return 1;
break;
case CHANNEL_WAKE_REPORTS:
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, wake reports");
#endif
// clang-format on
// todo add proper handling
notify_users = false;
return 1;
break;
default:
break;
@ -1363,6 +1390,95 @@ uint16_t BNO08x::parse_frs_read_response_report(bno08x_rx_packet_t* packet)
return 1;
}
uint16_t BNO08x::parse_feature_get_response_report(bno08x_rx_packet_t* packet)
{
uint16_t report_id = 0;
switch (packet->body[1])
{
case SENSOR_REPORT_ID_ACCELEROMETER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_GYROSCOPE:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_MAGNETIC_FIELD:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_LINEAR_ACCELERATION:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_ROTATION_VECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_GRAVITY:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_UNCALIBRATED_GYRO:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_GAME_ROTATION_VECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_GEOMAGNETIC_ROTATION_VECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_TAP_DETECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_STEP_COUNTER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_STABILITY_CLASSIFIER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_RAW_ACCELEROMETER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_RAW_GYROSCOPE:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_RAW_MAGNETOMETER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR:
report_id = packet->body[0];
break;
case SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR:
report_id = packet->body[0];
break;
default:
break;
}
return report_id;
}
/**
* @brief Parses received input report sent by BNO08x.
*
@ -1380,182 +1496,155 @@ uint16_t BNO08x::parse_frs_read_response_report(bno08x_rx_packet_t* packet)
* packet->body[10:11]: real/gyro temp/etc
* packet->body[12:13]: Accuracy estimate
*
* @param packet bno8x_rx_packet_t containing the input report to parse
*
* @return The report ID of the respective sensor, for ex. SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR, 0 if invalid.
*/
uint16_t BNO08x::parse_input_report(bno08x_rx_packet_t* packet)
{
uint16_t i = 0;
uint8_t status = 0;
uint8_t command = 0;
// Calculate the number of data bytes in this packet
uint8_t status = PARSE_INPUT_REPORT_STATUS_BITS(packet);
uint16_t data_length = PARSE_PACKET_DATA_LENGTH(packet);
data_length &= ~(1U << 15U); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
uint16_t report_id = PARSE_INPUT_REPORT_REPORT_ID(packet);
uint16_t data[6] = {0};
// Ignore it for now. TODO catch this as an error and exit
data_length &= ~(1U << 15U); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
// ignore it for now. TODO catch this as an error and exit
data_length -= 4; // Remove the header bytes from the data count
time_stamp = PARSE_PACKET_TIMESTAMP(packet);
// The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence,
// and status fields
if (packet->header[2] == CHANNEL_GYRO)
{
raw_quat_I = PARSE_INPUT_REPORT_RAW_QUAT_I(packet);
raw_quat_J = PARSE_INPUT_REPORT_RAW_QUAT_J(packet);
raw_quat_K = PARSE_INPUT_REPORT_RAW_QUAT_K(packet);
raw_quat_real = PARSE_INPUT_REPORT_RAW_QUAT_REAL(packet);
raw_velocity_gyro_X = PARSE_INPUT_REPORT_RAW_GYRO_VEL_X(packet);
raw_velocity_gyro_Y = PARSE_INPUT_REPORT_RAW_GYRO_VEL_Y(packet);
raw_velocity_gyro_Z = PARSE_INPUT_REPORT_RAW_GYRO_VEL_Z(packet);
return SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR;
}
status = PARSE_INPUT_REPORT_STATUS_BITS(packet);
uint16_t data1 = PARSE_INPUT_REPORT_DATA_1(packet);
uint16_t data2 = PARSE_INPUT_REPORT_DATA_2(packet);
uint16_t data3 = PARSE_INPUT_REPORT_DATA_3(packet);
uint16_t data4 = 0;
uint16_t data5 = 0;
uint16_t data6 = 0;
if (data_length - 5 > 9)
{
data4 = PARSE_INPUT_REPORT_DATA_4(packet);
}
if (data_length - 5 > 11)
{
data5 = PARSE_INPUT_REPORT_DATA_5(packet);
}
if (data_length - 5 > 13)
{
data6 = PARSE_INPUT_REPORT_DATA_6(packet);
}
parse_input_report_data(packet, data, data_length);
// Store these generic values to their proper global variable
switch (packet->body[5])
switch (report_id)
{
case SENSOR_REPORT_ID_ACCELEROMETER:
accel_accuracy = status;
raw_accel_X = data1;
raw_accel_Y = data2;
raw_accel_Z = data3;
update_accelerometer_data(data, status);
break;
case SENSOR_REPORT_ID_LINEAR_ACCELERATION:
accel_lin_accuracy = status;
raw_lin_accel_X = data1;
raw_lin_accel_Y = data2;
raw_lin_accel_Z = data3;
update_lin_accelerometer_data(data, status);
break;
case SENSOR_REPORT_ID_GYROSCOPE:
gyro_accuracy = status;
raw_gyro_X = data1;
raw_gyro_Y = data2;
raw_gyro_Z = data3;
update_gyro_data(data, status);
break;
case SENSOR_REPORT_ID_UNCALIBRATED_GYRO:
uncalib_gyro_accuracy = status;
raw_uncalib_gyro_X = data1;
raw_uncalib_gyro_Y = data2;
raw_uncalib_gyro_Z = data3;
raw_bias_X = data4;
raw_bias_Y = data5;
raw_bias_Z = data6;
update_uncalib_gyro_data(data, status);
break;
case SENSOR_REPORT_ID_MAGNETIC_FIELD:
magf_accuracy = status;
raw_magf_X = data1;
raw_magf_Y = data2;
raw_magf_Z = data3;
update_magf_data(data, status);
break;
case SENSOR_REPORT_ID_TAP_DETECTOR:
tap_detector = packet->body[5 + 4]; // Byte 4 only
update_tap_detector_data(packet);
break;
case SENSOR_REPORT_ID_STEP_COUNTER:
step_count = data3; // Bytes 8/9
update_step_counter_data(data);
break;
case SENSOR_REPORT_ID_STABILITY_CLASSIFIER:
stability_classifier = packet->body[5 + 4]; // Byte 4 only
update_stability_classifier_data(packet);
break;
case SENSOR_REPORT_ID_PERSONAL_ACTIVITY_CLASSIFIER:
activity_classifier = packet->body[5 + 5]; // Most likely state
// Load activity classification confidences into the array
for (i = 0; i < 9; i++) // Hardcoded to max of 9. TODO - bring in array size
activity_confidences[i] = packet->body[5 + 6 + i]; // 5 bytes of timestamp, byte 6 is first confidence
// byte
update_personal_activity_classifier_data(packet);
break;
case SENSOR_REPORT_ID_RAW_ACCELEROMETER:
mems_raw_accel_X = data1;
mems_raw_accel_Y = data2;
mems_raw_accel_Z = data3;
update_raw_accelerometer_data(data);
break;
case SENSOR_REPORT_ID_RAW_GYROSCOPE:
mems_raw_gyro_X = data1;
mems_raw_gyro_Y = data2;
mems_raw_gyro_Z = data3;
update_raw_gyro_data(data);
break;
case SENSOR_REPORT_ID_RAW_MAGNETOMETER:
mems_raw_magf_X = data1;
mems_raw_magf_Y = data2;
mems_raw_magf_Z = data3;
update_raw_magf_data(data);
break;
case SHTP_REPORT_COMMAND_RESPONSE:
// The BNO080 responds with this report to command requests. It's up to use to remember which command we
// issued.
command = packet->body[5 + 2]; // This is the Command byte of the response
if (command == COMMAND_ME_CALIBRATE)
calibration_status = packet->body[5 + 5]; // R0 - Status (0 = success, non-zero = fail)
update_command_data(packet);
break;
case SENSOR_REPORT_ID_GRAVITY:
gravity_accuracy = status;
gravity_X = data1;
gravity_Y = data2;
gravity_Z = data3;
update_gravity_data(data, status);
break;
default:
if (packet->body[5] == SENSOR_REPORT_ID_ROTATION_VECTOR || packet->body[5] == SENSOR_REPORT_ID_GAME_ROTATION_VECTOR ||
packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_ROTATION_VECTOR ||
packet->body[5] == SENSOR_REPORT_ID_ARVR_STABILIZED_GAME_ROTATION_VECTOR)
if (IS_ROTATION_VECTOR_REPORT(packet))
{
quat_accuracy = status;
raw_quat_I = data1;
raw_quat_J = data2;
raw_quat_K = data3;
raw_quat_real = data4;
// Only available on rotation vector and ar/vr stabilized rotation vector,
// not game rot vector and not ar/vr stabilized rotation vector
raw_quat_radian_accuracy = data5;
}
else
{
// This sensor report ID is unhandled.
// See reference manual to add additional feature reports as needed
return 0;
update_rotation_vector_data(data, status);
}
break;
}
// TODO additional feature reports may be strung together. Parse them all.
return packet->body[5];
return report_id;
}
/**
* @brief Parses data from received input report.
*
* @param packet bno8x_rx_packet_t containing the input report to parse
* @param data uint16_t array to store parsed data in
* @param data_length length of data in bytes parsed from packet header
*
* @return The report ID of the respective sensor, for ex. SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR, 0 if invalid.
*/
void BNO08x::parse_input_report_data(bno08x_rx_packet_t* packet, uint16_t* data, uint16_t data_length)
{
data[0] = PARSE_INPUT_REPORT_DATA_1(packet);
data[1] = PARSE_INPUT_REPORT_DATA_2(packet);
data[2] = PARSE_INPUT_REPORT_DATA_3(packet);
if (data_length - 5 > 9)
{
data[3] = PARSE_INPUT_REPORT_DATA_4(packet);
}
if (data_length - 5 > 11)
{
data[4] = PARSE_INPUT_REPORT_DATA_5(packet);
}
if (data_length - 5 > 13)
{
data[5] = PARSE_INPUT_REPORT_DATA_6(packet);
}
}
/**
* @brief Parses received gyro report sent by BNO08x.
*
* Unit responds with packet that contains the following:
*
* packet->header[0:3]: First, a 4 byte header
* packet->body[0:1]: Raw quaternion component I
* packet->body[2:3]: Raw quaternion component J
* packet->body[4:5]: Raw quaternion component K
* packet->body[6:7]: Raw quaternion real component
* packet->body[8:9]: Raw gyroscope angular velocity in X axis
* packet->body[10:11]: Raw gyroscope angular velocity in Y axis
* packet->body[12:13]: Raw gyroscope angular velocity in Z axis
*
* @return Integrated rotation vector report ID (always valid)
*/
uint16_t BNO08x::parse_gyro_report(bno08x_rx_packet_t* packet)
{
// calculate the number of data bytes in this packet
uint16_t data_length = PARSE_PACKET_DATA_LENGTH(packet);
data_length &= ~(1U << 15U); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
// the gyro-integrated input reports are sent via the special gyro channel and do not include the usual ID, sequence, and status fields
update_integrated_gyro_rotation_vector_data(packet);
return SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR;
}
/**
@ -1587,6 +1676,257 @@ uint16_t BNO08x::parse_command_report(bno08x_rx_packet_t* packet)
return 0;
}
/**
* @brief Updates accelerometer data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_accelerometer_data(uint16_t* data, uint8_t status)
{
accel_accuracy = status;
raw_accel_X = data[0];
raw_accel_Y = data[1];
raw_accel_Z = data[2];
}
/**
* @brief Updates linear accelerometer data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_lin_accelerometer_data(uint16_t* data, uint8_t status)
{
accel_lin_accuracy = status;
raw_lin_accel_X = data[0];
raw_lin_accel_Y = data[1];
raw_lin_accel_Z = data[2];
}
/**
* @brief Updates linear gyro data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_gyro_data(uint16_t* data, uint8_t status)
{
gyro_accuracy = status;
raw_gyro_X = data[0];
raw_gyro_Y = data[1];
raw_gyro_Z = data[2];
}
/**
* @brief Updates uncalibrated gyro data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_uncalib_gyro_data(uint16_t* data, uint8_t status)
{
uncalib_gyro_accuracy = status;
raw_uncalib_gyro_X = data[0];
raw_uncalib_gyro_Y = data[1];
raw_uncalib_gyro_Z = data[2];
raw_bias_X = data[3];
raw_bias_Y = data[4];
raw_bias_Z = data[5];
}
/**
* @brief Updates magnetic field data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_magf_data(uint16_t* data, uint8_t status)
{
magf_accuracy = status;
raw_magf_X = data[0];
raw_magf_Y = data[1];
raw_magf_Z = data[2];
}
/**
* @brief Updates gravity data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_gravity_data(uint16_t* data, uint8_t status)
{
gravity_accuracy = status;
gravity_X = data[0];
gravity_Y = data[1];
gravity_Z = data[2];
}
/**
* @brief Updates roation vector data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
* @param status uint8_t containing parsed status bits (represents accuracy)
*
* @return void, nothing to return
*/
void BNO08x::update_rotation_vector_data(uint16_t* data, uint8_t status)
{
quat_accuracy = status;
raw_quat_I = data[0];
raw_quat_J = data[1];
raw_quat_K = data[2];
raw_quat_real = data[3];
// Only available on rotation vector and ar/vr stabilized rotation vector,
// not game rot vector and not ar/vr stabilized rotation vector
raw_quat_radian_accuracy = data[4];
}
/**
* @brief Updates step counter data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
*
* @return void, nothing to return
*/
void BNO08x::update_step_counter_data(uint16_t* data)
{
step_count = data[2]; // bytes 8/9
}
/**
* @brief Updates raw accelerometer data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
*
* @return void, nothing to return
*/
void BNO08x::update_raw_accelerometer_data(uint16_t* data)
{
mems_raw_accel_X = data[0];
mems_raw_accel_Y = data[1];
mems_raw_accel_Z = data[2];
}
/**
* @brief Updates raw gyro data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
*
* @return void, nothing to return
*/
void BNO08x::update_raw_gyro_data(uint16_t* data)
{
mems_raw_gyro_X = data[0];
mems_raw_gyro_Y = data[1];
mems_raw_gyro_Z = data[2];
}
/**
* @brief Updates raw magnetic field data from parsed input report.
*
* @param data uint16_t array containing parsed input report data.
*
* @return void, nothing to return
*/
void BNO08x::update_raw_magf_data(uint16_t* data)
{
mems_raw_magf_X = data[0];
mems_raw_magf_Y = data[1];
mems_raw_magf_Z = data[2];
}
/**
* @brief Updates tap detector data from parsed input report.
*
* @param packet bno08x_rx_packet_t containing the packet with tap detector report.
*
* @return void, nothing to return
*/
void BNO08x::update_tap_detector_data(bno08x_rx_packet_t* packet)
{
tap_detector = packet->body[5 + 4]; // Byte 4 only
}
/**
* @brief Updates stability classifier data from parsed input report.
*
* @param packet bno08x_rx_packet_t containing the packet with stability classifier report.
*
* @return void, nothing to return
*/
void BNO08x::update_stability_classifier_data(bno08x_rx_packet_t* packet)
{
stability_classifier = packet->body[5 + 4]; // Byte 4 only
}
/**
* @brief Updates activity classifier data from parsed input report.
*
* @param packet bno08x_rx_packet_t containing the packet with activity classifier report.
*
* @return void, nothing to return
*/
void BNO08x::update_personal_activity_classifier_data(bno08x_rx_packet_t* packet)
{
activity_classifier = packet->body[5 + 5]; // Most likely state
// Load activity classification confidences into the array
for (int i = 0; i < 9; i++) // Hardcoded to max of 9. TODO - bring in array size
activity_confidences[i] = packet->body[5 + 6 + i]; // 5 bytes of timestamp, byte 6 is first confidence
// byte
}
/**
* @brief Updates command data from parsed input report.
*
* @param packet bno08x_rx_packet_t containing the packet with command response report.
*
* @return void, nothing to return
*/
void BNO08x::update_command_data(bno08x_rx_packet_t* packet)
{
uint8_t command = 0;
// the BNO080 responds with this report to command requests. It's up to use to remember which command we issued.
command = packet->body[5 + 2]; // This is the Command byte of the response
if (command == COMMAND_ME_CALIBRATE)
calibration_status = packet->body[5 + 5]; // R0 - Status (0 = success, non-zero = fail)
}
/**
* @brief Updates integrated gyro rotation vector data from SHTP channel 5 (CHANNEL_GYRO) special report data.
*
* @param packet bno08x_rx_packet_t containing the packet with command response report.
*
* @return void, nothing to return
*/
void BNO08x::update_integrated_gyro_rotation_vector_data(bno08x_rx_packet_t* packet)
{
raw_quat_I = PARSE_GYRO_REPORT_RAW_QUAT_I(packet);
raw_quat_J = PARSE_GYRO_REPORT_RAW_QUAT_J(packet);
raw_quat_K = PARSE_GYRO_REPORT_RAW_QUAT_K(packet);
raw_quat_real = PARSE_GYRO_REPORT_RAW_QUAT_REAL(packet);
raw_velocity_gyro_X = PARSE_GYRO_REPORT_RAW_GYRO_VEL_X(packet);
raw_velocity_gyro_Y = PARSE_GYRO_REPORT_RAW_GYRO_VEL_Y(packet);
raw_velocity_gyro_Z = PARSE_GYRO_REPORT_RAW_GYRO_VEL_Z(packet);
}
/**
* @brief Sends command to enable game rotation vector reports (See Ref. Manual 6.5.19)
*
@ -3311,6 +3651,7 @@ void BNO08x::data_proc_task_trampoline(void* arg)
void BNO08x::data_proc_task()
{
bno08x_rx_packet_t packet;
bool notify_users = false;
while (1) // receive packet from spi_task()
{
@ -3318,16 +3659,21 @@ void BNO08x::data_proc_task()
{
if (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
{
if (parse_packet(&packet) != 0) // check if packet is valid
if ((parse_packet(&packet, notify_users) != 0)) // check if packet is valid
{
// execute any registered callbacks
for (auto& cb_fxn : cb_list)
cb_fxn();
// get feature reports are valid packets but we don't need to notify the user unless they explicitly have requested them
if (notify_users)
{
// execute any registered callbacks
for (auto& cb_fxn : cb_list)
cb_fxn();
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET_BIT); // 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
{
print_packet(&packet);
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET_BIT); // indicated invalid packet to wait_for_data()
}
}