Raspberry Pi Pico (#12)

* Adding a 90% completed, compilable but untested ADS

* Made basic changes to actuator & sensor. Also added motor class

* Removed unnecessary .cpp files

* Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions

* Relocated 'main' to 'active-drag-system.cpp'. Added more info to README

* Removed main.cpp

* Added more details to README

* Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace

* Started writing the test cases

* Updated the .gitignore file

* Removed some files that should be gitignored

* Up to date with Jazz's pull request

* Test Launch Branch Created; PRU Servo Control with Test Program

* Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION]

Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly.

* Partial implementation of MPL sensor

Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES.

* *Hypothetically* complete MPL implementation

NEEDS HARDWARE TESTING

* IMU Header and init() method implementation

Needs like, all data handling still lol

* Hypothetically functional (Definitely won't compile)

* We ball?

* Conversion to Raspberry Pi Pico Build System; Removed Beaglebone
specific code; Simple blinking example in ADS source file; builds for
Pico W

* Rearranged build so dependent upon cmake file already existing in pico-sdk; current executable prints current altitude, velocity, and time taken to read and calculate said values; ~320 us to do so

* Altimeter interrupt callback for Pad to Boost State; dummy templates for other callbacks with comments describing potential implementation details

* Altimeter interrupts relatively finished; need to test with vacuum chamber to verify behavior

* Established interrupt pins as pullup and active-low; adjusted callback functions to properly use function pointers; still need to verify interrupt system with vacuum chamber

* Removed weird artifact in .gitignore, adjust CMakeLists to auto pull pico sdk, added Dockerfile

* added Docker dev container file

* modified CMakeLists to auto pull sdk if not already downloaded, add build.sh script, fixed Dockerfile

* added bno055 support

* changed bno055 lin accel struct to use float instead of double

* added bno055 support not tested, but compiles, fixed CMakLists to before I messed with it

* added absolute quaternion output from bno055

* Added Euler and aboslute linear accelration

* Flash implementation for data logging; each log entry is 32 bytes long

* added base pwm functions and started on apogee detection

* State machine verified functional with logging capabilities; currently on same core

* Ooops missed double define, renamed LOOP_HZ to LOOP_PERIOD; State machine functional after merge still

* Simple test program to see servo PWM range; logging with semaphores for safe multithreading

* Kalman filters generously provided from various sources for temporary replacement; minimum deployment 30 percent; state machine functionality restored; multithreading logging verified; altimeter broke and replaced

* Stop logging on END state; provide deployment function with AGL instead of ASL altitude

* Various minimal changes; Flash size from 1MB to 8MB; M1939 to M2500T burn time; pin assignments for new PCB; External Status LED to Internal Status LED

---------

Co-authored-by: Jazz Jackson <[email protected]>
Co-authored-by: Cian Capacci <[email protected]>
Co-authored-by: Gregory Wainer <[email protected]>

1 files changed, 201 insertions, 0 deletions

diff --git a/src/bno055.cpp b/src/bno055.cpp
new file mode 100644
index 0000000..fb51986
--- /dev/null
+++ b/src/bno055.cpp
@@ -0,0 +1,201 @@
+#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);
+}
+