02/24/2024 Test Launch Version (BB Black) (#11)

* 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?

---------

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

18 files changed, 1638 insertions, 52 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ea820ce..0a50318 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -12,3 +12,38 @@ add_executable(pru2
 
 target_link_options(pru1 PRIVATE -static)
 target_link_options(pru2 PRIVATE -static)
+cmake_minimum_required(VERSION 3.16.3)
+
+include_directories( ../include )
+
+# Set Variables
+set(TARGET ActiveDragSystem)
+set(SOURCES active-drag-system.cpp ads.cpp actuationPlan.cpp surfaceFitModel.cpp rocketUtils.cpp sensorIMU.cpp sensorAltimeter.cpp motor.cpp logger.cpp kalmanfilter.cpp)
+
+
+# Create Executables & Link Dependencies
+add_executable(${TARGET} ${SOURCES})
+
+add_executable(${TARGET_B} ${SOURCES_B})
+target_link_libraries(${TARGET_B} PUBLIC gtest_main)
+add_test(NAME ${TARGET_B} COMMAND ${TARGET_B})
+
+add_executable(${TARGET_C} ${SOURCES_C})
+target_link_libraries(${TARGET_C} PUBLIC gtest_main)
+add_test(NAME ${TARGET_C} COMMAND ${TARGET_C})
+
+add_executable(${TARGET_D} ${SOURCES_D})
+target_link_libraries(${TARGET_D} PUBLIC gtest_main)
+add_test(NAME ${TARGET_D} COMMAND ${TARGET_D})
+
+add_executable(${TARGET_E} ${SOURCES_E})
+target_link_libraries(${TARGET_E} PUBLIC gtest_main)
+add_test(NAME ${TARGET_E} COMMAND ${TARGET_E})
+
+add_executable(${TARGET_F} ${SOURCES_F})
+target_link_libraries(${TARGET_F} PUBLIC gtest_main)
+add_test(NAME ${TARGET_F} COMMAND ${TARGET_F})
+
+add_executable(${TARGET_G} ${SOURCES_G})
+target_link_libraries(${TARGET_G} PUBLIC gtest_main)
+add_test(NAME ${TARGET_G} COMMAND ${TARGET_G})
diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp
index dce4f86..7b63faa 100644
--- a/src/active_drag_system.cpp
+++ b/src/active_drag_system.cpp
@@ -1,32 +1,14 @@
-/* 
- * Author: Dawsyn Schraiber <[email protected]>
- * Date:   09/02/2023
- *
- * MIT License
- * 
- * Copyright (c) 2023 Rocketry at Virginia Tech
- * 
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- * 
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- * 
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
 #include <iostream>
+#include <cstdlib>
+#include "../include/surfaceFitModel.hpp"
+#include "../include/actuationPlan.hpp"
+#include "../include/ads.hpp"
 
-int main(int argc, char *argv[]) {
-    std::cout << "ADS" << std::endl;
+int main() {
+    
+    SurfaceFitModel sfm = SurfaceFitModel();
+    ActuationPlan plan = ActuationPlan(sfm);
+    ADS ads = ADS(plan);
+    ads.run();
+    return EXIT_SUCCESS;
 }
diff --git a/src/actuationPlan.cpp b/src/actuationPlan.cpp
new file mode 100644
index 0000000..a987478
--- /dev/null
+++ b/src/actuationPlan.cpp
@@ -0,0 +1,60 @@
+#include "../include/actuationPlan.hpp"
+
+ActuationPlan::ActuationPlan() {}
+
+ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) {
+
+}
+
+
+void ActuationPlan::runPlan(Vehicle& rocket) {
+
+
+    if (rocket.imuReadFail || rocket.altiReadFail) {
+        rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment
+    }
+
+    rocket.fail_time = time(nullptr);
+
+    // 2024 Mission---------------------------------------------------------------------
+    if (rocket.status == GLIDE) {
+
+        // Fin deployment based on current drag coefficient value
+        try {
+            double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude);
+            cd = std::min(std::max(0.0, cd), 100.0);
+            rocket.deployment_angle = deploy_percentage_to_angle(cd);
+        }
+
+        // Full deployment during coasting
+        catch (...) {
+            rocket.deployment_angle = deploy_percentage_to_angle(0);
+            
+            if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) {
+                rocket.deployment_angle = deploy_percentage_to_angle(100);
+            }
+        }        
+    }
+
+    else if (rocket.status == APOGEE) {
+
+        rocket.deployment_angle = deploy_percentage_to_angle(50);
+    }
+
+    else {
+
+        rocket.deploy_time = time(nullptr);
+    }
+    // End 2024 Mission------------------------------------------------------------------
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/ads.cpp b/src/ads.cpp
new file mode 100644
index 0000000..5484970
--- /dev/null
+++ b/src/ads.cpp
@@ -0,0 +1,286 @@
+#include "../include/ads.hpp"
+
+
+// Private----------------------------------------------------------------------
+void ADS::logSummary() {
+    
+    std::string output_string = "" + state_for_log[rocket.status];
+
+    if (!rocket.altiInitFail && !rocket.altiReadFail) {
+
+        output_string += format_data(" ", rocket.filtered_altitude, 3);     
+    }
+
+    output_string += format_data(" ", rocket.deployment_angle, 2);    
+
+    if (!rocket.imuInitFail && !rocket.imuReadFail) {
+
+        output_string += format_data(" ", rocket.acceleration[2], 2);    
+        output_string += format_data(" ", rocket.filtered_velocity, 2);    
+    }
+
+    Logger::Get().log(output_string);
+}
+
+
+void ADS::updateOnPadAltitude() {
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+    double avg_alt = 0;
+    double alt_read_count = 0;
+
+    while (alt_read_count < COUNT_LIMIT) {
+        
+        altimeter.getData(&rocket.current_altitude);
+        alt_read_count++;
+        avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count;
+    }
+ 
+    Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3));
+    rocket.ON_PAD_altitude = avg_alt;
+}
+
+
+void ADS::updateSensorData() {
+
+    if (!rocket.imuInitFail) {
+        
+        try {
+            imu.getData((void*)&rocket);
+        }
+
+        catch (...) {
+            std::exception_ptr e = std::current_exception();
+            Logger::Get().logErr(e.__cxa_exception_type()->name());
+            rocket.imuReadFail = true;
+        }
+    }  
+
+    rocket.previous_altitude = rocket.current_altitude; // Why was this placed here????
+    
+    if (!rocket.altiInitFail) {
+        
+        try {
+            altimeter.getData((void*)&rocket.current_altitude);
+            if (rocket.ON_PAD_fail) {
+                rocket.ON_PAD_altitude = rocket.current_altitude;
+                rocket.ON_PAD_fail = false;
+            }
+
+            rocket.altiReadFail = false;
+        }
+
+        catch (...) {
+            std::exception_ptr e = std::current_exception();
+            Logger::Get().logErr(e.__cxa_exception_type()->name());
+            rocket.altiReadFail = true;
+        }
+    }
+}
+
+
+void ADS::updateRocketState() {
+
+    // Filter sensor data
+    VectorXf control_input(1);
+    VectorXf measurement(1);
+    control_input << rocket.acceleration[2];
+    measurement << rocket.current_altitude;
+    VectorXf filtered = kf.run(control_input, measurement, rocket.dt);
+    rocket.filtered_altitude = filtered(0);
+    rocket.filtered_velocity = filtered(1);
+
+    if (rocket.apogee_altitude < rocket.filtered_altitude) {
+        rocket.apogee_altitude = rocket.filtered_altitude;
+    }
+
+    // (VEHICLE ON PAD)
+    if (rocket.status == ON_PAD) {
+
+        // If launch detected
+        if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0 
+            && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) {
+            Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
+        }
+
+        if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) {
+            Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
+        }
+
+        if (time(nullptr) - rocket.relog_time > 2*60*60 
+            && rocket.status == ON_PAD) {
+            std::cout << "OverWR Success" << std::endl;
+        }
+    }
+
+    // (VEHICLE BOOSTING)
+    else if (rocket.status == BOOST) {
+
+        if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0 
+            || time(nullptr) - rocket.liftoff_time >= TIME_BO) {
+            rocket.status = GLIDE;
+        }
+        
+    }
+
+    // (VEHICLE IN GLIDE)
+    else if (rocket.status == GLIDE) {
+
+        if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE 
+            || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) {
+            rocket.status = APOGEE;
+            Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2));
+        }
+    }
+
+    // (VEHICLE AT APOGEE)
+    else if (rocket.status == APOGEE) {
+
+        if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) {
+            rocket.status = DONE;
+            return;
+        }
+    }
+}
+
+
+// Public----------------------------------------------------------------------
+ADS::ADS(ActuationPlan plan) : plan(plan) {
+
+    rocket.status = ON_PAD;
+
+    rocket.apogee_altitude = 0;
+    rocket.previous_altitude = 0;
+    rocket.current_altitude = 0;
+    rocket.filtered_altitude = 0;
+
+    rocket.filtered_velocity = 0;
+
+    rocket.duty_span = DUTY_MAX - DUTY_MIN;
+    rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); 
+
+    rocket.dt = 0.1;
+
+    rocket.imuInitFail = false;
+    rocket.imuReadFail = false;
+    rocket.altiInitFail = false;
+    rocket.altiReadFail = false;
+
+    rocket.ON_PAD_altitude = 0;
+    rocket.ON_PAD_fail = false;
+
+    rocket.start_time = time(nullptr);
+    rocket.fail_time = rocket.start_time;
+    rocket.relog_time = rocket.start_time;
+    rocket.led_time = rocket.start_time;
+
+    imu = IMUSensor();
+    altimeter = AltimeterSensor();
+    motor = Motor();
+    kf = KalmanFilter(2, 1, 1, rocket.dt);
+
+    Logger::Get().openLog(LOG_FILENAME);
+    
+    motor.init(&rocket);
+
+    imu.init(nullptr);
+    altimeter.init(nullptr);
+
+    if (TEST_MODE) {
+
+        Logger::Get().log("TEST Record Start --");
+    }
+}
+
+
+
+void ADS::run() {
+
+    if (!rocket.altiInitFail) {
+        try {
+            updateOnPadAltitude();
+        }
+
+        catch (...) {
+            std::exception_ptr e = std::current_exception();
+            Logger::Get().logErr(e.__cxa_exception_type()->name());
+            rocket.ON_PAD_fail = true;
+        }
+    }
+
+    rocket.loop_time = time(nullptr);
+    while (rocket.status != DONE) {
+
+        updateSensorData();
+
+        if (!rocket.imuInitFail && !rocket.altiInitFail) {
+
+            updateRocketState();
+
+            // Run the Actuation Plan----------------------------------
+            plan.runPlan(rocket);
+
+            if (rocket.imuReadFail || rocket.altiReadFail) {
+
+                if (rocket.imuReadFail) {
+                    imu.init(nullptr); // Restart
+                    Logger::Get().log("Altimeter reset attempt");
+                }
+
+                if (rocket.altiReadFail) {
+                    altimeter.init(nullptr); // Restart
+                    Logger::Get().log("IMU reset attempt");
+                }
+            }
+        }
+
+        // Altimeter or IMU setup failed. Attempt to reinitialize
+        else {
+
+            if (time(nullptr) - rocket.fail_time >= TIME_END) {
+                rocket.status = DONE;
+            }
+
+            if (rocket.altiInitFail || rocket.altiReadFail) {
+                imu.init(nullptr); // Restart
+                Logger::Get().log("Altimeter reset attempt");
+            }
+
+            if (rocket.imuInitFail || rocket.imuReadFail) {
+                altimeter.init(nullptr); // Restart
+                Logger::Get().log("IMU reset attempt");
+            }
+
+            rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT);
+        }
+
+        // Actuate Servos
+        motor.writeData(&rocket);
+
+        logSummary();
+
+        // Blink Beaglebone LED 1
+        if (time(nullptr) - rocket.led_time > LED_GAP_TIME) {
+            led_out(&rocket);
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        rocket.dt = time(nullptr) - rocket.loop_time;
+        rocket.loop_time = time(nullptr);
+    }
+
+    Logger::Get().closeLog();
+    std::cout << "Done" << std::endl;
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp
new file mode 100644
index 0000000..735e8c8
--- /dev/null
+++ b/src/kalmanfilter.cpp
@@ -0,0 +1,106 @@
+#include "../include/kalmanfilter.hpp"
+
+// Private----------------------------------------------------------------------
+void KalmanFilter::matrixInit() {
+
+    state_vector.setZero(n);
+    state_covariance.setZero(n, n); 
+    state_transition_M = MatrixXf::Zero(n, n);  
+    control_input_M = MatrixXf::Zero(n, p);
+    I = MatrixXf::Identity(n, n);
+    measurement_M.setIdentity(m, n); // Setup Measurement Matrix
+    process_noise_covariance = MatrixXf::Zero(n, n); 
+    measurement_covariance = MatrixXf::Zero(m, m); 
+
+    // Setup State Transition Matrix
+    state_transition_M << 1.0, dt, 
+        0.0, 1.0;
+    
+    // Setup Control Input Matrix
+    control_input_M << 0.5 * std::pow(dt, 2), // (Linear Displacement Eq.)
+        dt; 
+    
+    // Setup Process-Noise Covariance
+    process_noise_covariance(0,0) = 0.01;
+    process_noise_covariance(1,1) = 0.1;
+
+    // Setup Measurement Covariance
+    measurement_covariance << 0.1;
+}
+
+
+void KalmanFilter::updateMatrices() {
+
+    state_transition_M(0, 1) = dt;
+    control_input_M(0, 0) = 0.5 * std::pow(dt, 2);
+    control_input_M(1, 0) = dt;
+}
+
+
+void KalmanFilter::prediction(VectorXf control_vec) {
+
+    state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec);
+    state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance;
+}
+
+void KalmanFilter::update(VectorXf measurement) {
+
+    // Innovation
+    VectorXf y = measurement - (measurement_M * state_vector);
+
+    // Residual/Innovation Covariance
+    MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance;
+
+    // Kalman Gain
+    MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse();
+
+    // Update
+    state_vector = state_vector + (K * y);
+    state_covariance = (I - (K * measurement_M)) * state_covariance;
+}
+
+
+
+// Public----------------------------------------------------------------------
+KalmanFilter::KalmanFilter() {
+    
+}
+
+
+KalmanFilter::KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt) 
+    : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) {
+
+    matrixInit();
+}
+
+bool KalmanFilter::setInitialState(VectorXf state_vec, MatrixXf state_cov) {
+
+    if (state_vec.size() != n || state_cov.rows() != n) {
+        std::cout << "Error: Max State & Covariance Dimension should be " << n << std::endl;
+        return false;
+    }
+
+    state_vector = state_vec;
+    state_covariance = state_cov;
+    return true;
+}
+
+
+
+
+VectorXf KalmanFilter::run(VectorXf control, VectorXf measurement, double _dt) {
+    
+     if (control.size() != p || measurement.size() != m) {
+        std::cout << "Error: Control Vector Size should be "<< p 
+            << " Measurement Vector Size should be " << m << std::endl;
+        return state_vector;
+    }
+
+    dt = _dt;
+    updateMatrices();
+
+    prediction(control);
+    update(measurement);
+
+    return state_vector;
+}
\ No newline at end of file
diff --git a/src/logger.cpp b/src/logger.cpp
new file mode 100644
index 0000000..a857be8
--- /dev/null
+++ b/src/logger.cpp
@@ -0,0 +1,132 @@
+#include "../include/logger.hpp"
+
+// Private----------------------------------------------------------------------
+std::string Logger::getDate() {
+
+    t = time(nullptr);
+    now = localtime(&t);
+    return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " 
+        + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")";
+}
+
+std::string Logger::getTime() {
+
+    t = time(nullptr);
+    now = localtime(&t);
+    std::string hour = std::to_string(now->tm_hour);
+    std::string min = std::to_string(now->tm_min);
+    std::string sec = std::to_string(now->tm_sec);
+    //string hour = "0" + to_string(now->tm_hour);
+
+    if (now->tm_hour < 10) {
+        hour = "0" + std::to_string(now->tm_hour);
+    }
+
+    if (now->tm_min < 10) {
+        min = "0" + std::to_string(now->tm_min);
+    }
+
+    if (now->tm_sec < 10) {
+        sec = "0" + std::to_string(now->tm_sec);
+    }
+
+    return hour + ":" + min + 
+        ":" + sec;
+}
+
+
+// Public----------------------------------------------------------------------
+Logger& Logger::Get() {
+
+    static Logger loggerSingleton;
+    return loggerSingleton;
+}
+
+//Logger Logger::loggerSingleton;
+
+
+bool Logger::openLog(std::string _filename) {
+
+    filename = _filename;
+    
+    if (file_open) {
+        return false;
+    }
+    
+    file.open(filename, std::ios::in | std::ios::out | std::ios::app);
+
+    if (!file) {
+        return false;
+    }
+
+    file_open = true;
+    std::string date = getDate();
+    std::string timestamp = getTime();
+    file << timestamp << infoTag << "Log Start---- " << date << std::endl;
+
+    return true;
+}
+
+
+void Logger::closeLog() {
+
+    std::string timestamp = getTime();
+    file << timestamp << infoTag << "Log End----\n\n";
+
+    file.close();
+    file_open = false;
+}
+
+
+bool Logger::log(std::string data) {
+
+    if (!file) {
+        return false;
+    }
+
+    if (!file_open) {
+        return false;
+    }
+    std::string timestamp = getTime();
+    file << timestamp << infoTag << data << std::endl;
+    return true;
+}
+
+bool Logger::logErr(std::string data) {
+
+    if (!file) {
+        return false;
+    }
+
+    if (!file_open) {
+        return false;
+    }
+
+    std::string timestamp = getTime();
+    file << timestamp << errorTag << data << std::endl;
+    return true;
+}
+
+
+bool Logger::printLog() {
+
+    if (file.is_open()) {
+        std::cout << "Log still open. Please close Log." << std::endl;
+        return false;
+    }
+
+    file.open(filename, std::ios::in);
+
+    if (!file.is_open()) {
+        return false;
+    }
+
+    std::string line;
+    while(getline(file, line)) {
+        std::cout << line << std::endl;
+    }
+
+    file.close();
+
+    return true;
+}
diff --git a/src/motor.cpp b/src/motor.cpp
new file mode 100644
index 0000000..84785a9
--- /dev/null
+++ b/src/motor.cpp
@@ -0,0 +1,46 @@
+#include "../include/motor.hpp"
+
+
+
+Motor::Motor() {
+
+
+}
+
+bool Motor::init(void* data) {
+
+    Vehicle *vehicle = (Vehicle *) data;
+    double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN);
+
+    // Initialize stuff
+    // .....
+    // .....
+
+
+    data = (void*) vehicle; // Is this necessary?
+    return true;
+}
+
+
+bool Motor::writeData(void* data) {
+
+    Vehicle *vehicle = (Vehicle *) data;
+    double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN);
+
+    // Send the Data somewhere
+    // ..... Pin
+    // ..... Duty
+    // ..... PWM frequency Hz
+    // ..... Polarity
+    
+
+    if (1 == 2) {
+        Logger::Get().logErr("Some type of Error");
+        return false;
+    }
+
+    data = (void*) vehicle; // Is this necessary?
+    return true;
+}
+
+
diff --git a/src/pru/AM335x_PRU.cmd b/src/pru/AM335x_PRU.cmd
new file mode 100644
index 0000000..b62f044
--- /dev/null
+++ b/src/pru/AM335x_PRU.cmd
@@ -0,0 +1,86 @@
+/****************************************************************************/
+/*  AM335x_PRU.cmd                                                          */
+/*  Copyright (c) 2015  Texas Instruments Incorporated                      */
+/*                                                                          */
+/*    Description: This file is a linker command file that can be used for  */
+/*                 linking PRU programs built with the C compiler and       */
+/*                 the resulting .out file on an AM335x device.             */
+/****************************************************************************/
+
+-cr								/* Link using C conventions */
+
+/* Specify the System Memory Map */
+MEMORY
+{
+      PAGE 0:
+	PRU_IMEM		: org = 0x00000000 len = 0x00002000  /* 8kB PRU0 Instruction RAM */
+
+      PAGE 1:
+
+	/* RAM */
+
+	PRU_DMEM_0_1	: org = 0x00000000 len = 0x00002000 CREGISTER=24 /* 8kB PRU Data RAM 0_1 */
+	PRU_DMEM_1_0	: org = 0x00002000 len = 0x00002000	CREGISTER=25 /* 8kB PRU Data RAM 1_0 */
+
+	  PAGE 2:
+	PRU_SHAREDMEM	: org = 0x00010000 len = 0x00003000 CREGISTER=28 /* 12kB Shared RAM */
+
+	DDR			    : org = 0x80000000 len = 0x00000100	CREGISTER=31
+	L3OCMC			: org = 0x40000000 len = 0x00010000	CREGISTER=30
+
+
+	/* Peripherals */
+
+	PRU_CFG			: org = 0x00026000 len = 0x00000044	CREGISTER=4
+	PRU_ECAP		: org = 0x00030000 len = 0x00000060	CREGISTER=3
+	PRU_IEP			: org = 0x0002E000 len = 0x0000031C	CREGISTER=26
+	PRU_INTC		: org = 0x00020000 len = 0x00001504	CREGISTER=0
+	PRU_UART		: org = 0x00028000 len = 0x00000038	CREGISTER=7
+
+	DCAN0			: org = 0x481CC000 len = 0x000001E8	CREGISTER=14
+	DCAN1			: org = 0x481D0000 len = 0x000001E8	CREGISTER=15
+	DMTIMER2		: org = 0x48040000 len = 0x0000005C	CREGISTER=1
+	PWMSS0			: org = 0x48300000 len = 0x000002C4	CREGISTER=18
+	PWMSS1			: org = 0x48302000 len = 0x000002C4	CREGISTER=19
+	PWMSS2			: org = 0x48304000 len = 0x000002C4	CREGISTER=20
+	GEMAC			: org = 0x4A100000 len = 0x0000128C	CREGISTER=9
+	I2C1			: org = 0x4802A000 len = 0x000000D8	CREGISTER=2
+	I2C2			: org = 0x4819C000 len = 0x000000D8	CREGISTER=17
+	MBX0			: org = 0x480C8000 len = 0x00000140	CREGISTER=22
+	MCASP0_DMA		: org = 0x46000000 len = 0x00000100	CREGISTER=8
+	MCSPI0			: org = 0x48030000 len = 0x000001A4	CREGISTER=6
+	MCSPI1			: org = 0x481A0000 len = 0x000001A4	CREGISTER=16
+	MMCHS0			: org = 0x48060000 len = 0x00000300	CREGISTER=5
+	SPINLOCK		: org = 0x480CA000 len = 0x00000880	CREGISTER=23
+	TPCC			: org = 0x49000000 len = 0x00001098	CREGISTER=29
+	UART1			: org = 0x48022000 len = 0x00000088	CREGISTER=11
+	UART2			: org = 0x48024000 len = 0x00000088	CREGISTER=12
+
+	RSVD10			: org = 0x48318000 len = 0x00000100	CREGISTER=10
+	RSVD13			: org = 0x48310000 len = 0x00000100	CREGISTER=13
+	RSVD21			: org = 0x00032400 len = 0x00000100	CREGISTER=21
+	RSVD27			: org = 0x00032000 len = 0x00000100	CREGISTER=27
+
+}
+
+/* Specify the sections allocation into memory */
+SECTIONS {
+	/* Forces _c_int00 to the start of PRU IRAM. Not necessary when loading
+	   an ELF file, but useful when loading a binary */
+	.text:_c_int00*	>  0x0, PAGE 0
+
+	.text		>  PRU_IMEM, PAGE 0
+	.stack		>  PRU_DMEM_0_1, PAGE 1
+	.bss		>  PRU_DMEM_0_1, PAGE 1
+	.cio		>  PRU_DMEM_0_1, PAGE 1
+	.data		>  PRU_DMEM_0_1, PAGE 1
+	.switch		>  PRU_DMEM_0_1, PAGE 1
+	.sysmem		>  PRU_DMEM_0_1, PAGE 1
+	.cinit		>  PRU_DMEM_0_1, PAGE 1
+	.rodata		>  PRU_DMEM_0_1, PAGE 1
+	.rofardata	>  PRU_DMEM_0_1, PAGE 1
+	.farbss		>  PRU_DMEM_0_1, PAGE 1
+	.fardata	>  PRU_DMEM_0_1, PAGE 1
+
+	.resource_table > PRU_DMEM_0_1, PAGE 1
+}
diff --git a/src/pru/intc_map.h b/src/pru/intc_map.h
new file mode 100644
index 0000000..eebf27b
--- /dev/null
+++ b/src/pru/intc_map.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2021 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *      * Redistributions of source code must retain the above copyright
+ *        notice, this list of conditions and the following disclaimer.
+ *
+ *      * Redistributions in binary form must reproduce the above copyright
+ *        notice, this list of conditions and the following disclaimer in the
+ *        documentation and/or other materials provided with the
+ *        distribution.
+ *
+ *      * Neither the name of Texas Instruments Incorporated nor the names of
+ *        its contributors may be used to endorse or promote products derived
+ *        from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _INTC_MAP_0_H_
+#define _INTC_MAP_0_H_
+
+/*
+ * ======== PRU INTC Map ========
+ *
+ * Define the INTC mapping for interrupts going to the ICSS / ICSSG:
+ * 	ICSS Host interrupts 0, 1
+ * 	ICSSG Host interrupts 0, 1, 10-19
+ *
+ * Note that INTC interrupts going to the ARM Linux host should not be defined
+ * in this file (ICSS/ICSSG Host interrupts 2-9).
+ *
+ * The INTC configuration for interrupts going to the ARM host should be defined
+ * in the device tree node of the client driver, "interrupts" property.
+ * See Documentation/devicetree/bindings/interrupt-controller/ti,pruss-intc.yaml
+ * entry #interrupt-cells for more.
+ *
+ * For example, on ICSSG:
+ *
+ * &client_driver0 {
+ * 	interrupt-parent = <&icssg0_intc>;
+ * 	interrupts = <21 2 2>, <22 3 3>;
+ * 	interrupt-names = "interrupt_name1", "interrupt_name2";
+ * };
+ *
+ */
+
+#include <stddef.h>
+#include <rsc_types.h>
+
+/*
+ * .pru_irq_map is used by the RemoteProc driver during initialization. However,
+ * the map is NOT used by the PRU firmware. That means DATA_SECTION and RETAIN
+ * are required to prevent the PRU compiler from optimizing out .pru_irq_map.
+ */
+#pragma DATA_SECTION(my_irq_rsc, ".pru_irq_map")
+#pragma RETAIN(my_irq_rsc)
+
+struct pru_irq_rsc my_irq_rsc  = {
+	0,			/* type = 0 */
+	1,			/* number of system events being mapped */
+	{
+		{7, 1, 1},	/* {sysevt, channel, host interrupt} */
+	},
+};
+
+#endif /* _INTC_MAP_0_H_ */
diff --git a/src/pru/pru_pwm.c b/src/pru/pru_pwm.c
new file mode 100644
index 0000000..796040b
--- /dev/null
+++ b/src/pru/pru_pwm.c
@@ -0,0 +1,102 @@
+#include <stdint.h>
+#include <pru_cfg.h>
+#include <pru_intc.h>
+#include <pru_iep.h>
+#include "intc_map.h"
+#include "resource_table_empty.h"
+
+#define CYCLES_PER_SECOND 200000000
+#define CYCLES_PER_PERIOD (CYCLES_PER_SECOND / 800)
+#define DEFAULT_DUTY_CYCLE 0
+#define P8_12 (1 << 14)
+#define P8_12_n ~(1 << 14)
+#define PRU0_DRAM 0x00000                 // Offset to DRAM
+#define PRU_TIMER_PASSCODE 0x31138423
+#define BURN_TIME 2 // seconds for motor burn
+
+volatile uint32_t *pru0_dram = (uint32_t *) (PRU0_DRAM + 0x200);
+
+
+volatile register uint32_t __R30;
+volatile register uint32_t __R31;
+
+
+void main(void) {
+    uint32_t duty_cycle = DEFAULT_DUTY_CYCLE;
+    uint32_t off_cycles = ((100 - duty_cycle) * CYCLES_PER_PERIOD / 100);
+    uint32_t on_cycles = ((100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100);
+    uint32_t off_count = off_cycles;
+    uint32_t on_count = on_cycles;
+    *pru0_dram = duty_cycle;
+
+    /* Clear SYSCFG[STANDBY_INIT] to enable OCP master port */
+    CT_CFG.SYSCFG_bit.STANDBY_INIT = 0;
+
+    while (1) {
+        if (pru0_dram[1] == PRU_TIMER_PASSCODE) {
+            /* Disable counter */
+            CT_IEP.TMR_GLB_CFG_bit.CNT_EN = 0;
+
+            /* Reset Count register */
+            CT_IEP.TMR_CNT = 0x0;
+
+            /* Clear overflow status register */
+            CT_IEP.TMR_GLB_STS_bit.CNT_OVF = 0x1;
+
+            /* Set compare value */
+            CT_IEP.TMR_CMP0 = (BURN_TIME * CYCLES_PER_SECOND); // 10 seconds @ 200MHz
+
+            /* Clear compare status */
+            CT_IEP.TMR_CMP_STS_bit.CMP_HIT = 0xFF;
+
+            /* Disable compensation */
+            CT_IEP.TMR_COMPEN_bit.COMPEN_CNT = 0x0;
+
+            /* Enable CMP0 and reset on event */
+            CT_IEP.TMR_CMP_CFG_bit.CMP0_RST_CNT_EN = 0x1;
+            CT_IEP.TMR_CMP_CFG_bit.CMP_EN = 0x1;
+
+            /* Clear the status of all interrupts */
+            CT_INTC.SECR0 = 0xFFFFFFFF;
+            CT_INTC.SECR1 = 0xFFFFFFFF;
+
+            /* Enable counter */
+            CT_IEP.TMR_GLB_CFG = 0x11;
+            break;
+        }
+    }
+
+	/* Poll until R31.31 is set */
+	do {
+		while ((__R31 & 0x80000000) == 0) {
+		}
+		/* Verify that the IEP is the source of the interrupt */
+	} while ((CT_INTC.SECR0 & (1 << 7)) == 0);
+
+	/* Disable counter */
+	CT_IEP.TMR_GLB_CFG_bit.CNT_EN = 0x0;
+
+	/* Disable Compare0 */
+	CT_IEP.TMR_CMP_CFG = 0x0;
+
+	/* Clear Compare status */
+	CT_IEP.TMR_CMP_STS = 0xFF;
+
+	/* Clear the status of the interrupt */
+	CT_INTC.SECR0 = (1 << 7);
+
+    while (1) {
+        if (on_count) {
+            __R30 |= P8_12;		// Set the GPIO pin to 1
+            on_count--;
+            __delay_cycles(3);
+        } else if (off_count) {
+            __R30 &= P8_12_n;		// Clear the GPIO pin
+            off_count--;
+        } else {
+            duty_cycle = *pru0_dram;
+            off_count = ((100 - duty_cycle) * CYCLES_PER_PERIOD / 100);
+            on_count = ((100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100);
+        }
+    }
+}
diff --git a/src/pru/pwm_test.c b/src/pru/pwm_test.c
new file mode 100644
index 0000000..df8bbf0
--- /dev/null
+++ b/src/pru/pwm_test.c
@@ -0,0 +1,80 @@
+/* 
+ *
+ *  pwm tester
+ *	The on cycle and off cycles are stored in each PRU's Data memory
+ *
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <sys/mman.h>
+
+#define PRU_ADDR		0x4A300000		// Start of PRU memory Page 184 am335x TRM
+#define PRU_LEN			0x80000			// Length of PRU memory
+#define PRU0_DRAM		0x00000			// Offset to DRAM
+#define PRU1_DRAM		0x02000
+#define PRU_SHAREDMEM	0x10000			// Offset to shared memory
+
+#define CYCLES_PER_SECOND 200000000
+#define CYCLES_PER_PERIOD (CYCLES_PER_SECOND / 800)
+
+uint32_t	*pru0DRAM_32int_ptr;		// Points to the start of local DRAM
+uint32_t	*pru1DRAM_32int_ptr;		// Points to the start of local DRAM
+uint32_t	*prusharedMem_32int_ptr;	// Points to the start of the shared memory
+
+/*******************************************************************************
+* int start_pwm_count(int ch, int countOn, int countOff)
+* 
+* Starts a pwm pulse on for countOn and off for countOff to a single channel (ch)
+*******************************************************************************/
+int start_pwm_count(uint32_t duty_cycle) {
+	uint32_t *pruDRAM_32int_ptr = pru0DRAM_32int_ptr;
+    uint32_t old_duty_cycle = pruDRAM_32int_ptr[0];
+    uint32_t old_count_on = (100 - (100 - old_duty_cycle)) * CYCLES_PER_PERIOD / 100;
+    uint32_t old_count_off = (100 - old_duty_cycle) * CYCLES_PER_PERIOD / 100;
+    uint32_t new_count_on = (100 - (100 - duty_cycle)) * CYCLES_PER_PERIOD / 100;
+    uint32_t new_count_off = (100 - duty_cycle) * CYCLES_PER_PERIOD / 100;
+	
+	printf("old:\n\tcountOn: %d, countOff: %d, count: %d\nnew:\n\tcountOn: %d, countOff: %d, count: %d\n", old_count_on, old_count_off, old_count_off + old_count_on, new_count_on, new_count_off, new_count_off + new_count_on);
+	// write to PRU shared memory
+	pruDRAM_32int_ptr[0] = duty_cycle;	// On time
+	return 0;
+}
+
+int main(int argc, char *argv[])
+{
+	uint32_t *pru;		// Points to start of PRU memory.
+	int	fd;
+	printf("Servo tester\n");
+	
+	fd = open ("/dev/mem", O_RDWR | O_SYNC);
+	if (fd == -1) {
+		printf ("ERROR: could not open /dev/mem.\n\n");
+		return 1;
+	}
+	pru = mmap (0, PRU_LEN, PROT_READ | PROT_WRITE, MAP_SHARED, fd, PRU_ADDR);
+	if (pru == MAP_FAILED) {
+		printf ("ERROR: could not map memory.\n\n");
+		return 1;
+	}
+	close(fd);
+	printf ("Using /dev/mem.\n");
+	
+	pru0DRAM_32int_ptr =     pru + PRU0_DRAM/4 + 0x200/4;	// Points to 0x200 of PRU0 memory
+	pru1DRAM_32int_ptr =     pru + PRU1_DRAM/4 + 0x200/4;	// Points to 0x200 of PRU1 memory
+	prusharedMem_32int_ptr = pru + PRU_SHAREDMEM/4;	// Points to start of shared memory
+
+    uint32_t desired_duty_cycle = 0;
+    while (1) {
+        printf("Enter a duty cycle: ");
+        scanf("%d", &desired_duty_cycle);
+        start_pwm_count(desired_duty_cycle);
+    }
+	
+	if(munmap(pru, PRU_LEN)) {
+		printf("munmap failed\n");
+	} else {
+		printf("munmap succeeded\n");
+	}
+}
diff --git a/src/pru/resource_table_empty.h b/src/pru/resource_table_empty.h
new file mode 100644
index 0000000..8e7743e
--- /dev/null
+++ b/src/pru/resource_table_empty.h
@@ -0,0 +1,38 @@
+/*
+ *  ======== resource_table_empty.h ========
+ *
+ *  Define the resource table entries for all PRU cores. This will be
+ *  incorporated into corresponding base images, and used by the remoteproc
+ *  on the host-side to allocated/reserve resources.  Note the remoteproc
+ *  driver requires that all PRU firmware be built with a resource table.
+ *
+ *  This file contains an empty resource table.  It can be used either as:
+ *
+ *        1) A template, or
+ *        2) As-is if a PRU application does not need to configure PRU_INTC
+ *                  or interact with the rpmsg driver
+ *
+ */
+
+#ifndef _RSC_TABLE_PRU_H_
+#define _RSC_TABLE_PRU_H_
+
+#include <stddef.h>
+#include <rsc_types.h>
+
+struct my_resource_table {
+	struct resource_table base;
+
+	uint32_t offset[1]; /* Should match 'num' in actual definition */
+};
+
+#pragma DATA_SECTION(pru_remoteproc_ResourceTable, ".resource_table")
+#pragma RETAIN(pru_remoteproc_ResourceTable)
+struct my_resource_table pru_remoteproc_ResourceTable = {
+	1,	/* we're the first version that implements this */
+	0,	/* number of entries in the table */
+	0, 0,	/* reserved, must be zero */
+	0,	/* offset[0] */
+};
+
+#endif /* _RSC_TABLE_PRU_H_ */
diff --git a/src/pru1/sensors.cpp b/src/pru1/sensors.cpp
deleted file mode 100644
index 508387e..0000000
--- a/src/pru1/sensors.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-/* 
- * Author: Gregory Wainer <[email protected]>
- * Date:   10/14/2023
- *
- */
-
-#include <iostream>
-#include <string>
-
-int main(int argc, char *argv[]) {
-    std::cout << "PRU1" << std::endl;
-}
diff --git a/src/pru2/servos.cpp b/src/pru2/servos.cpp
deleted file mode 100644
index ba71ce2..0000000
--- a/src/pru2/servos.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-/* 
- * Author: Gregory Wainer <[email protected]>
- * Date:   10/14/2023
- *
- */
-
-#include <iostream>
-
-int main(int argc, char* argv[]) {
-    std::cout << "PRU2" << std::endl;
-}
diff --git a/src/rocketUtils.cpp b/src/rocketUtils.cpp
new file mode 100644
index 0000000..45fcfc3
--- /dev/null
+++ b/src/rocketUtils.cpp
@@ -0,0 +1,35 @@
+#include "../include/rocketUtils.hpp"
+
+double deploy_percentage_to_angle(double percentage) {
+
+    return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE;
+}
+
+
+std::string format_data(std::string prefix, double data, int precision) {
+
+    std::stringstream stream;
+    stream << std::fixed << std::setprecision(precision) << data;
+    std::string s = stream.str();
+    return prefix + s;
+}
+
+bool led_out(Vehicle *vehicle) {
+
+    std::ofstream file;
+    file.open(LED_FILENAME);
+    if (!file.is_open()) {
+        return false;
+    }
+
+    file << std::to_string(vehicle->led_brightness);
+    file.close();
+
+    vehicle->led_time = time(nullptr);
+    vehicle->led_brightness = (vehicle->led_brightness + 1) % 2;
+
+    return true;
+}
+
+std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"};
+	
\ No newline at end of file
diff --git a/src/sensorAltimeter.cpp b/src/sensorAltimeter.cpp
new file mode 100644
index 0000000..8ec065d
--- /dev/null
+++ b/src/sensorAltimeter.cpp
@@ -0,0 +1,115 @@
+#include "sensorAltimeter.hpp"
+
+AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) {
+    I2C_FILE = I2C_FILE_in;
+    deviceAddress = 0x60;
+}
+
+//Startup routine copied from Adafruit library, as is most of the data getting methods
+//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation)
+bool AltimeterSensor::init() {
+
+    // Vehicle *vehicle = (Vehicle *) data;
+    // // Do Stuff 
+    // data = (void*) vehicle;
+
+    //Pass file string from parent to setup function, actual I2C bus gets stored internally.
+    setupI2C(I2C_FILE);
+
+    // Check a register with a hard-coded value to see if comms are working
+    uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI);
+    if (whoami != 0xC4) {
+        fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami);
+        return false;
+    }
+
+    //Send device dedicated reset byte to CTRL1 Register
+    writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST);
+    //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register
+    while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST);
+
+     //Set oversampling (?) and altitude mode by default
+    currentMode = MPL3115A2_ALTIMETER;
+    ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT;
+    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
+
+    //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess
+    writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE |
+                                    MPL3115A2_PT_DATA_CFG_PDEFE |
+                                    MPL3115A2_PT_DATA_CFG_DREM);
+
+    return true;
+}
+
+//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION
+void AltimeterSensor::setMode(mpl3115a2_mode_t mode) {
+    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
+    ctrl_reg1.bit.ALT = mode;
+    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
+    currentMode = mode;
+}
+
+double AltimeterSensor::getAltitude() {
+    //Request new data reading
+    requestOneShotReading();
+    //If new data is available, read it and store it to internal fields
+    if (isNewDataAvailable()) {
+        //Logger flag here for new data?
+        updateCurrentDataBuffer();
+    }
+    //Return internal field, whether updated or not
+    return internalAltitude;
+}
+
+double AltimeterSensor::getTemperature() {
+    //Request new data reading
+    requestOneShotReading();
+    //If new data is available, read it and store it to internal fields
+    if (isNewDataAvailable()) {
+        //Logger flag here for new data?
+        updateCurrentDataBuffer();
+    }
+    //Return internal field, whether updated or not
+    return internalTemperature;
+}
+
+void AltimeterSensor::requestOneShotReading() {
+    //Request current status of oneshot reading
+    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
+    //If oneshot is complete, proc a new one; if it isn't, do nothing.
+    //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR.
+    if (!ctrl_reg1.bit.OST) {
+        // initiate one-shot measurement
+        ctrl_reg1.bit.OST = 1;
+        writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
+    }
+}
+
+bool AltimeterSensor::isNewDataAvailable() {
+    //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available
+    //There *are* registers available for exclusively temperature *or* pressure/altitude, but 
+    //for simplicity's sake we'll use the combined one for now.
+    return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0);
+}
+
+//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead
+void AltimeterSensor::updateCurrentDataBuffer() {
+    uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0};
+    readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5);
+
+    //Pressure is no longer used, assumed rocket is only logging altitude
+    // uint32_t pressure;
+    // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 |
+    //         uint32_t(buffer[2]);
+    // return double(pressure) / 6400.0;
+
+    //Altitude Conversion
+    int32_t alt;
+    alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 |
+        uint32_t(buffer[2]) << 8;
+    internalAltitude = double(alt) / 65536.0;
+
+    int16_t t;
+    t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]);
+    internalTemperature = double(t) / 256.0;
+}
diff --git a/src/sensorIMU.cpp b/src/sensorIMU.cpp
new file mode 100644
index 0000000..941ea35
--- /dev/null
+++ b/src/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);
+}
+
+
+
+
+
+
+
+
+
+
diff --git a/src/surfaceFitModel.cpp b/src/surfaceFitModel.cpp
new file mode 100644
index 0000000..d48da49
--- /dev/null
+++ b/src/surfaceFitModel.cpp
@@ -0,0 +1,40 @@
+#include "../include/surfaceFitModel.hpp"
+
+SurfaceFitModel::SurfaceFitModel() {
+
+    p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1);
+
+    p(0, 0) = -781536.384794701;
+    p(1, 0) = 8623.59011973048;
+    p(0, 1) = 643.65918253;
+    p(2, 0) = -34.3646691281487;
+    p(1, 1) = -5.46066535343611;
+    p(0, 2) = -0.177121900557321;
+    p(3, 0) = 0.0573287698655951;
+    p(2, 1) = 0.0150031142038895;
+    p(1, 2) = 0.00101871763126609;
+    p(0, 3) = 1.63862900553892e-05;
+    p(4, 0) = -3.21785828407871e-05;
+    p(3, 1) = -1.3161091180883e-05;
+    p(2, 2) = -1.42505256569339e-06;
+    p(1, 3) = -4.76209793830867e-08;
+}
+
+
+double SurfaceFitModel::getFit(double x, double y) {
+
+    return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) +
+	p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) +
+	p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) +
+	p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) +
+	p(1, 3) * x * pow(y, 3); 
+}
+
+
+
+
+
+
+
+
+