Where to begin…. (#13)

+/- Reworked collection of altimeter related functions into altimeter class
+/- Reworked bno055 class to be imu class with minimal functionality

\- Removed external Kalman filter implementations in favor of own in house version
\- Removed any/unused files
\+ Added buffer logger for when sitting on pad for extended period of time in effort to prevent filling of flash chip
\+ Added heartbeat LED for alive status

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);
-}
-