summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/AltEst/algebra.cpp292
-rw-r--r--src/AltEst/algebra.h96
-rw-r--r--src/AltEst/altitude.cpp58
-rw-r--r--src/AltEst/altitude.h55
-rw-r--r--src/AltEst/filters.cpp202
-rw-r--r--src/AltEst/filters.h65
-rw-r--r--src/CMakeLists.txt101
-rw-r--r--src/SimpleKalmanFilter.cpp48
-rw-r--r--src/active_drag_system.cpp655
-rw-r--r--src/altimeter.cpp50
-rw-r--r--src/bno055.cpp201
-rw-r--r--src/kalmanfilter.cpp3
-rw-r--r--src/pwm.cpp35
-rw-r--r--src/read_flash.c71
-rw-r--r--src/servo_test.cpp27
-rw-r--r--src/spi_flash.c267
-rw-r--r--src/unused/actuationPlan.cpp60
-rw-r--r--src/unused/ads.cpp286
-rw-r--r--src/unused/logger.cpp132
-rw-r--r--src/unused/motor.cpp46
-rw-r--r--src/unused/rocketUtils.cpp35
-rw-r--r--src/unused/sensorAltimeter.cpp115
-rw-r--r--src/unused/sensorIMU.cpp385
-rw-r--r--src/unused/surfaceFitModel.cpp40
24 files changed, 3266 insertions, 59 deletions
diff --git a/src/AltEst/algebra.cpp b/src/AltEst/algebra.cpp
new file mode 100644
index 0000000..653c3b9
--- /dev/null
+++ b/src/AltEst/algebra.cpp
@@ -0,0 +1,292 @@
+/*
+ algebra.cpp: This file contains a number of utilities useful for handling
+ 3D vectors
+
+ This work is an adaptation from vvector.h, written by Linas Vepstras. The
+ original code can be found at:
+
+ https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h
+
+ HISTORY:
+ Written by Linas Vepstas, August 1991
+ Added 2D code, March 1993
+ Added Outer products, C++ proofed, Linas Vepstas October 1993
+ Adapted for altitude estimation tasks by Juan Gallostra June 2018
+*/
+
+//#include <cmath>
+#include <stdio.h>
+
+#include "algebra.h"
+
+// Copy 3D vector
+void copyVector(float b[3],float a[3])
+{
+ b[0] = a[0];
+ b[1] = a[1];
+ b[2] = a[2];
+}
+
+
+// Vector difference
+void subtractVectors(float v21[3], float v2[3], float v1[3])
+{
+ v21[0] = v2[0] - v1[0];
+ v21[1] = v2[1] - v1[1];
+ v21[2] = v2[2] - v1[2];
+}
+
+// Vector sum
+void sumVectors(float v21[3], float v2[3], float v1[3])
+{
+ v21[0] = v2[0] + v1[0];
+ v21[1] = v2[1] + v1[1];
+ v21[2] = v2[2] + v1[2];
+}
+
+// scalar times vector
+void scaleVector(float c[3],float a, float b[3])
+{
+ (c)[0] = a*b[0];
+ (c)[1] = a*b[1];
+ (c)[2] = a*b[2];
+}
+
+// accumulate scaled vector
+void accumulateScaledVector(float c[3], float a, float b[3])
+{
+ (c)[0] += a*b[0];
+ (c)[1] += a*b[1];
+ (c)[2] += a*b[2];
+}
+
+// Vector dot product
+void dotProductVectors(float * c, float a[3], float b[3])
+{
+ *c = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
+}
+
+// Vector length
+void vectorLength(float * len, float a[3])
+{
+ float tmp;
+ tmp = a[0]*a[0] + a[1]*a[1]+a[2]*a[2];
+ *len = sqrt(tmp);
+}
+
+// Normalize vector
+void normalizeVector(float a[3])
+{
+ float len;
+ vectorLength(& len,a);
+ if (len != 0.0) {
+ len = 1.0 / len;
+ a[0] *= len;
+ a[1] *= len;
+ a[2] *= len;
+ }
+}
+
+// 3D Vector cross product yeilding vector
+void crossProductVectors(float c[3], float a[3], float b[3])
+{
+ c[0] = a[1] * b[2] - a[2] * b[1];
+ c[1] = a[2] * b[0] - a[0] * b[2];
+ c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+// initialize matrix
+void identityMatrix3x3(float m[3][3])
+{
+ m[0][0] = 1.0;
+ m[0][1] = 0.0;
+ m[0][2] = 0.0;
+
+ m[1][0] = 0.0;
+ m[1][1] = 1.0;
+ m[1][2] = 0.0;
+
+ m[2][0] = 0.0;
+ m[2][1] = 0.0;
+ m[2][2] = 1.0;
+}
+
+// matrix copy
+void copyMatrix3x3(float b[3][3], float a[3][3])
+{
+ b[0][0] = a[0][0];
+ b[0][1] = a[0][1];
+ b[0][2] = a[0][2];
+
+ b[1][0] = a[1][0];
+ b[1][1] = a[1][1];
+ b[1][2] = a[1][2];
+
+ b[2][0] = a[2][0];
+ b[2][1] = a[2][1];
+ b[2][2] = a[2][2];
+}
+
+// matrix transpose
+void transposeMatrix3x3(float b[3][3], float a[3][3])
+{
+ b[0][0] = a[0][0];
+ b[0][1] = a[1][0];
+ b[0][2] = a[2][0];
+
+ b[1][0] = a[0][1];
+ b[1][1] = a[1][1];
+ b[1][2] = a[2][1];
+
+ b[2][0] = a[0][2];
+ b[2][1] = a[1][2];
+ b[2][2] = a[2][2];
+}
+
+// multiply matrix by scalar
+void scaleMatrix3x3(float b[3][3], float s, float a[3][3])
+{
+ b[0][0] = (s) * a[0][0];
+ b[0][1] = (s) * a[0][1];
+ b[0][2] = (s) * a[0][2];
+
+ b[1][0] = (s) * a[1][0];
+ b[1][1] = (s) * a[1][1];
+ b[1][2] = (s) * a[1][2];
+
+ b[2][0] = (s) * a[2][0];
+ b[2][1] = (s) * a[2][1];
+ b[2][2] = (s) * a[2][2];
+}
+
+// multiply matrix by scalar and add result to another matrix
+void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3])
+{
+ b[0][0] += s * a[0][0];
+ b[0][1] += s * a[0][1];
+ b[0][2] += s * a[0][2];
+
+ b[1][0] += s * a[1][0];
+ b[1][1] += s * a[1][1];
+ b[1][2] += s * a[1][2];
+
+ b[2][0] += s * a[2][0];
+ b[2][1] += s * a[2][1];
+ b[2][2] += s * a[2][2];
+}
+
+// matrix product
+// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y]
+void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3])
+{
+ c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];
+ c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];
+ c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];
+
+ c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];
+ c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];
+ c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];
+
+ c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];
+ c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];
+ c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];
+}
+
+// matrix times vector
+void matrixDotVector3x3(float p[3], float m[3][3], float v[3])
+{
+ p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
+ p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
+ p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
+}
+
+// determinant of matrix
+// Computes determinant of matrix m, returning d
+void determinant3x3(float * d, float m[3][3])
+{
+ *d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);
+ *d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);
+ *d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);
+}
+
+// adjoint of matrix
+// Computes adjoint of matrix m, returning a
+// (Note that adjoint is just the transpose of the cofactor matrix)
+void adjoint3x3(float a[3][3], float m[3][3])
+{
+ a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
+ a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);
+ a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
+ a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);
+ a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
+ a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);
+ a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
+ a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);
+ a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
+}
+
+// compute adjoint of matrix and scale
+// Computes adjoint of matrix m, scales it by s, returning a
+void scaleAdjoint3x3(float a[3][3], float s, float m[3][3])
+{
+ a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
+ a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);
+ a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
+
+ a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);
+ a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);
+ a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);
+
+ a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);
+ a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);
+ a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);
+}
+
+// inverse of matrix
+// Compute inverse of matrix a, returning determinant m and
+// inverse b
+void invert3x3(float b[3][3], float a[3][3])
+{
+ float tmp;
+ determinant3x3(& tmp, a);
+ tmp = 1.0 / (tmp);
+ scaleAdjoint3x3(b, tmp, a);
+}
+
+// skew matrix from vector
+void skew(float a[3][3], float v[3])
+{
+ a[0][1] = -v[2];
+ a[0][2] = v[1];
+ a[1][2] = -v[0];
+ a[1][0] = v[2];
+ a[2][0] = -v[1];
+ a[2][1] = v[0];
+ // set diagonal to 0
+ a[0][0] = 0.0;
+ a[1][1] = 0.0;
+ a[2][2] = 0.0;
+}
+
+void printMatrix3X3(float mmm[3][3])
+{
+ int i,j;
+ printf ("matrix mmm is \n");
+ if (mmm == NULL) {
+ printf (" Null \n");
+ } else {
+ for (i=0; i<3; i++) {
+ for (j=0; j<3; j++) {
+ printf ("%f ", mmm[i][j]);
+ }
+ printf (" \n");
+ }
+ }
+}
+
+void vecPrint(float a[3])
+{
+ float len;
+ vectorLength(& len, a);
+ printf(" a is %f %f %f length of a is %f \n", a[0], a[1], a[2], len);
+}
diff --git a/src/AltEst/algebra.h b/src/AltEst/algebra.h
new file mode 100644
index 0000000..382103e
--- /dev/null
+++ b/src/AltEst/algebra.h
@@ -0,0 +1,96 @@
+/*
+ algebra.h: This file contains a number of utilities useful for handling
+ 3D vectors
+
+ This work is an adaptation from vvector.h, written by Linas Vepstras. The
+ original code can be found at:
+
+ https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h
+
+ HISTORY:
+ Written by Linas Vepstas, August 1991
+ Added 2D code, March 1993
+ Added Outer products, C++ proofed, Linas Vepstas October 1993
+ Adapted for altitude estimation tasks by Juan Gallostra June 2018
+ Separated .h, .cpp by Simon D. Levy July 2018
+*/
+
+#pragma once
+
+//#include <cmath>
+#include <math.h>
+
+// Copy 3D vector
+void copyVector(float b[3],float a[3]);
+
+
+// Vector difference
+void subtractVectors(float v21[3], float v2[3], float v1[3]);
+
+// Vector sum
+void sumVectors(float v21[3], float v2[3], float v1[3]);
+
+// scalar times vector
+void scaleVector(float c[3],float a, float b[3]);
+
+// accumulate scaled vector
+void accumulateScaledVector(float c[3], float a, float b[3]);
+
+// Vector dot product
+void dotProductVectors(float * c, float a[3], float b[3]);
+
+// Vector length
+void vectorLength(float * len, float a[3]);
+
+// Normalize vector
+void normalizeVector(float a[3]);
+
+// 3D Vector cross product yeilding vector
+void crossProductVectors(float c[3], float a[3], float b[3]);
+
+// initialize matrix
+void identityMatrix3x3(float m[3][3]);
+
+// matrix copy
+void copyMatrix3x3(float b[3][3], float a[3][3]);
+
+// matrix transpose
+void transposeMatrix3x3(float b[3][3], float a[3][3]);
+
+// multiply matrix by scalar
+void scaleMatrix3x3(float b[3][3], float s, float a[3][3]);
+
+// multiply matrix by scalar and add result to another matrix
+void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3]);
+
+// matrix product
+// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y]
+void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3]);
+
+// matrix times vector
+void matrixDotVector3x3(float p[3], float m[3][3], float v[3]);
+
+// determinant of matrix
+// Computes determinant of matrix m, returning d
+void determinant3x3(float * d, float m[3][3]);
+
+// adjoint of matrix
+// Computes adjoint of matrix m, returning a
+// (Note that adjoint is just the transpose of the cofactor matrix);
+void adjoint3x3(float a[3][3], float m[3][3]);
+
+// compute adjoint of matrix and scale
+// Computes adjoint of matrix m, scales it by s, returning a
+void scaleAdjoint3x3(float a[3][3], float s, float m[3][3]);
+
+// inverse of matrix
+// Compute inverse of matrix a, returning determinant m and
+// inverse b
+void invert3x3(float b[3][3], float a[3][3]);
+
+// skew matrix from vector
+void skew(float a[3][3], float v[3]);
+
+void printMatrix3X3(float mmm[3][3]);
+
+void vecPrint(float a[3]);
diff --git a/src/AltEst/altitude.cpp b/src/AltEst/altitude.cpp
new file mode 100644
index 0000000..8838b36
--- /dev/null
+++ b/src/AltEst/altitude.cpp
@@ -0,0 +1,58 @@
+/*
+ altitude.cpp: Altitude estimation via barometer/accelerometer fusion
+*/
+
+#include "filters.h"
+#include "algebra.h"
+#include "altitude.h"
+
+AltitudeEstimator::AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
+ float ca, float accelThreshold)
+:kalman(ca, sigmaGyro, sigmaAccel), complementary(sigmaAccel, sigmaBaro, accelThreshold)
+{
+ this->sigmaAccel = sigmaAccel;
+ this->sigmaGyro = sigmaGyro;
+ this->sigmaBaro = sigmaBaro;
+ this->ca = ca;
+ this->accelThreshold = accelThreshold;
+}
+
+void AltitudeEstimator::estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp)
+{
+ float deltat = (float)(timestamp-previousTime)/1000000.0f;
+ float verticalAccel = kalman.estimate(pastGyro,
+ pastAccel,
+ deltat);
+ complementary.estimate(& estimatedVelocity,
+ & estimatedAltitude,
+ baroHeight,
+ pastAltitude,
+ pastVerticalVelocity,
+ pastVerticalAccel,
+ deltat);
+ // update values for next iteration
+ copyVector(pastGyro, gyro);
+ copyVector(pastAccel, accel);
+ pastAltitude = estimatedAltitude;
+ pastVerticalVelocity = estimatedVelocity;
+ pastVerticalAccel = verticalAccel;
+ previousTime = timestamp;
+}
+
+float AltitudeEstimator::getAltitude()
+{
+ // return the last estimated altitude
+ return estimatedAltitude;
+}
+
+float AltitudeEstimator::getVerticalVelocity()
+{
+ // return the last estimated vertical velocity
+ return estimatedVelocity;
+}
+
+float AltitudeEstimator::getVerticalAcceleration()
+{
+ // return the last estimated vertical acceleration
+ return pastVerticalAccel;
+}
diff --git a/src/AltEst/altitude.h b/src/AltEst/altitude.h
new file mode 100644
index 0000000..1ca6cb0
--- /dev/null
+++ b/src/AltEst/altitude.h
@@ -0,0 +1,55 @@
+/*
+ altitude.h: Altitude estimation via barometer/accelerometer fusion
+*/
+
+# pragma once
+
+#include "filters.h"
+#include "algebra.h"
+#include "pico/time.h"
+#include "pico/types.h"
+
+class AltitudeEstimator {
+
+ private:
+ // required parameters for the filters used for the estimations
+ // sensor's standard deviations
+ float sigmaAccel;
+ float sigmaGyro;
+ float sigmaBaro;
+ // Acceleration markov chain model state transition constant
+ float ca;
+ // Zero-velocity update acceleration threshold
+ float accelThreshold;
+ // gravity
+ float g = 9.81;
+ // For computing the sampling period
+ absolute_time_t prevTime = get_absolute_time();
+ uint32_t previousTime = to_us_since_boot(prevTime);
+ // required filters for altitude and vertical velocity estimation
+ KalmanFilter kalman;
+ ComplementaryFilter complementary;
+ // Estimated past vertical acceleration
+ float pastVerticalAccel = 0;
+ float pastVerticalVelocity = 0;
+ float pastAltitude = 0;
+ float pastGyro[3] = {0, 0, 0};
+ float pastAccel[3] = {0, 0, 0};
+ // estimated altitude and vertical velocity
+ float estimatedAltitude = 0;
+ float estimatedVelocity = 0;
+
+ public:
+
+ AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
+ float ca, float accelThreshold);
+
+ void estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp);
+
+ float getAltitude();
+
+ float getVerticalVelocity();
+
+ float getVerticalAcceleration();
+
+}; // class AltitudeEstimator
diff --git a/src/AltEst/filters.cpp b/src/AltEst/filters.cpp
new file mode 100644
index 0000000..7902065
--- /dev/null
+++ b/src/AltEst/filters.cpp
@@ -0,0 +1,202 @@
+/*
+ filters.cpp: Filter class implementations
+ */
+
+//#include <cmath>
+#include <stdlib.h> // XXX eventually use fabs() instead of abs() ?
+
+#include "filters.h"
+
+void KalmanFilter::getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat)
+{
+ // required matrices for the operations
+ float sigma[3][3];
+ float identity[3][3];
+ identityMatrix3x3(identity);
+ float skewMatrix[3][3];
+ skew(skewMatrix, previousState);
+ float tmp[3][3];
+ // Compute the prediction covariance matrix
+ scaleMatrix3x3(sigma, pow(sigmaGyro, 2), identity);
+ matrixProduct3x3(tmp, skewMatrix, sigma);
+ matrixProduct3x3(covariance, tmp, skewMatrix);
+ scaleMatrix3x3(covariance, -pow(deltat, 2), covariance);
+}
+
+void KalmanFilter::getMeasurementCovariance(float covariance[3][3])
+{
+ // required matrices for the operations
+ float sigma[3][3];
+ float identity[3][3];
+ identityMatrix3x3(identity);
+ float norm;
+ // Compute measurement covariance
+ scaleMatrix3x3(sigma, pow(sigmaAccel, 2), identity);
+ vectorLength(& norm, previousAccelSensor);
+ scaleAndAccumulateMatrix3x3(sigma, (1.0/3.0)*pow(ca, 2)*norm, identity);
+ copyMatrix3x3(covariance, sigma);
+}
+
+void KalmanFilter::predictState(float predictedState[3], float gyro[3], float deltat)
+{
+ // helper matrices
+ float identity[3][3];
+ identityMatrix3x3(identity);
+ float skewFromGyro[3][3];
+ skew(skewFromGyro, gyro);
+ // Predict state
+ scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro);
+ matrixDotVector3x3(predictedState, identity, currentState);
+ normalizeVector(predictedState);
+}
+
+void KalmanFilter::predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat)
+{
+ // required matrices
+ float Q[3][3];
+ float identity[3][3];
+ identityMatrix3x3(identity);
+ float skewFromGyro[3][3];
+ skew(skewFromGyro, gyro);
+ float tmp[3][3];
+ float tmpTransposed[3][3];
+ float tmp2[3][3];
+ // predict error covariance
+ getPredictionCovariance(Q, currentState, deltat);
+ scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro);
+ copyMatrix3x3(tmp, identity);
+ transposeMatrix3x3(tmpTransposed, tmp);
+ matrixProduct3x3(tmp2, tmp, currErrorCovariance);
+ matrixProduct3x3(covariance, tmp2, tmpTransposed);
+ scaleAndAccumulateMatrix3x3(covariance, 1.0, Q);
+}
+
+void KalmanFilter::updateGain(float gain[3][3], float errorCovariance[3][3])
+{
+ // required matrices
+ float R[3][3];
+ float HTransposed[3][3];
+ transposeMatrix3x3(HTransposed, H);
+ float tmp[3][3];
+ float tmp2[3][3];
+ float tmp2Inverse[3][3];
+ // update kalman gain
+ // P.dot(H.T).dot(inv(H.dot(P).dot(H.T) + R))
+ getMeasurementCovariance(R);
+ matrixProduct3x3(tmp, errorCovariance, HTransposed);
+ matrixProduct3x3(tmp2, H, tmp);
+ scaleAndAccumulateMatrix3x3(tmp2, 1.0, R);
+ invert3x3(tmp2Inverse, tmp2);
+ matrixProduct3x3(gain, tmp, tmp2Inverse);
+}
+
+void KalmanFilter::updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3])
+{
+ // required matrices
+ float tmp[3];
+ float tmp2[3];
+ float measurement[3];
+ scaleVector(tmp, ca, previousAccelSensor);
+ subtractVectors(measurement, accel, tmp);
+ // update state with measurement
+ // predicted_state + K.dot(measurement - H.dot(predicted_state))
+ matrixDotVector3x3(tmp, H, predictedState);
+ subtractVectors(tmp, measurement, tmp);
+ matrixDotVector3x3(tmp2, gain, tmp);
+ sumVectors(updatedState, predictedState, tmp2);
+ normalizeVector(updatedState);
+}
+
+void KalmanFilter::updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3])
+{
+ // required matrices
+ float identity[3][3];
+ identityMatrix3x3(identity);
+ float tmp[3][3];
+ float tmp2[3][3];
+ // update error covariance with measurement
+ matrixProduct3x3(tmp, gain, H);
+ matrixProduct3x3(tmp2, tmp, errorCovariance);
+ scaleAndAccumulateMatrix3x3(identity, -1.0, tmp2);
+ copyMatrix3x3(covariance, tmp2);
+}
+
+
+KalmanFilter::KalmanFilter(float ca, float sigmaGyro, float sigmaAccel)
+{
+ this->ca = ca;
+ this->sigmaGyro = sigmaGyro;
+ this->sigmaAccel = sigmaAccel;
+}
+
+float KalmanFilter::estimate(float gyro[3], float accel[3], float deltat)
+{
+ float predictedState[3];
+ float updatedState[3];
+ float errorCovariance[3][3];
+ float updatedErrorCovariance[3][3];
+ float gain[3][3];
+ float accelSensor[3];
+ float tmp[3];
+ float accelEarth;
+ scaleVector(accel, 9.81, accel); // Scale accel readings since they are measured in gs
+ // perform estimation
+ // predictions
+ predictState(predictedState, gyro, deltat);
+ predictErrorCovariance(errorCovariance, gyro, deltat);
+ // updates
+ updateGain(gain, errorCovariance);
+ updateState(updatedState, predictedState, gain, accel);
+ updateErrorCovariance(updatedErrorCovariance, errorCovariance, gain);
+ // Store required values for next iteration
+ copyVector(currentState, updatedState);
+ copyMatrix3x3(currErrorCovariance, updatedErrorCovariance);
+ // return vertical acceleration estimate
+ scaleVector(tmp, 9.81, updatedState);
+ subtractVectors(accelSensor, accel, tmp);
+ copyVector(previousAccelSensor, accelSensor);
+ dotProductVectors(& accelEarth, accelSensor, updatedState);
+ return accelEarth;
+}
+
+
+float ComplementaryFilter::ApplyZUPT(float accel, float vel)
+{
+ // first update ZUPT array with latest estimation
+ ZUPT[ZUPTIdx] = accel;
+ // and move index to next slot
+ uint8_t nextIndex = (ZUPTIdx + 1) % ZUPT_SIZE;
+ ZUPTIdx = nextIndex;
+ // Apply Zero-velocity update
+ for (uint8_t k = 0; k < ZUPT_SIZE; ++k) {
+ if (abs(ZUPT[k]) > accelThreshold) return vel;
+ }
+ return 0.0;
+}
+
+
+ComplementaryFilter::ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold)
+{
+ // Compute the filter gain
+ gain[0] = sqrt(2 * sigmaAccel / sigmaBaro);
+ gain[1] = sigmaAccel / sigmaBaro;
+ // If acceleration is below the threshold the ZUPT counter
+ // will be increased
+ this->accelThreshold = accelThreshold;
+ // initialize zero-velocity update
+ ZUPTIdx = 0;
+ for (uint8_t k = 0; k < ZUPT_SIZE; ++k) {
+ ZUPT[k] = 0;
+ }
+}
+
+void ComplementaryFilter::estimate(float * velocity, float * altitude, float baroAltitude,
+ float pastAltitude, float pastVelocity, float accel, float deltat)
+{
+ // Apply complementary filter
+ *altitude = pastAltitude + deltat*(pastVelocity + (gain[0] + gain[1]*deltat/2)*(baroAltitude-pastAltitude))+
+ accel*pow(deltat, 2)/2;
+ *velocity = pastVelocity + deltat*(gain[1]*(baroAltitude-pastAltitude) + accel);
+ // Compute zero-velocity update
+ *velocity = ApplyZUPT(accel, *velocity);
+}
diff --git a/src/AltEst/filters.h b/src/AltEst/filters.h
new file mode 100644
index 0000000..2e316a3
--- /dev/null
+++ b/src/AltEst/filters.h
@@ -0,0 +1,65 @@
+/*
+ filters.h: Filter class declarations
+ */
+
+#pragma once
+
+//#include <cmath>
+#include <math.h>
+#include <stdint.h>
+
+#include "algebra.h"
+
+class KalmanFilter {
+ private:
+ float currentState[3] = {0, 0, 1};
+ float currErrorCovariance[3][3] = {{100, 0, 0},{0, 100, 0},{0, 0, 100}};
+ float H[3][3] = {{9.81, 0, 0}, {0, 9.81, 0}, {0, 0, 9.81}};
+ float previousAccelSensor[3] = {0, 0, 0};
+ float ca;
+ float sigmaGyro;
+ float sigmaAccel;
+
+ void getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat);
+
+ void getMeasurementCovariance(float covariance[3][3]);
+
+ void predictState(float predictedState[3], float gyro[3], float deltat);
+
+ void predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat);
+
+ void updateGain(float gain[3][3], float errorCovariance[3][3]);
+
+ void updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3]);
+
+ void updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3]);
+
+ public:
+
+ KalmanFilter(float ca, float sigmaGyro, float sigmaAccel);
+
+ float estimate(float gyro[3], float accel[3], float deltat);
+
+}; // Class KalmanFilter
+
+class ComplementaryFilter {
+
+ private:
+
+ // filter gain
+ float gain[2];
+ // Zero-velocity update
+ float accelThreshold;
+ static const uint8_t ZUPT_SIZE = 12;
+ uint8_t ZUPTIdx;
+ float ZUPT[ZUPT_SIZE];
+
+ float ApplyZUPT(float accel, float vel);
+
+ public:
+
+ ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold);
+
+ void estimate(float * velocity, float * altitude, float baroAltitude,
+ float pastAltitude, float pastVelocity, float accel, float deltat);
+}; // Class ComplementaryFilter
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0a50318..59d21de 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,49 +1,54 @@
add_executable(ads
- active_drag_system.cpp
-)
-
-add_executable(pru1
- pru1/sensors.cpp
-)
-
-add_executable(pru2
- pru2/servos.cpp
-)
-
-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})
+ active_drag_system.cpp
+ spi_flash.c
+ bno055.cpp
+ SimpleKalmanFilter.cpp
+ # kalmanfilter.cpp
+ pwm.cpp
+ AltEst/algebra.cpp
+ AltEst/altitude.cpp
+ AltEst/filters.cpp
+ )
+
+add_executable(read_flash
+ read_flash.c
+ spi_flash.c
+ )
+
+add_executable(servo_test
+ servo_test.cpp
+ pwm.cpp
+ )
+
+add_executable(alt_test
+ altimeter.cpp
+ )
+
+# pull in common dependencies
+target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm pico_cyw43_arch_none ${Eigen_LIBRARIES})
+target_include_directories(ads PUBLIC ../include)
+
+target_link_libraries(read_flash pico_stdlib hardware_spi)
+target_include_directories(read_flash PUBLIC ../include)
+
+target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c)
+target_include_directories(servo_test PUBLIC ../include)
+
+target_link_libraries(alt_test pico_stdlib hardware_i2c hardware_gpio)
+
+pico_enable_stdio_usb(ads 1)
+pico_enable_stdio_uart(ads 0)
+
+pico_enable_stdio_usb(read_flash 1)
+pico_enable_stdio_uart(read_flash 0)
+
+pico_enable_stdio_usb(servo_test 1)
+pico_enable_stdio_uart(servo_test 0)
+
+pico_enable_stdio_usb(alt_test 1)
+pico_enable_stdio_uart(alt_test 0)
+# create map/bin/hex file etc.
+pico_add_extra_outputs(ads)
+pico_add_extra_outputs(read_flash)
+pico_add_extra_outputs(servo_test)
+pico_add_extra_outputs(alt_test)
diff --git a/src/SimpleKalmanFilter.cpp b/src/SimpleKalmanFilter.cpp
new file mode 100644
index 0000000..88ba5d4
--- /dev/null
+++ b/src/SimpleKalmanFilter.cpp
@@ -0,0 +1,48 @@
+/*
+ * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.
+ * Created by Denys Sene, January, 1, 2017.
+ * Released under MIT License - see LICENSE file for details.
+ */
+
+#include "SimpleKalmanFilter.h"
+#include <math.h>
+
+SimpleKalmanFilter::SimpleKalmanFilter(float mea_e, float est_e, float q)
+{
+ _err_measure=mea_e;
+ _err_estimate=est_e;
+ _q = q;
+}
+
+float SimpleKalmanFilter::updateEstimate(float mea)
+{
+ _kalman_gain = _err_estimate/(_err_estimate + _err_measure);
+ _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);
+ _err_estimate = (1.0f - _kalman_gain)*_err_estimate + fabsf(_last_estimate-_current_estimate)*_q;
+ _last_estimate=_current_estimate;
+
+ return _current_estimate;
+}
+
+void SimpleKalmanFilter::setMeasurementError(float mea_e)
+{
+ _err_measure=mea_e;
+}
+
+void SimpleKalmanFilter::setEstimateError(float est_e)
+{
+ _err_estimate=est_e;
+}
+
+void SimpleKalmanFilter::setProcessNoise(float q)
+{
+ _q=q;
+}
+
+float SimpleKalmanFilter::getKalmanGain() {
+ return _kalman_gain;
+}
+
+float SimpleKalmanFilter::getEstimateError() {
+ return _err_estimate;
+}
diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp
index 7b63faa..5d248e2 100644
--- a/src/active_drag_system.cpp
+++ b/src/active_drag_system.cpp
@@ -1,14 +1,649 @@
-#include <iostream>
-#include <cstdlib>
-#include "../include/surfaceFitModel.hpp"
-#include "../include/actuationPlan.hpp"
-#include "../include/ads.hpp"
+#include <algorithm>
+#include <stdio.h>
+#include "pico/multicore.h"
+#include "pico/platform.h"
+#include "pico/sem.h"
+#include "spi_flash.h"
+#include "boards/pico_w.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "pico/stdlib.h"
+#include "pico/time.h"
+#include "pico/types.h"
+#include "pico/cyw43_arch.h"
+#include <math.h>
+#include "bno055.hpp"
+#include "AltEst/altitude.h"
+#include "pwm.hpp"
+#include "SimpleKalmanFilter.h"
+
+#define ALT_ADDR 0x60
+#define MAX_SCL 400000
+#define DATA_RATE_HZ 100
+#define LOOP_PERIOD (1.0f / DATA_RATE_HZ)
+#define INT1_PIN 6 // INT1 PIN on MPL3115A2 connected to GPIO PIN 9 (GP6)
+#define MOSFET_PIN 26 // MOSFET PIN connected to GPIO PIN 31 (GP26)
+
+#define GRAVITY -9.81
+#define LOG_RATE_HZ 4
+
+#define MOTOR_BURN_TIME 3900 // Burn time in milliseconds for M2500T
+
+typedef enum {
+ PAD,
+ BOOST,
+ COAST,
+ APOGEE,
+ RECOVERY,
+ END
+} state_t;
+
+BNO055 bno055;
+PWM pwm;
+static AltitudeEstimator vKF = AltitudeEstimator(0.0005, // sigma Accel
+ 0.0005, // sigma Gyro
+ 0.018, // sigma Baro
+ 0.5, // ca
+ 0.1);// accelThreshold
+
+void pad_callback(uint gpio, uint32_t event_mask);
+int64_t boost_callback(alarm_id_t id, void* user_data);
+int64_t apogee_callback(alarm_id_t id, void* user_data);
+int64_t coast_callback(alarm_id_t id, void* user_data);
+void recovery_callback(uint gpio, uint32_t event_mask);
+void init_altimeter();
+float get_deploy_percent(float velocity, float altitude);
+
+bool timer_callback(repeating_timer_t *rt);
+bool test_timer_callback(repeating_timer_t *rt);
+
+float get_altitude();
+float get_velocity();
+
+void snapshot();
+bool logging_callback(repeating_timer_t *rt);
+void logging_core();
+
+semaphore_t sem;
+
+volatile float altitude = 0.0f;
+volatile float prev_altitude = 0.0f;
+volatile float velocity = 0.0f;
+volatile state_t state = PAD;
+volatile float threshold_altitude = 30.0f;
+volatile float threshold_velocity = 30.0f;
+volatile uint8_t deployment_percent = 0;
+
+volatile vector3f linear_acceleration;
+volatile vector3f acceleration;
+volatile quarternion abs_quaternion;
+volatile vector3f velocity_vector;
+
+volatile vector3f euler_angles;
+volatile vector3f abs_lin_accel;
+volatile vector3f prev_abs_lin_accel;
+volatile vector3f rot_y_vec;
+volatile vector3f vel_at_angle;
+
+volatile vector3f accel_gravity;
+
+volatile CALIB_STATUS calib_status;
+uint8_t accel[6];
+uint8_t quat[8];
+
+repeating_timer_t data_timer;
+repeating_timer_t log_timer;
+
+float ground_altitude = 0.0f;
+
+SimpleKalmanFilter altitudeKF(2, 2, 0.01);
+SimpleKalmanFilter velocityKF(1, 1, 0.01);
+
+/**
+ * @brief Main function
+ *
+ * @return int error code
+ */
int main() {
+ // stdio_init_all();
+
+ cyw43_arch_init();
+ i2c_init(i2c_default, MAX_SCL);
+ gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+ gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+ gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+ gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+ // Enable SPI 0 at 60 MHz and connect to GPIOs
+ spi_init(spi_default, 1000 * 1000 * 60);
+ gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
+
+ // Chip select is active-low, so we'll initialise it to a driven-high state
+ gpio_init(PICO_DEFAULT_SPI_CSN_PIN);
+ gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
+ gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
+
+ gpio_init(INT1_PIN);
+ gpio_pull_up(INT1_PIN);
+
+ alarm_pool_init_default();
+
+
+ // Initialize altimeter
+ init_altimeter();
+
+ // Initialize BNO055
+ bno055.init();
+
+ // Initialize PWM
+ pwm.init();
+
+ // Initialize MOSFET
+ gpio_init(MOSFET_PIN);
+ gpio_set_dir(MOSFET_PIN, GPIO_OUT);
+
+ // Initialize Kalman Filter
+ float measurement = get_altitude();
+ float v_measurement = get_velocity();
+ altitudeKF.updateEstimate(measurement);
+ velocityKF.updateEstimate(measurement);
+
+ ground_altitude = altitude;
+
+ sem_init(&sem, 1, 1);
+
+ if (!add_repeating_timer_us(-1000000 / DATA_RATE_HZ, &timer_callback, NULL, &data_timer)) {
+ // printf("Failed to add timer!\n");
+ return -1;
+ }
+
+ multicore_launch_core1(logging_core);
+
+ while (1) {
+ tight_loop_contents();
+ }
+}
+
+void logging_core() {
+ add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_callback, NULL, &log_timer);
+
+ while (1) {
+ tight_loop_contents();
+ }
+}
+
+/**
+ * @brief Initializes the altimeter
+ *
+ * @param altitude passes the altitude variable to the function
+ * @param threshold_altitude passes the threshold altitude variable to the function
+ */
+void init_altimeter() {
+
+ uint8_t config[2] = {0};
+
+ // Select control register(0x26)
+ // Active mode, OSR = 16, altimeter mode(0xB8)
+ config[0] = 0x26;
+ config[1] = 0x89;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select data configuration register(0x13)
+ // Data ready event enabled for altitude, pressure, temperature(0x07)
+ // config[0] = 0x13;
+ // config[1] = 0x07;
+ // i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Below configures the interrupt for the first transition from PAD to BOOST
+ // Initial Reading
+
+ sleep_ms(1000);
+
+ while (altitude == 0.0f) {
+ altitude = get_altitude();
+ }
+
+ threshold_altitude += altitude; // 30 meters above ground
+
+ // printf("threshold_altitude: %4.2f", threshold_altitude);
+
+ // Select control register 3 (0x28)
+ // Set bot interrupt pins to active low and enable internal pullups
+ config[0] = 0x28;
+ config[1] = 0x01;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select pressure target MSB register(0x16)
+ // Set altitude target to 30 meters above ground altitude
+ config[0] = 0x16;
+ config[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
+ // printf("threshold_alt upper half: %X\n", config[1]);
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select pressure target LSB register(0x17)
+ // Set altitude target to 30 meters above ground altitude
+ config[0] = 0x17;
+ config[1] = (uint8_t) (((int16_t)(threshold_altitude)));
+ // printf("threshold_alt lower half: %X\n", config[1]);
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select interrupt enable register (0x29)
+ // Set interrupt enabled for altitude threshold(0x08)
+ config[0] = 0x29;
+ config[1] = 0x08;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select interrupt configuration register (0x2A)
+ // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+ config[0] = 0x2A;
+ config[1] = 0x08;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &pad_callback);
+ // End of configuration of interrupt for first transition from PAD to BOOST
+}
+
+void snapshot() {
+ if (state != END) {
+ uint8_t entry[PACKET_SIZE];
+ absolute_time_t now = get_absolute_time();
+ uint64_t now_us = to_us_since_boot(now);
+ uint32_t alt_bits = *((uint32_t *)&altitude);
+ uint32_t vel_bits = *((uint32_t *)&velocity);
+ uint32_t acc_bits = *((uint32_t *)&abs_lin_accel.z);
+ entry[0] = now_us >> 56;
+ entry[1] = now_us >> 48;
+ entry[2] = now_us >> 40;
+ entry[3] = now_us >> 32;
+ entry[4] = now_us >> 24;
+ entry[5] = now_us >> 16;
+ entry[6] = now_us >> 8;
+ entry[7] = now_us;
+
+ switch (state) {
+ case PAD:
+ entry[8] = 'P';
+ break;
+ case BOOST:
+ entry[8] = 'B';
+ break;
+ case COAST:
+ entry[8] = 'C';
+ break;
+ case APOGEE:
+ entry[8] = 'A';
+ break;
+ case RECOVERY:
+ entry[8] = 'R';
+ break;
+ case END:
+ entry[8] = 'E';
+ break;
+ }
+
+ entry[9] = deployment_percent;
+ entry[10] = alt_bits >> 24;
+ entry[11] = alt_bits >> 16;
+ entry[12] = alt_bits >> 8;
+ entry[13] = alt_bits;
+ entry[14] = vel_bits >> 24;
+ entry[15] = vel_bits >> 16;
+ entry[16] = vel_bits >> 8;
+ entry[17] = vel_bits;
+
+ entry[18] = quat[0];
+ entry[19] = quat[1];
+ entry[20] = quat[2];
+ entry[21] = quat[3];
+ entry[22] = quat[4];
+ entry[23] = quat[5];
+ entry[24] = quat[6];
+ entry[25] = quat[7];
+
+ entry[26] = acc_bits >> 24;
+ entry[27] = acc_bits >> 16;
+ entry[28] = acc_bits >> 8;
+ entry[29] = acc_bits;
+ entry[30] = accel[4];
+ entry[31] = accel[5];
+ write_entry(entry);
+ }
+}
+
+bool logging_callback(repeating_timer_t *rt) {
+ static bool LED_STATUS = 0;
+ sem_acquire_blocking(&sem);
+ LED_STATUS = !LED_STATUS;
+ cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, LED_STATUS);
+ snapshot();
+ sem_release(&sem);
+ return true;
+}
+
+bool timer_callback(repeating_timer_t *rt) {
+ absolute_time_t last = get_absolute_time();
+ sem_acquire_blocking(&sem);
+ float measurement = get_altitude();
+ altitude = altitudeKF.updateEstimate(measurement);
+ // float in_velocity = (altitude - prev_altitude) * DATA_RATE_HZ;
+ // velocity = velocityKF.updateEstimate(in_velocity);
+ float acceldata[3];
+ float gyrodata[3];
+
+
+ // printf("Velocity_Delta: %4.2f\tVelocity_Prev: %4.2f\n", velocity, ((altitude - prev_altitude) * DATA_RATE_HZ));
+
+ bno055.read_lin_accel();
+ // printf("Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n",
+ // linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);
+
+ bno055.read_abs_quaternion();
+ // printf("Absolute Quaternion:\n" "w: %f\n" "x: %f\n" "y: %f\n" "z: %f\n",
+ // abs_quaternion.w, abs_quaternion.x, abs_quaternion.y, abs_quaternion.z);
+
+ bno055.read_euler_angles();
+ // printf("Euler Angles:\n" "Roll: %f\n" "Pitch: %f\n" "Yaw: %f\n",
+ // euler_angles.x, euler_angles.y, euler_angles.z);
+
+ // Linear Acceleration and Absolute Quaternion are used to calculate Absolute Linear Acceleration
+ // They must be read before calling this function
+ bno055.calculate_abs_linear_acceleration();
+ // printf("Absolute Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n",
+ // abs_lin_accel.x, abs_lin_accel.y, abs_lin_accel.z);
- SurfaceFitModel sfm = SurfaceFitModel();
- ActuationPlan plan = ActuationPlan(sfm);
- ADS ads = ADS(plan);
- ads.run();
- return EXIT_SUCCESS;
+ acceldata[0] = abs_lin_accel.x;
+ acceldata[1] = abs_lin_accel.y;
+ acceldata[2] = abs_lin_accel.z - 0.4f;
+ gyrodata[0] = 0;
+ gyrodata[1] = 0;
+ gyrodata[2] = 0;
+
+ vKF.estimate(acceldata, gyrodata, altitude, to_us_since_boot(last));
+ velocity = vKF.getVerticalVelocity();
+ // printf("Measurement: %4.2f, Altitude: %4.2f, Velocity: %4.2f\n", measurement, altitude, velocity);
+ // This is wrong but i'm going home.
+ // velocity_vector.x = (prev_abs_lin_accel.x - abs_lin_accel.x) / 0.01f);
+ // velocity_vector.y = (prev_abs_lin_accel.y - abs_lin_accel.y) / 0.01f);
+ // velocity_vector.z = (prev_abs_lin_accel.z - abs_lin_accel.z) / 0.01f);
+ // printf("Velocity Vector:\n" "x: %f\n" "y: %f\n" "z: %f\n",
+ // velocity_vector.x, velocity_vector.y, velocity_vector.z);
+
+ prev_abs_lin_accel.x = abs_lin_accel.x;
+ prev_abs_lin_accel.y = abs_lin_accel.y;
+ prev_abs_lin_accel.z = abs_lin_accel.z;
+
+ bno055.accel_to_gravity();
+ float agl = altitude - ground_altitude;
+
+ deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, agl)), 100.0f));
+
+ switch(state) {
+ case PAD:
+ gpio_put(MOSFET_PIN, 0);
+ pwm.set_servo_percent(0);
+ deployment_percent = 0;
+ break;
+ case BOOST:
+ gpio_put(MOSFET_PIN, 1);
+ pwm.set_servo_percent(0);
+ deployment_percent = 0;
+ break;
+ case COAST:
+ gpio_put(MOSFET_PIN, 1);
+ pwm.set_servo_percent(deployment_percent);
+ break;
+ case APOGEE:
+ gpio_put(MOSFET_PIN, 1);
+ pwm.set_servo_percent(0);
+ deployment_percent = 0;
+ break;
+ case RECOVERY:
+ gpio_put(MOSFET_PIN, 1);
+ pwm.set_servo_percent(0);
+ deployment_percent = 0;
+ break;
+ case END:
+ gpio_put(MOSFET_PIN, 1);
+ pwm.set_servo_percent(0);
+ deployment_percent = 0;
+ break;
+ }
+ prev_altitude = altitude;
+ sem_release(&sem);
+ return true;
}
+
+/**
+ * @brief Test function for timer callback outputs data in ROS2 format
+ *
+ * @param rt
+ * @return true
+ * @return false
+ */
+// bool test_timer_callback(repeating_timer_t *rt) {
+// static float prev_altitude = altitude;
+// absolute_time_t last = get_absolute_time();
+// altitude = get_altitude();
+// velocity = ((altitude - prev_altitude) / 0.01f);
+// prev_altitude = altitude;
+//
+// bno055.read_lin_accel();
+// bno055.read_abs_quaternion();
+//
+// absolute_time_t now = get_absolute_time();
+// int64_t time_delta = absolute_time_diff_us(last, now);
+//
+// std::cout << altitude << " " << abs_quaternion.w << " "
+// << abs_quaternion.x << " "
+// << abs_quaternion.y << " "
+// << abs_quaternion.z << " "
+// << linear_acceleration.x << " "
+// << linear_acceleration.y << " "
+// << linear_acceleration.z << std::endl;
+//
+// /* switch (state) {
+// case PAD:
+// printf("P\n");
+// break;
+// case BOOST:
+// printf("B\n");
+// break;
+// case COAST:
+// printf("C\n");
+// break;
+// case APOGEE:
+// printf("A\n");
+// break;
+// case RECOVERY:
+// printf("R\n");
+// break;
+// case END:
+// printf("E\n");
+// break;
+// }*/
+//
+// // absolute_time_t now = get_absolute_time();
+// // int64_t time_delta = absolute_time_diff_us(last, now);
+// // printf("Time Delta: %" PRIi64"\n", time_delta);
+// // std::flush(std::cout);
+// prev_altitude = altitude;
+// return true;
+// }
+
+/**
+ * @brief Call back function for when rocket is on the pad
+ *
+ * @param gpio pin number of interrupt
+ * @param event_mask interrupt condition, value is set by PICO_SDK
+ * GPIO_IRQ_LEVEL_LOW = 0x1u,
+ * GPIO_IRQ_LEVEL_HIGH = 0x2u,
+ * GPIO_IRQ_EDGE_FALL = 0x4u,
+ * GPIO_IRQ_EDGE_RISE = 0x8u,
+ * @link https://www.raspberrypi.com/documentation/pico-sdk/hardware/gpio.html#ga6347e27da3ab34f1ea65b5ae16ab724f
+ */
+void pad_callback(uint gpio, uint32_t event_mask) {
+
+ /// @link https://www.raspberrypi.com/documentation/pico-sdk/hardware.html#ga6165f07f4b619dd08ea6dc97d069e78a
+ /// Each pin only supports one call back, so by calling this we overwrite the previous one
+ gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, false, &pad_callback);
+ uint8_t config[2] = {0};
+ // Select interrupt enable register (0x29)
+ // Set interrupt enabled for altitude threshold(0x08)
+ config[0] = 0x29;
+ config[1] = 0x00;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select interrupt configuration register (0x2A)
+ // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+ config[0] = 0x2A;
+ config[1] = 0x00;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ sem_acquire_blocking(&sem);
+ state = BOOST;
+ // start motor burn timer with this function as callback
+ add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false);
+ snapshot();
+ sem_release(&sem);
+}
+
+int64_t boost_callback(alarm_id_t id, void* user_data) {
+ // Configure accelerometer and/or altimeter to generate interrupt
+ // for when velocity is negative with this function as callback to
+ // transition to APOGEE
+ sem_acquire_blocking(&sem);
+ state = COAST;
+ snapshot();
+ sem_release(&sem);
+ add_alarm_in_ms(1000, &coast_callback, NULL, false);
+ return 0;
+}
+
+int64_t coast_callback(alarm_id_t id, void* user_data) {
+ // Want to somehow immediately transition to RECOVERY from APOGEE (extremely short timer?)
+ if (velocity <= 0.0f) {
+ sem_acquire_blocking(&sem);
+ state = APOGEE;
+ snapshot();
+ sem_release(&sem);
+ add_alarm_in_ms(1, &apogee_callback, NULL, false);
+ } else {
+ add_alarm_in_ms(250, &coast_callback, NULL, false);
+ }
+ return 0;
+}
+
+int64_t apogee_callback(alarm_id_t id, void* user_data) {
+ // Set altimeter interrupt to occur for when rocket touches back to the ground
+
+ uint8_t config[2] = {0};
+ // Select pressure target MSB register(0x16)
+ // Set altitude target to 10 meters above ground altitude
+ float recov_altitude = ground_altitude + 10.0f;
+ // Select pressure target MSB register(0x16)
+ // Set altitude target to 30 meters above ground altitude
+ config[0] = 0x16;
+ config[1] = (uint8_t) (((int16_t)(recov_altitude)) >> 8);
+ // printf("threshold_alt upper half: %X\n", config[1]);
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select pressure target LSB register(0x17)
+ // Set altitude target to 30 meters above ground altitude
+ config[0] = 0x17;
+ config[1] = (uint8_t) (((int16_t)(recov_altitude)));
+ // printf("threshold_alt lower half: %X\n", config[1]);
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select interrupt enable register (0x29)
+ // Set interrupt enabled for altitude threshold(0x08)
+ config[0] = 0x29;
+ config[1] = 0x08;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ // Select interrupt configuration register (0x2A)
+ // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+ config[0] = 0x2A;
+ config[1] = 0x08;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+
+ sem_acquire_blocking(&sem);
+ state = RECOVERY;
+ snapshot();
+ sem_release(&sem);
+ gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &recovery_callback);
+ return 0;
+}
+
+void recovery_callback(uint gpio, uint32_t event_mask) {
+ // Essentially just a signal to stop logging data
+ sem_acquire_blocking(&sem);
+ state = END;
+ snapshot();
+ sem_acquire_blocking(&sem);
+}
+
+float get_altitude() {
+ uint8_t reg = 0x01;
+ uint8_t data[5];
+ i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
+ i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
+ // Exactly how MPL3115A2 datasheet says to retrieve altitude
+ float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
+ // uint32_t temp_alt = (data[1] << 24) | (data[2] << 16) | (data[3] << 8);
+ // float altitude = temp_alt / 65536.0f;
+ return altitude;
+}
+
+/**
+ * @brief Calculates the fitted Coeficient of Drag using the Surface Fit Model for the current rocket design.
+ * @param velocity Velocity
+ * @param altitude Altitude
+ *
+ * @return: Drag Coefficient (CD)
+ */
+float get_deploy_percent(float velocity, float altitude) {
+ // Lookup table (Data from 'dragSurfFit.m' for Surface Fit Model Formula)
+ float p00 = -8.498e+04;
+ float p10 = 924.4;
+ float p01 = 69.98;
+ float p20 = -3.62;
+ float p11 = -0.6196;
+ float p02 = -0.01897;
+ float p30 = 0.005983;
+ float p21 = 0.001756;
+ float p12 = 0.0001271;
+ float p03 = 1.693e-06;
+ float p40 = -3.451e-06;
+ float p31 = -1.582e-06;
+ float p22 = -2.004e-07;
+ float p13 = -7.476e-09;
+
+
+ /* MATLAB Code:
+ * return p00 + p10 * V + p01 * H + p20 * V ** 2 + \
+ * p11 * V * H + p02 * H ** 2 + p30 * V ** 3 + \
+ * p21 * V ** 2 * H + p12 * V * H ** 2 + p03 * H ** 3 + \
+ * p40 * V ** 4 + p31 * V ** 3 * H + p22 * V ** 2 * H ** 2 + \
+ * p13 * V * H ** 3
+ */
+
+ return (p00 + p10 * velocity + p01 * altitude + p20 * std::pow(velocity, 2) + p11 * velocity * altitude + p02 * std::pow(altitude, 2)
+ + p30 * std::pow(velocity, 3) + p21 * std::pow(velocity, 2) * altitude + p12 * velocity * std::pow(altitude, 2) + p03 * std::pow(altitude, 3)
+ + p40 * std::pow(velocity, 4) + p31 * std::pow(velocity, 3) * altitude + p22 * std::pow(velocity, 2) * std::pow(altitude, 2) + p13 * velocity * std::pow(altitude, 3));
+
+
+}
+
+float get_velocity() {
+ uint8_t reg = 0x07;
+ uint8_t data[5];
+ i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
+ i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
+ float delta = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
+ float vel = delta * DATA_RATE_HZ;
+ return vel;
+}
+
diff --git a/src/altimeter.cpp b/src/altimeter.cpp
new file mode 100644
index 0000000..a5c8849
--- /dev/null
+++ b/src/altimeter.cpp
@@ -0,0 +1,50 @@
+#include <stdio.h>
+
+#include "hardware/gpio.h"
+#include "boards/pico_w.h"
+#include "hardware/i2c.h"
+#include "pico/stdio.h"
+#include "pico/time.h"
+
+#define ALT_ADDR 0x60
+#define MAX_SCL 400000
+#define DATA_RATE_HZ 15
+
+float altitude = 0.0f;
+float get_altitude();
+
+int main() {
+ stdio_init_all();
+
+ i2c_init(i2c_default, MAX_SCL);
+ gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+ gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+ gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+ gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+ uint8_t config[2] = {0};
+
+ // Select control register(0x26)
+ // Active mode, OSR = 16, altimeter mode(0xB8)
+ config[0] = 0x26;
+ config[1] = 0xB9;
+ i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+ sleep_ms(1500);
+
+ while (1) {
+ sleep_ms(1000);
+ altitude = get_altitude();
+ printf("Altitude: %4.2f\n", altitude);
+ }
+}
+
+float get_altitude() {
+ uint8_t reg = 0x01;
+ uint8_t data[5];
+ i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
+ i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
+ // Exactly how MPL3115A2 datasheet says to retrieve altitude
+ float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
+ return altitude;
+}
+
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);
+}
+
diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp
index 735e8c8..8174513 100644
--- a/src/kalmanfilter.cpp
+++ b/src/kalmanfilter.cpp
@@ -1,3 +1,4 @@
+#include <climits>
#include "../include/kalmanfilter.hpp"
// Private----------------------------------------------------------------------
@@ -25,7 +26,7 @@ void KalmanFilter::matrixInit() {
process_noise_covariance(1,1) = 0.1;
// Setup Measurement Covariance
- measurement_covariance << 0.1;
+ measurement_covariance << 1e-12;
}
diff --git a/src/pwm.cpp b/src/pwm.cpp
new file mode 100644
index 0000000..3a78d23
--- /dev/null
+++ b/src/pwm.cpp
@@ -0,0 +1,35 @@
+#include "pwm.hpp"
+
+void PWM::init() {
+ // Tell GPIO 0 they are allocated to the PWM
+ gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
+ // Find out which PWM slice is connected to GPIO 0 (it's slice 0)
+ uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
+
+ // Configure PWM slice and set it running
+ pwm_config cfg = pwm_get_default_config();
+ // Set the PWM clock divider to 38.15
+ pwm_config_set_clkdiv(&cfg, CLOCK_DIV_RATE);
+ // Set the PWM wrap value to 65535
+ pwm_set_wrap(slice_num, WRAP_VALUE);
+ // Set the PWM duty cycle to 0 and enable the PWM
+ pwm_init(slice_num, &cfg, true);
+
+ // Enable the PWM again cause idk
+ pwm_set_enabled(slice_num, true);
+}
+
+void PWM::set_duty_cycle(int duty_cycle_percent) {
+ // Calculate the raw value
+ uint32_t raw_value = WRAP_VALUE * (duty_cycle_percent / 100.0);
+
+ // Set the duty cycle
+ pwm_set_gpio_level(SERVO_PIN, raw_value);
+}
+
+void PWM::set_servo_percent(int percent) {
+ // Calculate the value by clamping the percent from 0 to 100
+ // to the SERVO_MIN and SERVO_MAX
+ uint32_t value = ((percent * SERVO_RANGE) / 100) + SERVO_MIN;
+ PWM::set_duty_cycle(value);
+}
diff --git a/src/read_flash.c b/src/read_flash.c
new file mode 100644
index 0000000..71d2870
--- /dev/null
+++ b/src/read_flash.c
@@ -0,0 +1,71 @@
+#include <stdio.h>
+#include <inttypes.h>
+#include "boards/pico_w.h"
+#include "hardware/spi.h"
+#include "spi_flash.h"
+
+int main() {
+ stdio_init_all();
+ getchar();
+ // Enable SPI 0 at 1 MHz and connect to GPIOs
+ spi_init(spi_default, 1000 * 1000 * 60);
+ gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
+
+ // Chip select is active-low, so we'll initialise it to a driven-high state
+ gpio_init(PICO_DEFAULT_SPI_CSN_PIN);
+ gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
+ gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
+
+ uint8_t entry[PACKET_SIZE];
+
+ // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN);
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
+ for (uint16_t i = 0; i < FLASH_PAGE_SIZE; i += PACKET_SIZE) {
+ if (page_buffer[i] == 0xFF) {
+ base_addr += i;
+ break;
+ }
+ if ((i + PACKET_SIZE) == FLASH_PAGE_SIZE) {
+ base_addr += FLASH_PAGE_SIZE;
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
+ i = 0;
+ }
+ }
+
+ printf("\nRead Data:\n");
+ printf("time (us) | state | dep pcnt | alt (m) | vel (m/s) | quat_w | quat_x | quat_y | quat_z | lin_ax | lin_ay | lin_az\n");
+ for (uint32_t i = 0; i < base_addr; i += PACKET_SIZE) {
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, i, entry, PACKET_SIZE);
+ uint64_t now_us = (((uint64_t)entry[0] << 56) | ((uint64_t)entry[1] << 48) | \
+ ((uint64_t)entry[2] << 40) | ((uint64_t)entry[3] << 32) | \
+ ((uint64_t)entry[4] << 24) | ((uint64_t)entry[5] << 16) | \
+ ((uint64_t)entry[6] << 8) | ((uint64_t)entry[7]));
+
+ uint8_t state = entry[8];
+ uint8_t deploy_percent = entry[9];
+
+ uint32_t alt_bits = (entry[10] << 24) | (entry[11] << 16) | (entry[12] << 8) | (entry[13]);
+ uint32_t vel_bits = (entry[14] << 24) | (entry[15] << 16) | (entry[16] << 8) | (entry[17]);
+ float altitude = *(float *)(&alt_bits);
+ float velocity = *(float *)(&vel_bits);
+
+ int16_t w = ((int16_t)entry[18]) | (((int16_t)entry[19]) << 8);
+ int16_t x = ((int16_t)entry[20]) | (((int16_t)entry[21]) << 8);
+ int16_t y = ((int16_t)entry[22]) | (((int16_t)entry[23]) << 8);
+ int16_t z = ((int16_t)entry[24]) | (((int16_t)entry[25]) << 8);
+ float qw = ((float)w) / 16384.0; // 2^14 LSB
+ float qx = ((float)x) / 16384.0;
+ float qy = ((float)y) / 16384.0;
+ float qz = ((float)z) / 16384.0;
+ int16_t ax = ((int16_t)entry[26]) | (((int16_t)entry[27]) << 8);
+ int16_t ay = ((int16_t)entry[28]) | (((int16_t)entry[29]) << 8);
+ int16_t az = ((int16_t)entry[30]) | (((int16_t)entry[31]) << 8);
+ float lax = ((float)x) / 100.0;
+ float lay = ((float)y) / 100.0;
+ float laz = ((float)z) / 100.0;
+ printf("%"PRIu64" | %c | %"PRIu8" | %4.2f | %4.2f | %4.2f | %4.2f| %4.2f | %4.2f | %4.2f | %4.2f |%4.2f\n", \
+ now_us, state, deploy_percent, altitude, velocity, qw, qx, qy, qz, lax, lay, laz);
+ }
+}
diff --git a/src/servo_test.cpp b/src/servo_test.cpp
new file mode 100644
index 0000000..c5e8e6e
--- /dev/null
+++ b/src/servo_test.cpp
@@ -0,0 +1,27 @@
+#include <stdio.h>
+#include <inttypes.h>
+#include "pico/stdio.h"
+#include "pwm.hpp"
+
+#define MOSFET_PIN 1
+
+PWM pwm;
+
+int main() {
+ stdio_init_all();
+ // Initialize MOSFET
+ gpio_init(MOSFET_PIN);
+ gpio_set_dir(MOSFET_PIN, GPIO_OUT);
+ gpio_put(MOSFET_PIN, 1);
+ pwm.init();
+ uint8_t duty_cycle = 13;
+ while (1) {
+ getchar();
+ if (duty_cycle == 2) {
+ duty_cycle = 13;
+ }
+ pwm.set_duty_cycle(duty_cycle);
+ printf("Currenty Duty Cycle: %" PRIu8 "\n", duty_cycle);
+ duty_cycle--;
+ }
+}
diff --git a/src/spi_flash.c b/src/spi_flash.c
new file mode 100644
index 0000000..32babc5
--- /dev/null
+++ b/src/spi_flash.c
@@ -0,0 +1,267 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+// Example of reading/writing an external serial flash using the PL022 SPI interface
+
+#include "spi_flash.h"
+
+static inline void cs_select(uint cs_pin) {
+ asm volatile("nop \n nop \n nop"); // FIXME
+ gpio_put(cs_pin, 0);
+ asm volatile("nop \n nop \n nop"); // FIXME
+}
+
+static inline void cs_deselect(uint cs_pin) {
+ asm volatile("nop \n nop \n nop"); // FIXME
+ gpio_put(cs_pin, 1);
+ asm volatile("nop \n nop \n nop"); // FIXME
+}
+
+void __not_in_flash_func(flash_read)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* dest, size_t len) {
+ cs_select(cs_pin);
+ uint8_t cmdbuf[4] = {
+ FLASH_CMD_READ,
+ addr >> 16,
+ addr >> 8,
+ addr
+ };
+ spi_write_blocking(spi, cmdbuf, 4);
+ spi_read_blocking(spi, 0, dest, len);
+ cs_deselect(cs_pin);
+}
+
+void __not_in_flash_func(flash_write_enable)(spi_inst_t *spi, uint cs_pin) {
+ cs_select(cs_pin);
+ uint8_t cmd = FLASH_CMD_WRITE_EN;
+ spi_write_blocking(spi, &cmd, 1);
+ cs_deselect(cs_pin);
+}
+
+void __not_in_flash_func(flash_wait_done)(spi_inst_t *spi, uint cs_pin) {
+ uint8_t status;
+ do {
+ cs_select(cs_pin);
+ uint8_t buf[2] = {FLASH_CMD_STATUS, 0};
+ spi_write_read_blocking(spi, buf, buf, 2);
+ cs_deselect(cs_pin);
+ status = buf[1];
+ } while (status & FLASH_STATUS_BUSY_MASK);
+}
+
+void __not_in_flash_func(flash_sector_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
+ uint8_t cmdbuf[4] = {
+ FLASH_CMD_SECTOR_ERASE,
+ addr >> 16,
+ addr >> 8,
+ addr
+ };
+ flash_write_enable(spi, cs_pin);
+ cs_select(cs_pin);
+ spi_write_blocking(spi, cmdbuf, 4);
+ cs_deselect(cs_pin);
+ flash_wait_done(spi, cs_pin);
+}
+
+void __not_in_flash_func(flash_block_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
+ uint8_t cmdbuf[4] = {
+ FLASH_CMD_BLOCK_ERASE,
+ addr >> 16,
+ addr >> 8,
+ addr
+ };
+ flash_write_enable(spi, cs_pin);
+ cs_select(cs_pin);
+ spi_write_blocking(spi, cmdbuf, 4);
+ cs_deselect(cs_pin);
+ flash_wait_done(spi, cs_pin);
+}
+
+
+void __not_in_flash_func(flash_erase)(spi_inst_t *spi, uint cs_pin) {
+ uint8_t cmdbuf[1] = {
+ FLASH_CMD_CHIP_ERASE
+ };
+ flash_write_enable(spi, cs_pin);
+ cs_select(cs_pin);
+ spi_write_blocking(spi, cmdbuf, 1);
+ cs_deselect(cs_pin);
+ flash_wait_done(spi, cs_pin);
+}
+
+void __not_in_flash_func(flash_page_program)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src) {
+ uint8_t cmdbuf[4] = {
+ FLASH_CMD_PAGE_PROGRAM,
+ addr >> 16,
+ addr >> 8,
+ addr
+ };
+ flash_write_enable(spi, cs_pin);
+ cs_select(cs_pin);
+ spi_write_blocking(spi, cmdbuf, 4);
+ spi_write_blocking(spi, src, FLASH_PAGE_SIZE);
+ cs_deselect(cs_pin);
+ flash_wait_done(spi, cs_pin);
+}
+
+void __not_in_flash_func(flash_write)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src, size_t size) {
+ uint8_t cmdbuf[4] = {
+ FLASH_CMD_PAGE_PROGRAM,
+ addr >> 16,
+ addr >> 8,
+ addr
+ };
+ flash_write_enable(spi, cs_pin);
+ cs_select(cs_pin);
+ spi_write_blocking(spi, cmdbuf, 4);
+ spi_write_blocking(spi, src, size);
+ cs_deselect(cs_pin);
+ flash_wait_done(spi, cs_pin);
+}
+
+void write_entry(uint8_t* data_entry) {
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
+ for (uint16_t i = 0; i < FLASH_PAGE_SIZE; i += PACKET_SIZE) {
+ if (page_buffer[i] == 0xFF) {
+ base_addr += i;
+ break;
+ }
+ if ((i + PACKET_SIZE) == FLASH_PAGE_SIZE) {
+ base_addr += FLASH_PAGE_SIZE;
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
+ i = 0;
+ }
+ }
+ flash_write(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, data_entry, PACKET_SIZE);
+ base_addr += PACKET_SIZE;
+}
+
+
+#ifdef FLASH_TEST
+ #include "pico/multicore.h"
+ #include <stdint.h>
+ #include <inttypes.h>
+ #include <stdio.h>
+
+
+ void printbuf(uint8_t buf[FLASH_PAGE_SIZE]) {
+ for (int i = 0; i < FLASH_PAGE_SIZE; ++i) {
+ if (i % 16 == 15)
+ printf("%02x\n", buf[i]);
+ else
+ printf("%02x ", buf[i]);
+ }
+ }
+
+
+
+ void core1_entry() {
+ printf("SPI flash example\n");
+
+ // Enable SPI 0 at 1 MHz and connect to GPIOs
+ spi_init(spi_default, 1000 * 1000 * 60);
+ gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);
+ gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
+
+ // Chip select is active-low, so we'll initialise it to a driven-high state
+ gpio_init(PICO_DEFAULT_SPI_CSN_PIN);
+ gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
+ gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
+
+ // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN);
+ // flash_sector_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0);
+ printf("Before program:\n");
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
+ printbuf(page_buffer);
+
+ uint8_t entry[PACKET_SIZE];
+
+ printf("Written Data:\n");
+ printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\n");
+ for (uint16_t i = 0; i < 500; i++) {
+ absolute_time_t now = get_absolute_time();
+ uint64_t now_us= to_us_since_boot(now);
+ float altitude = 10.0f * i;
+ float velocity = 5.0f * i;
+ uint8_t deploy_percent = (i*1000) / 200;
+ printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\tDAWSYN_SCHRAIB\n", now_us, (uint8_t)(i), deploy_percent, altitude, velocity);
+ uint32_t alt_bits = *((uint32_t *)&altitude);
+ uint32_t vel_bits = *((uint32_t *)&velocity);
+ entry[0] = now_us >> 56;
+ entry[1] = now_us >> 48;
+ entry[2] = now_us >> 40;
+ entry[3] = now_us >> 32;
+ entry[4] = now_us >> 24;
+ entry[5] = now_us >> 16;
+ entry[6] = now_us >> 8;
+ entry[7] = now_us;
+ entry[8] = i;
+ entry[9] = deploy_percent;
+ entry[10] = alt_bits >> 24;
+ entry[11] = alt_bits >> 16;
+ entry[12] = alt_bits >> 8;
+ entry[13] = alt_bits;
+ entry[14] = vel_bits >> 24;
+ entry[15] = vel_bits >> 16;
+ entry[16] = vel_bits >> 8;
+ entry[17] = vel_bits;
+ entry[18] = 'D';
+ entry[19] = 'A';
+ entry[20] = 'W';
+ entry[21] = 'S';
+ entry[22] = 'Y';
+ entry[23] = 'N';
+ entry[24] = '_';
+ entry[25] = 'S';
+ entry[26] = 'C';
+ entry[27] = 'H';
+ entry[28] = 'R';
+ entry[29] = 'A';
+ entry[30] = 'I';
+ entry[31] = 'B';
+ write_entry(entry);
+ }
+
+ printf("After program:\n");
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
+ printbuf(page_buffer);
+
+ printf("\nRead Data:\n");
+ printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\n");
+ for (uint32_t i = 0; i < base_addr; i += PACKET_SIZE) {
+ flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, i, entry, PACKET_SIZE);
+ uint64_t now_us = (((uint64_t)entry[0] << 56) | ((uint64_t)entry[1] << 48) | \
+ ((uint64_t)entry[2] << 40) | ((uint64_t)entry[3] << 32) | \
+ ((uint64_t)entry[4] << 24) | ((uint64_t)entry[5] << 16) | \
+ ((uint64_t)entry[6] << 8) | ((uint64_t)entry[7]));
+
+ uint8_t state = entry[8];
+ uint8_t deploy_percent = entry[9];
+
+ uint32_t alt_bits = (entry[10] << 24) | (entry[11] << 16) | (entry[12] << 8) | (entry[13]);
+ uint32_t vel_bits = (entry[14] << 24) | (entry[15] << 16) | (entry[16] << 8) | (entry[17]);
+ float altitude = *(float *)(&alt_bits);
+ float velocity = *(float *)(&vel_bits);
+ printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\t%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n", \
+ now_us, state, deploy_percent, altitude, velocity, \
+ entry[18],entry[19],entry[20],entry[21],entry[22],entry[23],entry[24],entry[25],entry[26],entry[27],entry[28],entry[29],entry[30],entry[31]);
+ }
+
+ }
+
+ int main() {
+ // Enable UART so we can print status output
+ stdio_init_all();
+ getchar();
+
+ multicore_launch_core1(core1_entry);
+
+ while (1) {
+ tight_loop_contents();
+ }
+
+ }
+#endif
diff --git a/src/unused/actuationPlan.cpp b/src/unused/actuationPlan.cpp
new file mode 100644
index 0000000..a987478
--- /dev/null
+++ b/src/unused/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/unused/ads.cpp b/src/unused/ads.cpp
new file mode 100644
index 0000000..5484970
--- /dev/null
+++ b/src/unused/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/unused/logger.cpp b/src/unused/logger.cpp
new file mode 100644
index 0000000..a857be8
--- /dev/null
+++ b/src/unused/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/unused/motor.cpp b/src/unused/motor.cpp
new file mode 100644
index 0000000..84785a9
--- /dev/null
+++ b/src/unused/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/unused/rocketUtils.cpp b/src/unused/rocketUtils.cpp
new file mode 100644
index 0000000..45fcfc3
--- /dev/null
+++ b/src/unused/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/unused/sensorAltimeter.cpp b/src/unused/sensorAltimeter.cpp
new file mode 100644
index 0000000..8ec065d
--- /dev/null
+++ b/src/unused/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/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);
+}
+
+
+
+
+
+
+
+
+
+
diff --git a/src/unused/surfaceFitModel.cpp b/src/unused/surfaceFitModel.cpp
new file mode 100644
index 0000000..d48da49
--- /dev/null
+++ b/src/unused/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);
+}
+
+
+
+
+
+
+
+
+
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-12 20:32:45 +0000