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

calibration routine fix, formatting changes

This commit is contained in:
myles-parfeniuk 2024-03-02 02:55:33 -08:00
parent aca0ee421d
commit a372e4cfdd
3 changed files with 991 additions and 824 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.clang-format
View File

@ -1,11 +1,11 @@
---
BasedOnStyle: Google
BasedOnStyle: LLVM
Standard: Cpp11
UseTab: Never
IndentWidth: 4
ConstructorInitializerIndentWidth: 8
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 8
AccessModifierOffset: -4
@ -16,7 +16,7 @@ ConstructorInitializerAllOnOneLineOrOnePerLine: false
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: Inline
AllowShortFunctionsOnASingleLine: None
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
@ -29,4 +29,8 @@ SpaceAfterCStyleCast: true
CommentPragmas: '^[/!]<'
ColumnLimit: 120
ColumnLimit: 150 # Adjusted column limit to prevent line breaks
BreakBeforeBraces: Allman
IndentAccessModifiers: true

607
BNO08x.cpp
View File

File diff suppressed because it is too large Load Diff

128
BNO08x.hpp
View File

@ -16,7 +16,8 @@
#include <cstring>
/// @brief SHTP protocol channels
enum channels_t {
enum channels_t
{
CHANNEL_COMMAND,
CHANNEL_EXECUTABLE,
CHANNEL_CONTROL,
@ -26,10 +27,16 @@ enum channels_t {
};
/// @brief Sensor accuracy returned during sensor calibration
enum sensor_accuracy_t { LOW_ACCURACY = 1, MED_ACCURACY, HIGH_ACCURACY };
enum class IMUAccuracy
{
LOW = 1,
MED,
HIGH
};
/// @brief IMU configuration settings passed into constructor
typedef struct bno08x_config_t {
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)
@ -54,7 +61,9 @@ typedef struct bno08x_config_t {
, io_rst(GPIO_NUM_7)
, io_wake(GPIO_NUM_NC)
, sclk_speed(2000000UL) // 2MHz SCLK speed
, debug_en(false) {}
, debug_en(false)
{
}
#else
/// @brief Default IMU configuration settings constructor for ESP32
bno08x_config_t()
@ -70,11 +79,12 @@ typedef struct bno08x_config_t {
// , sclk_speed(10000U), //clock slowed to see on AD2
, debug_en(false)
{}
{
}
#endif
/// @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, uint64_t sclk_speed, bool debug)
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, uint64_t sclk_speed, bool debug)
: spi_peripheral(spi_peripheral)
, io_mosi(io_mosi)
, io_miso(io_miso)
@ -86,11 +96,13 @@ typedef struct bno08x_config_t {
, sclk_speed(sclk_speed)
, debug_en(false)
{}
{
}
} bno08x_config_t;
class BNO08x {
public:
class BNO08x
{
public:
BNO08x(bno08x_config_t imu_config = default_imu_config);
bool initialize();
@ -127,8 +139,7 @@ public:
void enable_tap_detector(uint16_t time_between_reports);
void enable_step_counter(uint16_t time_between_reports);
void enable_stability_classifier(uint16_t time_between_reports);
void enable_activity_classifier(
uint16_t time_between_reports, uint32_t activities_to_enable, uint8_t (&activity_confidence_vals)[9]);
void enable_activity_classifier(uint16_t time_between_reports, uint32_t activities_to_enable, uint8_t (&activity_confidence_vals)[9]);
void enable_raw_accelerometer(uint16_t time_between_reports);
void enable_raw_gyro(uint16_t time_between_reports);
void enable_raw_magnetometer(uint16_t time_between_reports);
@ -241,7 +252,27 @@ public:
static const constexpr uint16_t FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED = 0xE309;
static const constexpr uint16_t FRS_RECORDID_ROTATION_VECTOR = 0xE30B;
private:
static const constexpr uint8_t TARE_AXIS_ALL = 0x07; ///< Tare all axes (used with tare now command)
static const constexpr uint8_t TARE_AXIS_Z = 0x04; ///< Tar yaw axis only (used with tare now command)
// 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_GAME_ROTATION_VECTOR = 1; ///<Tare game rotation vector
static const constexpr uint8_t TARE_GEOMAGNETIC_ROTATION_VECTOR = 2; ///< 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_AR_VR_STABILIZED_ROTATION_VECTOR = 4; ///< Tare ARVR stabilized rotation vector
static const constexpr uint8_t TARE_AR_VR_STABILIZED_GAME_ROTATION_VECTOR = 5; ///<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_ACCURACY_Q1 = 12; ///< Rotation vector accuracy estimate Q point (See SH-2 Ref. Manual 6.5.18)
static const constexpr int16_t ACCELEROMETER_Q1 = 8; ///< Acceleration Q point (See SH-2 Ref. Manual 6.5.9)
static const constexpr int16_t LINEAR_ACCELEROMETER_Q1 = 8; ///< Linear acceleration Q point (See SH-2 Ref. Manual 6.5.10)
static const constexpr int16_t GYRO_Q1 = 9; ///< Gyro Q point (See SH-2 Ref. Manual 6.5.13)
static const constexpr int16_t MAGNETOMETER_Q1 = 4; ///< Magnetometer Q point (See SH-2 Ref. Manual 6.5.16)
static const constexpr int16_t ANGULAR_VELOCITY_Q1 = 10; ///< Angular velocity Q point (See SH-2 Ref. Manual 6.5.44)
static const constexpr int16_t GRAVITY_Q1 = 8; ///< Gravity Q point (See SH-2 Ref. Manual 6.5.11)
private:
bool wait_for_device_int();
bool receive_packet();
void send_packet();
@ -250,23 +281,20 @@ private:
void queue_feature_command(uint8_t report_ID, uint16_t time_between_reports);
void queue_feature_command(uint8_t report_ID, uint16_t time_between_reports, uint32_t specific_config);
void queue_calibrate_command(uint8_t _to_calibrate);
void queue_tare_command(
uint8_t command, uint8_t axis = TARE_AXIS_ALL, uint8_t rotation_vector_basis = TARE_ROTATION_VECTOR);
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();
static bno08x_config_t default_imu_config; ///< default imu config settings
volatile uint8_t
tx_packet_queued; ///<Whether or not a packet is currently waiting to be sent, a queued packet is sent on assertion of BNO08x HINT pin)
SemaphoreHandle_t
tx_semaphore; ///<Mutex semaphore used to prevent sending or receiving of packets if packet is currently being queued
SemaphoreHandle_t tx_semaphore; ///<Mutex semaphore used to prevent sending or receiving of packets if packet is currently being queued
uint8_t rx_buffer[300]; ///<buffer used to receive packet with receive_packet()
uint8_t tx_buffer[50]; ///<buffer used for sending packet with send_packet()
uint8_t packet_header_rx[4]; ///<SHTP header received with receive_packet()
uint8_t commands[20]; ///<Command to be sent with send_packet()
uint8_t sequence_number[6]; ///<Sequence num of each com channel, 6 in total
uint32_t meta_data
[9]; ///<First 9 bytes of meta data returned from FRS read operation (we don't really need the rest) (See Ref. Manual 5.1)
uint32_t meta_data[9]; ///<First 9 bytes of meta data returned from FRS read operation (we don't really need the rest) (See Ref. Manual 5.1)
uint8_t command_sequence_number = 0; ///<Sequence num of command, sent within command packet.
uint16_t packet_length_tx = 0; ///<Packet length to be sent with send_packet()
uint16_t packet_length_rx = 0; ///<Packet length received (calculated from packet_header_rx)
@ -312,35 +340,19 @@ private:
static void spi_task_trampoline(void* arg);
void spi_task();
volatile bool
int_asserted; ///<Interrupt asserted flag, sets true after hint_handler ISR launches SPI task and it has run to completion
volatile bool int_asserted; ///<Interrupt asserted flag, sets true after hint_handler ISR launches SPI task and it has run to completion
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 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 ACCELEROMETER_Q1 = 8; ///< Acceleration Q point (See SH-2 Ref. Manual 6.5.9)
static const constexpr int16_t LINEAR_ACCELEROMETER_Q1 =
8; ///< Linear acceleration Q point (See SH-2 Ref. Manual 6.5.10)
static const constexpr int16_t GYRO_Q1 = 9; ///< Gyro Q point (See SH-2 Ref. Manual 6.5.13)
static const constexpr int16_t MAGNETOMETER_Q1 = 4; ///< Magnetometer Q point (See SH-2 Ref. Manual 6.5.16)
static const constexpr int16_t ANGULAR_VELOCITY_Q1 =
10; ///< Angular velocity Q point (See SH-2 Ref. Manual 6.5.44)
static const constexpr int16_t GRAVITY_Q1 = 8; ///< Gravity Q point (See SH-2 Ref. Manual 6.5.11)
static const constexpr uint64_t HOST_INT_TIMEOUT_US =
150000ULL; ///<Max wait between HINT being asserted by BNO08x before transaction is considered failed.
150000ULL; ///<Max wait between HINT being asserted by BNO08x before transaction is considered failed (in microseconds)
// 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 = 0; ///<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_MAG =
2; ///<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_MAG = 2; ///<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_ACCEL_GYRO_MAG =
4; ///<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
@ -348,17 +360,12 @@ private:
// Command IDs (see Ref. Manual 6.4)
static const constexpr uint8_t COMMAND_ERRORS = 1;
static const constexpr uint8_t COMMAND_COUNTER = 2;
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_INITIALIZE =
4; ///<Reinitialize sensor hub components See (SH2 Ref. Manual 6.4.5)
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_INITIALIZE = 4; ///<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_ME_CALIBRATE =
7; ///<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_OSCILLATOR =
10; ///<Retrieve oscillator type command (See SH2 Ref. Manual 6.4)
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_DCD_PERIOD_SAVE = 9; ///<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_CLEAR_DCD = 11; ///<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)
@ -381,8 +388,7 @@ private:
static const constexpr uint8_t SENSOR_REPORTID_UNCALIBRATED_GYRO = 0x07; ///< See SH2 Ref. Manual 6.5.14
static const constexpr uint8_t SENSOR_REPORTID_GAME_ROTATION_VECTOR = 0x08; ///< See SH2 Ref. Manual 6.5.19
static const constexpr uint8_t SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR = 0x09; ///< See SH2 Ref. Manual 6.5.20
static const constexpr uint8_t SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR =
0x2A; ///< See SH2 Ref. Manual 6.5.44
static const constexpr uint8_t SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR = 0x2A; ///< See SH2 Ref. Manual 6.5.44
static const constexpr uint8_t SENSOR_REPORTID_TAP_DETECTOR = 0x10; ///< See SH2 Ref. Manual 6.5.27
static const constexpr uint8_t SENSOR_REPORTID_STEP_COUNTER = 0x11; ///< See SH2 Ref. Manual 6.5.29
static const constexpr uint8_t SENSOR_REPORTID_STABILITY_CLASSIFIER = 0x13; ///< See SH2 Ref. Manual 6.5.31
@ -390,27 +396,13 @@ private:
static const constexpr uint8_t SENSOR_REPORTID_RAW_GYROSCOPE = 0x15; ///< See SH2 Ref. Manual 6.5.12
static const constexpr uint8_t SENSOR_REPORTID_RAW_MAGNETOMETER = 0x16; ///< See SH2 Ref. Manual 6.5.15
static const constexpr uint8_t SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER = 0x1E; ///< See SH2 Ref. Manual 6.5.36
static const constexpr uint8_t SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR =
0x28; ///< See SH2 Ref. Manual 6.5.42
static const constexpr uint8_t SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR =
0x29; ///< See SH2 Ref. Manual 6.5.43
static const constexpr uint8_t SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR = 0x28; ///< See SH2 Ref. Manual 6.5.42
static const constexpr uint8_t SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR = 0x29; ///< See SH2 Ref. Manual 6.5.43
// 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_PERSIST = 1; ///< 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_AXIS_ALL = 0x07; ///< Tare all axes (used with tare now command)
static const constexpr uint8_t TARE_AXIS_Z = 0x04; ///< Tar yaw axis only (used with tare now command)
// 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_GAME_ROTATION_VECTOR = 1; ///<Tare game rotation vector
static const constexpr uint8_t TARE_GEOMAGNETIC_ROTATION_VECTOR = 2; ///< 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_AR_VR_STABILIZED_ROTATION_VECTOR = 4; ///< Tare ARVR stabilized rotation vector
static const constexpr uint8_t TARE_AR_VR_STABILIZED_GAME_ROTATION_VECTOR =
5; ///<Tare ARVR stabilized game rotation vector
static const constexpr char* TAG = "BNO08x"; ///< Class tag used for serial print statements
};