diff options
Diffstat (limited to 'include/bno055.hpp')
| -rw-r--r-- | include/bno055.hpp | 292 |
1 files changed, 292 insertions, 0 deletions
diff --git a/include/bno055.hpp b/include/bno055.hpp new file mode 100644 index 0000000..7fbac4d --- /dev/null +++ b/include/bno055.hpp @@ -0,0 +1,292 @@ +#pragma once + +#include <vector> +#include <string> + +#include "boards/pico_w.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include "pico/time.h" +#include "pico/types.h" + +#include <Eigen/Geometry> + +//Register addresses and data structs copied from Adafruit implementation +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} bno055_opmode_t; + +/** BNO055 Registers **/ +typedef enum { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR = 0X07, + + /* PAGE0 REGISTER DEFINITION START*/ + BNO055_CHIP_ID_ADDR = 0x00, + BNO055_ACCEL_REV_ID_ADDR = 0x01, + BNO055_MAG_REV_ID_ADDR = 0x02, + BNO055_GYRO_REV_ID_ADDR = 0x03, + BNO055_SW_REV_ID_LSB_ADDR = 0x04, + BNO055_SW_REV_ID_MSB_ADDR = 0x05, + BNO055_BL_REV_ID_ADDR = 0X06, + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, + BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, + BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, + BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, + BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, + BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, + BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, + BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, + BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, + BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, + BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, + BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, + BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, + BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, + BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, + BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR = 0X1A, + BNO055_EULER_H_MSB_ADDR = 0X1B, + BNO055_EULER_R_LSB_ADDR = 0X1C, + BNO055_EULER_R_MSB_ADDR = 0X1D, + BNO055_EULER_P_LSB_ADDR = 0X1E, + BNO055_EULER_P_MSB_ADDR = 0X1F, + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, + BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, + BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, + BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, + BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, + BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, + BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, + BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, + BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, + BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, + BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, + BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, + BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, + BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, + BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, + BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, + BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, + BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, + + /* Temperature data register */ + BNO055_TEMP_ADDR = 0X34, + + /* Status registers */ + BNO055_CALIB_STAT_ADDR = 0X35, + BNO055_SELFTEST_RESULT_ADDR = 0X36, + BNO055_INTR_STAT_ADDR = 0X37, + + BNO055_SYS_CLK_STAT_ADDR = 0X38, + BNO055_SYS_STAT_ADDR = 0X39, + BNO055_SYS_ERR_ADDR = 0X3A, + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR = 0X3B, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A, + + /* Interrupt registers*/ + RST_INT = 0x01, + + NO_MOTION_INT_ADDR = 0x00, + SLOW_NO_MOTION_INT_ADDR = 0x01, + THRESHOLD_INT_ADDR = 0x02, +} bno055_reg_t; + +/** BNO055 power settings */ +typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 +} bno055_powermode_t; + +/** Vector Mappings **/ +typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR +} vector_type_t; + +struct quarternion { + float w; + float x; + float y; + float z; +}; + +struct vector3f { + float x; + float y; + float z; +}; + +struct CALIB_STATUS { + uint8_t sys; + uint8_t gyro; + uint8_t accel; + uint8_t mag; +}; + +extern volatile vector3f linear_acceleration; +extern volatile vector3f acceleration; +extern volatile quarternion abs_quaternion; +extern volatile CALIB_STATUS calib_status; + +extern volatile vector3f euler_angles; +extern volatile vector3f abs_lin_accel; +extern volatile vector3f rot_y_vec; +extern volatile vector3f vel_at_angle; + +extern volatile vector3f accel_gravity; + +extern uint8_t accel[6]; +extern uint8_t quat[8]; + +class BNO055 { + public: + BNO055(); + //Sanity check for factory device ID + void reset_bno055(); + void init(); + void read_lin_accel(); + void read_abs_quaternion(); + void read_euler_angles(); + void read_accel(); + void read_calib_status(); + void calculate_abs_linear_acceleration(); + void accel_to_gravity(); + void quaternion_to_euler(); + void get_rotation_vector(); + void clamp_close_zero(volatile float &val); + + private: + unsigned char bno055_address; + int32_t _sensorID; + bno055_opmode_t default_mode; +}; |