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, 385 insertions, 0 deletions

diff --git a/src/unused/sensorIMU.cpp b/src/unused/sensorIMU.cpp
new file mode 100644
index 0000000..941ea35
--- /dev/null
+++ b/src/unused/sensorIMU.cpp
@@ -0,0 +1,385 @@
+#include "../include/sensorIMU.hpp"
+
+IMUSensor::IMUSensor(std::string I2C_FILE) {
+    this -> I2C_FILE = I2C_FILE;
+}
+
+bool IMUSensor::init(void* data) {
+
+    //I2C_File passed on object creation, stored in sensorI2C parent
+    setupI2C(I2C_FILE);
+    
+    //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second
+    //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that
+
+    //Sanity check for factory device ID 
+    uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR);
+    if (id != BNO055_ID) {
+        fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!");
+        return false;
+    }
+
+    //Set default operating mode of IMU into config from startup (will be set properly after config phase)
+    setModeHard(OPERATION_MODE_CONFIG);
+
+    //Writes 1 to the system reset bit in the trigger register
+    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20);
+    //Wait for reset to complete by doing sanity check again
+    while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID);
+
+    //Set power mode for sensor
+    writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
+
+    //Sensor chip uses two "pages" to multiplex register values
+    //Page 0 contains the sensor data (not configuration), which is what we want
+    writeRegister(BNO055_PAGE_ID_ADDR, 0);
+
+    //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess
+    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0);
+
+    setModeTemp(default_mode);
+
+    return true;
+}
+
+//Sets mode so it can be undone for temporary changes, like operation setting
+void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
+    currentMode = mode;
+    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
+}
+
+//Sets mode *AND* internal state variable
+void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
+    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
+}
+
+adafruit_bno055_opmode_t IMUSensor::getMode() {
+    return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR);
+}
+
+imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) {
+    imu::Vector<3> xyz;
+    uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6);
+
+    int16_t x, y, z;
+    x = y = z = 0;
+
+    /* Read vector data (6 bytes) */
+    x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
+    y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
+    z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
+
+    /*!
+    * Convert the value to an appropriate range (section 3.6.4)
+    * and assign the value to the Vector type
+    */
+    switch (vector_type) {
+        case VECTOR_MAGNETOMETER:
+            /* 1uT = 16 LSB */
+            xyz[0] = ((double)x) / 16.0;
+            xyz[1] = ((double)y) / 16.0;
+            xyz[2] = ((double)z) / 16.0;
+            break;
+        case VECTOR_GYROSCOPE:
+            /* 1dps = 16 LSB */
+            xyz[0] = ((double)x) / 16.0;
+            xyz[1] = ((double)y) / 16.0;
+            xyz[2] = ((double)z) / 16.0;
+            break;
+        case VECTOR_EULER:
+            /* 1 degree = 16 LSB */
+            xyz[0] = ((double)x) / 16.0;
+            xyz[1] = ((double)y) / 16.0;
+            xyz[2] = ((double)z) / 16.0;
+            break;
+        case VECTOR_ACCELEROMETER:
+            /* 1m/s^2 = 100 LSB */
+            xyz[0] = ((double)x) / 100.0;
+            xyz[1] = ((double)y) / 100.0;
+            xyz[2] = ((double)z) / 100.0;
+            break;
+        case VECTOR_LINEARACCEL:
+            /* 1m/s^2 = 100 LSB */
+            xyz[0] = ((double)x) / 100.0;
+            xyz[1] = ((double)y) / 100.0;
+            xyz[2] = ((double)z) / 100.0;
+            break;
+        case VECTOR_GRAVITY:
+            /* 1m/s^2 = 100 LSB */
+            xyz[0] = ((double)x) / 100.0;
+            xyz[1] = ((double)y) / 100.0;
+            xyz[2] = ((double)z) / 100.0;
+            break;
+    }
+
+    return xyz;
+}
+imu::Quaternion IMUSensor::getQuat() {
+    uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8);
+
+    int16_t x, y, z, w;
+    x = y = z = w = 0;
+
+    //Bit shift data into the right places and store it
+    w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
+    x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
+    y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
+    z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
+
+    /*!
+    * Assign to Quaternion
+    * See
+    * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
+    * 3.6.5.5 Orientation (Quaternion)
+    */
+    const double scale = (1.0 / (1 << 14));
+    imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
+    return quat;
+}
+
+int8_t IMUSensor::getTemp() {
+    int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR));
+    return temp;
+}
+
+void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) {
+    //Put into proper config for mapping stuff
+    setModeTemp(OPERATION_MODE_CONFIG);
+    writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
+
+    //Return mode to operating mode
+    setModeTemp(currentMode);
+}
+
+void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
+    //See above method, pretty much the exact same
+    setModeTemp(OPERATION_MODE_CONFIG);
+    writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
+    setModeTemp(currentMode);
+}    
+
+//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it.
+void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) {
+    //Make sure IMU is on proper register page to get system status
+    writeRegister(BNO055_PAGE_ID_ADDR, 0);
+    
+    //If system status requested, read the status.
+    if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR);
+    //If self test result requested, pull the self test results.
+    if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR);
+    //Finally, if there's an error pull and stash it.
+    if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR);
+}
+
+//Same as above method, byte pointers are fed into it as parameters that get populated by method.
+void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
+    uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR);
+    if (sys != NULL) {
+        *sys = (calData >> 6) & 0x03;
+    }
+    if (gyro != NULL) {
+        *gyro = (calData >> 4) & 0x03;
+    }
+    if (accel != NULL) {
+        *accel = (calData >> 2) & 0x03;
+    }
+    if (mag != NULL) {
+        *mag = calData & 0x03;
+    }
+}
+
+/* Functions to deal with raw calibration data */
+bool IMUSensor::getSensorOffsets(uint8_t *calibData) {
+    if (isFullyCalibrated()) {
+        setModeTemp(OPERATION_MODE_CONFIG);
+
+        calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS);
+
+        setModeTemp(currentMode);
+        return true;
+    }
+    return false;
+}
+
+//Fully populated offset getter using type of offset, not just calibration data
+bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) {
+    if (isFullyCalibrated()) {
+        setModeTemp(OPERATION_MODE_CONFIG);
+
+        /* Accel offset range depends on the G-range:
+        +/-2g  = +/- 2000 mg
+        +/-4g  = +/- 4000 mg
+        +/-8g  = +/- 8000 mg
+        +/-1§g = +/- 16000 mg */
+        offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
+                                    (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR));
+        offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
+                                    (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR));
+        offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
+                                    (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR));
+
+        /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
+        offsets_type.mag_offset_x =
+            (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR));
+        offsets_type.mag_offset_y =
+            (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR));
+        offsets_type.mag_offset_z =
+            (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR));
+
+        /* Gyro offset range depends on the DPS range:
+        2000 dps = +/- 32000 LSB
+        1000 dps = +/- 16000 LSB
+        500 dps = +/- 8000 LSB
+        250 dps = +/- 4000 LSB
+        125 dps = +/- 2000 LSB
+        ... where 1 DPS = 16 LSB */
+        offsets_type.gyro_offset_x =
+            (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR));
+        offsets_type.gyro_offset_y =
+            (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR));
+        offsets_type.gyro_offset_z =
+            (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR));
+
+        /* Accelerometer radius = +/- 1000 LSB */
+        offsets_type.accel_radius =
+            (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR));
+
+        /* Magnetometer radius = +/- 960 LSB */
+        offsets_type.mag_radius =
+            (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR));
+
+        setModeTemp(currentMode);
+        return true;
+    }
+    return false;
+}
+
+void IMUSensor::setSensorOffsets(const uint8_t *calibData) {
+    setModeTemp(OPERATION_MODE_CONFIG);
+
+    /* Note: Configuration will take place only when user writes to the last
+        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+        Therefore the last byte must be written whenever the user wants to
+        changes the configuration. */
+
+    /* A writeLen() would make this much cleaner */
+    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
+    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
+    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
+    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
+    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
+    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
+
+    writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
+    writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
+    writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
+    writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
+    writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
+    writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
+
+    writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
+    writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
+    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
+    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
+    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
+    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
+
+    writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
+    writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
+
+    writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]);
+    writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]);
+
+    setModeTemp(currentMode);
+}
+
+void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) {
+    setModeTemp(OPERATION_MODE_CONFIG);
+
+    /* Note: Configuration will take place only when user writes to the last
+        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+        Therefore the last byte must be written whenever the user wants to
+        changes the configuration. */
+
+    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
+    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
+    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
+    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
+    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
+    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
+
+    writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
+    writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
+    writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
+    writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
+    writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
+    writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
+
+    writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
+    writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
+    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
+    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
+    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
+    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
+
+    writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
+    writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
+
+    writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
+    writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
+
+    setModeTemp(currentMode);
+
+}
+
+bool IMUSensor::isFullyCalibrated() {
+    uint8_t system, gyro, accel, mag;
+    getCalibration(&system, &gyro, &accel, &mag);
+
+    switch (currentMode) {
+        case OPERATION_MODE_ACCONLY:
+            return (accel == 3);
+        case OPERATION_MODE_MAGONLY:
+            return (mag == 3);
+        case OPERATION_MODE_GYRONLY:
+        case OPERATION_MODE_M4G: /* No magnetometer calibration required. */
+            return (gyro == 3);
+        case OPERATION_MODE_ACCMAG:
+        case OPERATION_MODE_COMPASS:
+            return (accel == 3 && mag == 3);
+        case OPERATION_MODE_ACCGYRO:
+        case OPERATION_MODE_IMUPLUS:
+            return (accel == 3 && gyro == 3);
+        case OPERATION_MODE_MAGGYRO:
+            return (mag == 3 && gyro == 3);
+        default:
+            return (system == 3 && gyro == 3 && accel == 3 && mag == 3);
+    }   
+}
+
+/* Power managments functions */
+void IMUSensor::enterSuspendMode() {
+    /* Switch to config mode (just in case since this is the default) */
+    setModeTemp(OPERATION_MODE_CONFIG);
+    writeRegister(BNO055_PWR_MODE_ADDR, 0x02);
+    /* Set the requested operating mode (see section 3.3) */
+    setModeTemp(currentMode);
+}
+
+void IMUSensor::enterNormalMode() {
+    /* Switch to config mode (just in case since this is the default) */
+    setModeTemp(OPERATION_MODE_CONFIG);
+    writeRegister(BNO055_PWR_MODE_ADDR, 0x00);
+    /* Set the requested operating mode (see section 3.3) */
+    setModeTemp(modeback);
+}
+
+
+
+
+
+
+
+
+
+