summaryrefslogtreecommitdiff
path: root/src/bno055.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/bno055.cpp')
-rw-r--r--src/bno055.cpp201
1 files changed, 0 insertions, 201 deletions
diff --git a/src/bno055.cpp b/src/bno055.cpp
deleted file mode 100644
index fb51986..0000000
--- a/src/bno055.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-#include "bno055.hpp"
-
-/// @link [Pico BNO055 Example](https://learnembeddedsystems.co.uk/bno005-i2c-example-code)
-
-BNO055::BNO055() {
- bno055_address = BNO055_ADDRESS_A;
- _sensorID = BNO055_ID;
- default_mode = OPERATION_MODE_NDOF;
-}
-
-void BNO055::reset_bno055() {
- uint8_t data[2];
- data[0] = BNO055_SYS_TRIGGER_ADDR;
- data[1] = 0x20; // Reset system
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
- sleep_ms(1000); // Wait 650ms for the sensor to reset
-}
-
-void BNO055::init() {
- sleep_ms(1000); // Wait 650ms for the sensor to reset
- uint8_t chip_id_addr = BNO055_CHIP_ID_ADDR;
- uint8_t id[1];
- i2c_write_blocking(i2c_default, bno055_address, &chip_id_addr, 1, false);
- i2c_read_blocking(i2c_default, bno055_address, id, 1, false);
- if (!id[0] == _sensorID) {
- printf("BNO055 not detected\n");
- }
-
- // Use internal oscillator
- uint8_t data[2];
- data[0] = BNO055_SYS_TRIGGER_ADDR;
- data[1] = 0x40; // Set to use internal oscillator
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
- // Reset all interrupt status bits
- data[0] = BNO055_SYS_TRIGGER_ADDR;
- data[1] = 0x01; // Reset interrupt status
- // 0x05 = Reset system
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
- // Set to normal power mode
- data[0] = BNO055_PWR_MODE_ADDR;
- data[1] = 0x00; // Normal power mode
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
- sleep_ms(50); // Wait 50ms for the sensor to switch to normal power mode
-
- // Page 25 of the datasheet
- // Default Axis Config
- data[0] = BNO055_AXIS_MAP_CONFIG_ADDR;
- data[1] = 0x24; // P1=Z, P2=Y, P3=X
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
- // Default Axis Sign
- data[0] = BNO055_AXIS_MAP_SIGN_ADDR;
- data[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
- // Set units to m/s^2
- data[0] = BNO055_UNIT_SEL_ADDR;
- data[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
- i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
- sleep_ms(30);
-
- //The default operation mode after power-on is CONFIGMODE
- // Set mode to NDOF
- // Takes 7ms to switch from CONFIG mode; see page 21 on datasheet (3.3)
- data[0] = BNO055_OPR_MODE_ADDR;
- data[1] = default_mode; // NDOF
- i2c_write_blocking(i2c_default, bno055_address, data, 2, false);
- sleep_ms(100);
-
-
-}
-
-void BNO055::read_calib_status() {
- uint8_t calib_stat_reg = BNO055_CALIB_STAT_ADDR;
- uint8_t calib_stat[1];
- i2c_write_blocking(i2c_default, bno055_address, &calib_stat_reg, 1, true);
- i2c_read_blocking(i2c_default, bno055_address, calib_stat, 1, false);
- calib_status.mag = ((calib_stat[0] & 0b00000011) >> 0);
- calib_status.accel = ((calib_stat[0] & 0b00001100) >> 2);
- calib_status.gyro = ((calib_stat[0] & 0b00110000) >> 4);
- calib_status.sys = ((calib_stat[0] & 0b11000000) >> 6);
-}
-
-void BNO055::read_lin_accel() {
- uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
- i2c_write_blocking(i2c_default, bno055_address, &lin_accel_reg, 1, true);
- i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
- int16_t x, y, z;
- x = y = z = 0;
- x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
- y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
- z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
- linear_acceleration.x = ((float)x) / 100.0;
- linear_acceleration.y = ((float)y) / 100.0;
- linear_acceleration.z = ((float)z) / 100.0;
-}
-
-void BNO055::clamp_close_zero(volatile float &val) {
- if (val < 0.01 && val > -0.01) {
- val = 0.0;
- }
-}
-
-void BNO055::accel_to_gravity() {
- accel_gravity.x = abs_lin_accel.x / 9.81;
- accel_gravity.y = abs_lin_accel.y / 9.81;
- accel_gravity.z = abs_lin_accel.z / 9.81;
-}
-
-void BNO055::read_abs_quaternion() {
- uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR;
- i2c_write_blocking(i2c_default, bno055_address, &quat_reg, 1, true);
- i2c_read_blocking(i2c_default, bno055_address, quat, 8, false);
- int16_t w, x, y, z;
- w = x = y = z = 0;
- w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8);
- x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8);
- y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8);
- z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8);
- abs_quaternion.w = ((float)w) / 16384.0; // 2^14 LSB
- abs_quaternion.x = ((float)x) / 16384.0;
- abs_quaternion.y = ((float)y) / 16384.0;
- abs_quaternion.z = ((float)z) / 16384.0;
-}
-
-void BNO055::read_euler_angles() {
- uint8_t euler[6];
- uint8_t euler_reg = BNO055_EULER_H_LSB_ADDR;
- i2c_write_blocking(i2c_default, bno055_address, &euler_reg, 1, true);
- i2c_read_blocking(i2c_default, bno055_address, euler, 6, false);
- /// @note heading = yaw
- int16_t heading, roll, pitch;
- heading = roll = pitch = 0;
- heading = ((int16_t)euler[0]) | (((int16_t)euler[1]) << 8);
- roll = ((int16_t)euler[2]) | (((int16_t)euler[3]) << 8);
- pitch = ((int16_t)euler[4]) | (((int16_t)euler[5]) << 8);
- euler_angles.x = ((float)roll) / 16.0;
- euler_angles.y = ((float)pitch) / 16.0;
- euler_angles.z = ((float)heading) / 16.0;
-}
-
-void BNO055::read_accel() {
- uint8_t accel[6];
- uint8_t accel_reg = BNO055_ACCEL_DATA_X_LSB_ADDR;
- i2c_write_blocking(i2c_default, bno055_address, &accel_reg, 1, true);
- i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
- int16_t x, y, z;
- x = y = z = 0;
- x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
- y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
- z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
- acceleration.x = ((float)x) / 100.0;
- acceleration.y = ((float)y) / 100.0;
- acceleration.z = ((float)z) / 100.0;
-}
-
-void BNO055::quaternion_to_euler() {
- Eigen::Quaternion<float> q;
- q.w() = abs_quaternion.w;
- q.x() = abs_quaternion.x;
- q.y() = abs_quaternion.y;
- q.z() = abs_quaternion.z;
- q.normalize();
- Eigen::Matrix3f m = q.toRotationMatrix();
- euler_angles.x = atan2f(m(2,1), m(2,2));
- euler_angles.y = asinf(-m(2,0));
- euler_angles.z = atan2f(m(1,0), m(0,0));
-}
-
-void BNO055::calculate_abs_linear_acceleration() {
- Eigen::Quaternion<float> q;
- q.w() = abs_quaternion.w;
- q.x() = abs_quaternion.x;
- q.y() = abs_quaternion.y;
- q.z() = abs_quaternion.z;
- // q.normalize();
- Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
- Eigen::Vector3f lin_accel;
- lin_accel.x() = linear_acceleration.x;
- lin_accel.y() = linear_acceleration.y;
- lin_accel.z() = linear_acceleration.z;
- abs_lin_accel.x = lin_accel.x() * rotation_matrix(0, 0) + lin_accel.y() * rotation_matrix(0, 1) + lin_accel.z() * rotation_matrix(0, 2);
- abs_lin_accel.y = lin_accel.x() * rotation_matrix(1, 0) + lin_accel.y() * rotation_matrix(1, 1) + lin_accel.z() * rotation_matrix(1, 2);
- abs_lin_accel.z = -1.0f * (lin_accel.x() * rotation_matrix(2, 0) + lin_accel.y() * rotation_matrix(2, 1) + lin_accel.z() * rotation_matrix(2, 2));
-}
-
-void BNO055::get_rotation_vector() {
- Eigen::Quaternion<float> q;
- q.w() = abs_quaternion.w;
- q.x() = abs_quaternion.x;
- q.y() = abs_quaternion.y;
- q.z() = abs_quaternion.z;
- q.normalize();
- Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
- rot_y_vec.x = rotation_matrix(1, 0);
- rot_y_vec.y = rotation_matrix(1, 1);
- rot_y_vec.z = rotation_matrix(1, 2);
-}
-
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-12 16:23:37 +0000