diff options
Diffstat (limited to 'src')
32 files changed, 567 insertions, 3906 deletions
diff --git a/src/AltEst/algebra.cpp b/src/AltEst/algebra.cpp deleted file mode 100644 index 653c3b9..0000000 --- a/src/AltEst/algebra.cpp +++ /dev/null @@ -1,292 +0,0 @@ -/* - 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 deleted file mode 100644 index 382103e..0000000 --- a/src/AltEst/algebra.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - 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 deleted file mode 100644 index 8838b36..0000000 --- a/src/AltEst/altitude.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* - 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 deleted file mode 100644 index 1ca6cb0..0000000 --- a/src/AltEst/altitude.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - 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 deleted file mode 100644 index 7902065..0000000 --- a/src/AltEst/filters.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/* - 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 deleted file mode 100644 index 2e316a3..0000000 --- a/src/AltEst/filters.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - 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 59d21de..e372541 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,54 +1,17 @@ add_executable(ads - 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}) + active_drag_system.cpp + spi_flash.c + imu.cpp + pwm.cpp + altimeter.cpp + kalman_filter.cpp +) + +target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm hardware_adc 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_usb(ads 0) 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 deleted file mode 100644 index 88ba5d4..0000000 --- a/src/SimpleKalmanFilter.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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 5d248e2..6f5d57e 100644 --- a/src/active_drag_system.cpp +++ b/src/active_drag_system.cpp @@ -1,37 +1,45 @@ -#include <algorithm> -#include <stdio.h> +#include "cyw43_configport.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 "hardware/adc.h" #include "pico/stdlib.h" #include "pico/time.h" #include "pico/types.h" #include "pico/cyw43_arch.h" +#include <inttypes.h> #include <math.h> -#include "bno055.hpp" -#include "AltEst/altitude.h" #include "pwm.hpp" -#include "SimpleKalmanFilter.h" +#include "imu.hpp" +#include "altimeter.hpp" +#include "kalman_filter.hpp" +#include "spi_flash.h" + +#define MPL3115A2_ADDR 0x60 + +#define BNO055_ADDR 0x28 +#define BNO055_ID 0xA0 -#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 LOG_RATE_HZ 8 +#define HEART_RATE_HZ 5 #define MOTOR_BURN_TIME 3900 // Burn time in milliseconds for M2500T +#define PAD_SECONDS 8 +#define PAD_BUFFER_SIZE (PACKET_SIZE * LOG_RATE_HZ * PAD_SECONDS) + typedef enum { - PAD, + PAD = 0, BOOST, COAST, APOGEE, @@ -39,76 +47,67 @@ typedef enum { 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 +kalman_filter *kf; +VectorXf control(1); +VectorXf measurement(1); +VectorXf res(2); 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 populate_entry(); +bool logging_buffer_callback(repeating_timer_t *rt); +bool logging_flash_callback(repeating_timer_t *rt); +bool heartbeat_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 uint8_t led_counter; +volatile uint32_t pad_buffer_offset = 0; -volatile vector3f linear_acceleration; -volatile vector3f acceleration; -volatile quarternion abs_quaternion; -volatile vector3f velocity_vector; +Eigen::Vector3f linear_acceleration; +Eigen::Vector4f quaternion; +Eigen::Vector3f euler_angles; -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]; +volatile calibration_status_t calib_status; repeating_timer_t data_timer; repeating_timer_t log_timer; +repeating_timer_t heartbeat_timer; float ground_altitude = 0.0f; -SimpleKalmanFilter altitudeKF(2, 2, 0.01); -SimpleKalmanFilter velocityKF(1, 1, 0.01); +altimeter altimeter(i2c_default, MPL3115A2_ADDR); +imu imu(i2c_default, BNO055_ADDR, BNO055_ID, NDOF); + +uint8_t *altimeter_buffer; +uint8_t *acceleration_buffer; +uint8_t *quaternion_buffer; + +uint8_t entry_buffer[PACKET_SIZE]; + +uint8_t *pad_buffer; -/** - * @brief Main function - * - * @return int error code - */ int main() { - // stdio_init_all(); + adc_init(); + adc_set_temp_sensor_enabled(true); 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); @@ -131,34 +130,39 @@ int main() { alarm_pool_init_default(); + altimeter.initialize(30.0f, INT1_PIN, &pad_callback); - // Initialize altimeter - init_altimeter(); + imu.initialize(); + imu.linear_acceleration(linear_acceleration); + imu.quaternion(quaternion); + imu.quaternion_euler(euler_angles, quaternion); - // 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); + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1); - ground_altitude = altitude; + pad_buffer = (uint8_t*)malloc(PAD_BUFFER_SIZE); + + // Initialize Kalman Filter + kf = new kalman_filter(2, 1, 1, 0.01); + VectorXf state_vec(2); + MatrixXf state_cov(2, 2); + state_vec << altimeter.get_altitude_converted(), linear_acceleration.z(); + state_cov << 0.018, 0.0, 0.0, 0.0005; + kf->state_initialize(state_vec, state_cov); + ground_altitude = altimeter.get_altitude_converted(); + + altimeter.expose_buffer(&altimeter_buffer); + imu.expose_acceleration_buffer(&acceleration_buffer); + imu.expose_quaternion_buffer(&quaternion_buffer); 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; - } + add_repeating_timer_us(-1000000 / DATA_RATE_HZ, &timer_callback, NULL, &data_timer); multicore_launch_core1(logging_core); @@ -167,218 +171,22 @@ int main() { } } -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; -} +// PRIMARY THREAD RELATED FUNCTIONS AND CALLBACKS +//=============================================================================== 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)); + imu.linear_acceleration(linear_acceleration); + imu.quaternion(quaternion); - 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); + control(0) = linear_acceleration.z(); + measurement(0) = altimeter.get_altitude_converted(); + res = kf->run(control, measurement, 0.01f); + altitude = res(0); + velocity = res(1); - 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); - - 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)); + deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, (altitude - ground_altitude))), 100.0f)); switch(state) { case PAD: @@ -411,69 +219,11 @@ bool timer_callback(repeating_timer_t *rt) { 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 @@ -485,29 +235,12 @@ bool timer_callback(repeating_timer_t *rt) { * @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); + altimeter.unset_threshold_altitude(INT1_PIN); 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); + // start motor burn timer with boost transition function as callback + add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false); } int64_t boost_callback(alarm_id_t id, void* user_data) { @@ -516,7 +249,8 @@ int64_t boost_callback(alarm_id_t id, void* user_data) { // transition to APOGEE sem_acquire_blocking(&sem); state = COAST; - snapshot(); + populate_entry(); + write_entry(entry_buffer); sem_release(&sem); add_alarm_in_ms(1000, &coast_callback, NULL, false); return 0; @@ -527,7 +261,8 @@ int64_t coast_callback(alarm_id_t id, void* user_data) { if (velocity <= 0.0f) { sem_acquire_blocking(&sem); state = APOGEE; - snapshot(); + populate_entry(); + write_entry(entry_buffer); sem_release(&sem); add_alarm_in_ms(1, &apogee_callback, NULL, false); } else { @@ -537,43 +272,14 @@ int64_t coast_callback(alarm_id_t id, void* user_data) { } int64_t apogee_callback(alarm_id_t id, void* user_data) { + state = RECOVERY; // 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); + altimeter.set_threshold_altitude((ground_altitude + 10.0f), INT1_PIN, &recovery_callback); sem_acquire_blocking(&sem); - state = RECOVERY; - snapshot(); + populate_entry(); + write_entry(entry_buffer); sem_release(&sem); - gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &recovery_callback); return 0; } @@ -581,22 +287,11 @@ void recovery_callback(uint gpio, uint32_t event_mask) { // Essentially just a signal to stop logging data sem_acquire_blocking(&sem); state = END; - snapshot(); + populate_entry(); + write_entry(entry_buffer); sem_acquire_blocking(&sem); } -float get_altitude() { - uint8_t reg = 0x01; - uint8_t data[5]; - i2c_write_blocking(i2c_default, ALT_ADDR, ®, 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 @@ -637,13 +332,105 @@ float get_deploy_percent(float velocity, float altitude) { } -float get_velocity() { - uint8_t reg = 0x07; - uint8_t data[5]; - i2c_write_blocking(i2c_default, ALT_ADDR, ®, 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; +// LOGGING THREAD RELATED FUNCTIONS AND CALLBACKS +//=============================================================================== + +void logging_core() { + add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_buffer_callback, NULL, &log_timer); + add_repeating_timer_us(-1000000 / HEART_RATE_HZ, &heartbeat_callback, NULL, &heartbeat_timer); + + while (1) { + tight_loop_contents(); + } +} + +void populate_entry() { + absolute_time_t now = get_absolute_time(); + uint64_t now_us = to_us_since_boot(now); + uint32_t vel_bits = *((uint32_t *)&velocity); + entry_buffer[0] = now_us >> 56; + entry_buffer[1] = now_us >> 48; + entry_buffer[2] = now_us >> 40; + entry_buffer[3] = now_us >> 32; + entry_buffer[4] = now_us >> 24; + entry_buffer[5] = now_us >> 16; + entry_buffer[6] = now_us >> 8; + entry_buffer[7] = now_us; + + adc_select_input(4); + uint16_t temperature = adc_read(); + entry_buffer[8] = ((*(uint8_t *)(&state)) << 4) | (temperature >> 8); + entry_buffer[9] = temperature; + + entry_buffer[10] = deployment_percent; + entry_buffer[11] = altimeter_buffer[0]; + entry_buffer[12] = altimeter_buffer[1]; + entry_buffer[13] = altimeter_buffer[2]; + entry_buffer[14] = vel_bits >> 24; + entry_buffer[15] = vel_bits >> 16; + entry_buffer[16] = vel_bits >> 8; + entry_buffer[17] = vel_bits; + entry_buffer[18] = acceleration_buffer[0]; + entry_buffer[19] = acceleration_buffer[1]; + entry_buffer[20] = acceleration_buffer[2]; + entry_buffer[21] = acceleration_buffer[3]; + entry_buffer[22] = acceleration_buffer[4]; + entry_buffer[23] = acceleration_buffer[5]; + entry_buffer[24] = quaternion_buffer[0]; + entry_buffer[25] = quaternion_buffer[1]; + entry_buffer[26] = quaternion_buffer[2]; + entry_buffer[27] = quaternion_buffer[3]; + entry_buffer[28] = quaternion_buffer[4]; + entry_buffer[29] = quaternion_buffer[5]; + entry_buffer[30] = quaternion_buffer[6]; + entry_buffer[31] = quaternion_buffer[7]; +} + +bool logging_buffer_callback(repeating_timer_t *rt) { + sem_acquire_blocking(&sem); + populate_entry(); + sem_release(&sem); + for (uint32_t i = 0; i < PACKET_SIZE; i++) { + pad_buffer[i + pad_buffer_offset] = entry_buffer[i]; + } + pad_buffer_offset += PACKET_SIZE; + pad_buffer_offset %= PAD_BUFFER_SIZE; + + if (state != PAD) { + uint32_t idx = ((pad_buffer_offset + PACKET_SIZE) % PAD_BUFFER_SIZE); + sem_acquire_blocking(&sem); + do { + write_entry(pad_buffer + idx); + idx += PACKET_SIZE; + idx %= PAD_BUFFER_SIZE; + } while (idx != pad_buffer_offset); + sem_release(&sem); + cancel_repeating_timer(&log_timer); + free(pad_buffer); + add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_flash_callback, NULL, &log_timer); + } + return true; +} + +bool logging_flash_callback(repeating_timer_t *rt) { + sem_acquire_blocking(&sem); + populate_entry(); + write_entry(entry_buffer); + sem_release(&sem); + if (state == END) { + cancel_repeating_timer(&log_timer); + } + return true; +} + +bool heartbeat_callback(repeating_timer_t *rt) { + const bool sequence[] = {true, false, true, false, false}; + const uint8_t sequence_length = 5; + + bool led_status = sequence[led_counter]; + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_status); + led_counter++; + led_counter %= sequence_length; + return true; } diff --git a/src/actuationPlan.cpp b/src/actuationPlan.cpp deleted file mode 100644 index a987478..0000000 --- a/src/actuationPlan.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "../include/actuationPlan.hpp" - -ActuationPlan::ActuationPlan() {} - -ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) { - -} - - -void ActuationPlan::runPlan(Vehicle& rocket) { - - - if (rocket.imuReadFail || rocket.altiReadFail) { - rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment - } - - rocket.fail_time = time(nullptr); - - // 2024 Mission--------------------------------------------------------------------- - if (rocket.status == GLIDE) { - - // Fin deployment based on current drag coefficient value - try { - double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude); - cd = std::min(std::max(0.0, cd), 100.0); - rocket.deployment_angle = deploy_percentage_to_angle(cd); - } - - // Full deployment during coasting - catch (...) { - rocket.deployment_angle = deploy_percentage_to_angle(0); - - if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) { - rocket.deployment_angle = deploy_percentage_to_angle(100); - } - } - } - - else if (rocket.status == APOGEE) { - - rocket.deployment_angle = deploy_percentage_to_angle(50); - } - - else { - - rocket.deploy_time = time(nullptr); - } - // End 2024 Mission------------------------------------------------------------------ -} - - - - - - - - - - - diff --git a/src/ads.cpp b/src/ads.cpp deleted file mode 100644 index 5484970..0000000 --- a/src/ads.cpp +++ /dev/null @@ -1,286 +0,0 @@ -#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/altimeter.cpp b/src/altimeter.cpp index a5c8849..9c47ceb 100644 --- a/src/altimeter.cpp +++ b/src/altimeter.cpp @@ -1,50 +1,145 @@ -#include <stdio.h> - +#include "altimeter.hpp" #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 +altimeter::altimeter(i2c_inst_t* inst, uint8_t addr) { + this->inst = inst; + this->addr = addr; +} -float altitude = 0.0f; -float get_altitude(); +void altimeter::initialize() { + // Select control register(0x26) + // Active mode, OSR = 16, altimeter mode(0xB8) + this->buffer[0] = 0x26; + this->buffer[1] = 0x89; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); +} -int main() { - stdio_init_all(); +void altimeter::initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) { + this->initialize(); - 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); + // Below configures the interrupt for the first transition from PAD to BOOST + // Initial Reading - uint8_t config[2] = {0}; + sleep_ms(1000); - // 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 altitude = 0.0f; + + while (altitude == 0.0f) { + altitude = this->get_altitude_converted(); } + + threshold_altitude += altitude; // 30 meters above ground + + // Select control register 3 (0x28) + // Set bot interrupt pins to active low and enable internal pullups + this->buffer[0] = 0x28; + this->buffer[1] = 0x01; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select pressure target MSB register(0x16) + // Set altitude target to 30 meters above ground altitude + this->buffer[0] = 0x16; + this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8); + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select pressure target LSB register(0x17) + // Set altitude target to 30 meters above ground altitude + this->buffer[0] = 0x17; + this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude))); + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + this->buffer[0] = 0x29; + this->buffer[1] = 0x08; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select interrupt this->bufferuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + this->buffer[0] = 0x2A; + this->buffer[1] = 0x08; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback); + // End of configuration of interrupt for first transition from PAD to BOOST } -float get_altitude() { +void altimeter::set_threshold_altitude(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) { + float altitude = 0.0f; + + while (altitude == 0.0f) { + altitude = get_altitude_converted(); + } + + threshold_altitude += altitude; + + // Select control register 3 (0x28) + // Set bot interrupt pins to active low and enable internal pullups + this->buffer[0] = 0x28; + this->buffer[1] = 0x01; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select pressure target MSB register(0x16) + // Set altitude target to 30 meters above ground altitude + this->buffer[0] = 0x16; + this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8); + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select pressure target LSB register(0x17) + // Set altitude target to provided threshold altitude + this->buffer[0] = 0x17; + this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude))); + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + this->buffer[0] = 0x29; + this->buffer[1] = 0x08; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select interrupt this->bufferuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + this->buffer[0] = 0x2A; + this->buffer[1] = 0x08; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback); + // End of configuration of interrupt for first transition from PAD to BOOST +} + +void altimeter::unset_threshold_altitude(uint8_t interrupt_pin) { + gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, false, NULL); + + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + this->buffer[0] = 0x29; + this->buffer[1] = 0x00; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + + // Select interrupt configuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + this->buffer[0] = 0x2A; + this->buffer[1] = 0x00; + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); +} + +float altimeter::get_altitude_converted() { uint8_t reg = 0x01; - uint8_t data[5]; - i2c_write_blocking(i2c_default, ALT_ADDR, ®, 1, true); - i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false); + i2c_write_blocking(this->inst, this->addr, ®, 1, true); + i2c_read_blocking(this->inst, this->addr, this->altitude_buffer, 4, 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; + float altitude = (float) ((int16_t) ((this->altitude_buffer[0] << 8) | this->altitude_buffer[1])) + (float) (this->altitude_buffer[2] >> 4) * 0.0625; return altitude; } +void altimeter::get_altitude_raw(uint8_t* buffer) { + uint8_t reg = 0x01; + i2c_write_blocking(this->inst, this->addr, ®, 1, true); + i2c_read_blocking(this->inst, this->addr, buffer, 3, false); +} + +uint32_t altimeter::expose_buffer(uint8_t** buffer) { + *buffer = this->altitude_buffer; + return sizeof(this->altitude_buffer); +} + diff --git a/src/bno055.cpp b/src/bno055.cpp deleted file mode 100644 index fb51986..0000000 --- a/src/bno055.cpp +++ /dev/null @@ -1,201 +0,0 @@ -#include "bno055.hpp" - -/// @link [Pico BNO055 Example](https://learnembeddedsystems.co.uk/bno005-i2c-example-code) - -BNO055::BNO055() { - bno055_address = BNO055_ADDRESS_A; - _sensorID = BNO055_ID; - default_mode = OPERATION_MODE_NDOF; -} - -void BNO055::reset_bno055() { - uint8_t data[2]; - data[0] = BNO055_SYS_TRIGGER_ADDR; - data[1] = 0x20; // Reset system - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - sleep_ms(1000); // Wait 650ms for the sensor to reset -} - -void BNO055::init() { - sleep_ms(1000); // Wait 650ms for the sensor to reset - uint8_t chip_id_addr = BNO055_CHIP_ID_ADDR; - uint8_t id[1]; - i2c_write_blocking(i2c_default, bno055_address, &chip_id_addr, 1, false); - i2c_read_blocking(i2c_default, bno055_address, id, 1, false); - if (!id[0] == _sensorID) { - printf("BNO055 not detected\n"); - } - - // Use internal oscillator - uint8_t data[2]; - data[0] = BNO055_SYS_TRIGGER_ADDR; - data[1] = 0x40; // Set to use internal oscillator - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - - // Reset all interrupt status bits - data[0] = BNO055_SYS_TRIGGER_ADDR; - data[1] = 0x01; // Reset interrupt status - // 0x05 = Reset system - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - - // Set to normal power mode - data[0] = BNO055_PWR_MODE_ADDR; - data[1] = 0x00; // Normal power mode - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - sleep_ms(50); // Wait 50ms for the sensor to switch to normal power mode - - // Page 25 of the datasheet - // Default Axis Config - data[0] = BNO055_AXIS_MAP_CONFIG_ADDR; - data[1] = 0x24; // P1=Z, P2=Y, P3=X - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - - // Default Axis Sign - data[0] = BNO055_AXIS_MAP_SIGN_ADDR; - data[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - - // Set units to m/s^2 - data[0] = BNO055_UNIT_SEL_ADDR; - data[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2 - i2c_write_blocking(i2c_default, bno055_address, data, 2, true); - sleep_ms(30); - - //The default operation mode after power-on is CONFIGMODE - // Set mode to NDOF - // Takes 7ms to switch from CONFIG mode; see page 21 on datasheet (3.3) - data[0] = BNO055_OPR_MODE_ADDR; - data[1] = default_mode; // NDOF - i2c_write_blocking(i2c_default, bno055_address, data, 2, false); - sleep_ms(100); - - -} - -void BNO055::read_calib_status() { - uint8_t calib_stat_reg = BNO055_CALIB_STAT_ADDR; - uint8_t calib_stat[1]; - i2c_write_blocking(i2c_default, bno055_address, &calib_stat_reg, 1, true); - i2c_read_blocking(i2c_default, bno055_address, calib_stat, 1, false); - calib_status.mag = ((calib_stat[0] & 0b00000011) >> 0); - calib_status.accel = ((calib_stat[0] & 0b00001100) >> 2); - calib_status.gyro = ((calib_stat[0] & 0b00110000) >> 4); - calib_status.sys = ((calib_stat[0] & 0b11000000) >> 6); -} - -void BNO055::read_lin_accel() { - uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; - i2c_write_blocking(i2c_default, bno055_address, &lin_accel_reg, 1, true); - i2c_read_blocking(i2c_default, bno055_address, accel, 6, false); - int16_t x, y, z; - x = y = z = 0; - x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8); - y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8); - z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8); - linear_acceleration.x = ((float)x) / 100.0; - linear_acceleration.y = ((float)y) / 100.0; - linear_acceleration.z = ((float)z) / 100.0; -} - -void BNO055::clamp_close_zero(volatile float &val) { - if (val < 0.01 && val > -0.01) { - val = 0.0; - } -} - -void BNO055::accel_to_gravity() { - accel_gravity.x = abs_lin_accel.x / 9.81; - accel_gravity.y = abs_lin_accel.y / 9.81; - accel_gravity.z = abs_lin_accel.z / 9.81; -} - -void BNO055::read_abs_quaternion() { - uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR; - i2c_write_blocking(i2c_default, bno055_address, &quat_reg, 1, true); - i2c_read_blocking(i2c_default, bno055_address, quat, 8, false); - int16_t w, x, y, z; - w = x = y = z = 0; - w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8); - x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8); - y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8); - z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8); - abs_quaternion.w = ((float)w) / 16384.0; // 2^14 LSB - abs_quaternion.x = ((float)x) / 16384.0; - abs_quaternion.y = ((float)y) / 16384.0; - abs_quaternion.z = ((float)z) / 16384.0; -} - -void BNO055::read_euler_angles() { - uint8_t euler[6]; - uint8_t euler_reg = BNO055_EULER_H_LSB_ADDR; - i2c_write_blocking(i2c_default, bno055_address, &euler_reg, 1, true); - i2c_read_blocking(i2c_default, bno055_address, euler, 6, false); - /// @note heading = yaw - int16_t heading, roll, pitch; - heading = roll = pitch = 0; - heading = ((int16_t)euler[0]) | (((int16_t)euler[1]) << 8); - roll = ((int16_t)euler[2]) | (((int16_t)euler[3]) << 8); - pitch = ((int16_t)euler[4]) | (((int16_t)euler[5]) << 8); - euler_angles.x = ((float)roll) / 16.0; - euler_angles.y = ((float)pitch) / 16.0; - euler_angles.z = ((float)heading) / 16.0; -} - -void BNO055::read_accel() { - uint8_t accel[6]; - uint8_t accel_reg = BNO055_ACCEL_DATA_X_LSB_ADDR; - i2c_write_blocking(i2c_default, bno055_address, &accel_reg, 1, true); - i2c_read_blocking(i2c_default, bno055_address, accel, 6, false); - int16_t x, y, z; - x = y = z = 0; - x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8); - y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8); - z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8); - acceleration.x = ((float)x) / 100.0; - acceleration.y = ((float)y) / 100.0; - acceleration.z = ((float)z) / 100.0; -} - -void BNO055::quaternion_to_euler() { - Eigen::Quaternion<float> q; - q.w() = abs_quaternion.w; - q.x() = abs_quaternion.x; - q.y() = abs_quaternion.y; - q.z() = abs_quaternion.z; - q.normalize(); - Eigen::Matrix3f m = q.toRotationMatrix(); - euler_angles.x = atan2f(m(2,1), m(2,2)); - euler_angles.y = asinf(-m(2,0)); - euler_angles.z = atan2f(m(1,0), m(0,0)); -} - -void BNO055::calculate_abs_linear_acceleration() { - Eigen::Quaternion<float> q; - q.w() = abs_quaternion.w; - q.x() = abs_quaternion.x; - q.y() = abs_quaternion.y; - q.z() = abs_quaternion.z; - // q.normalize(); - Eigen::Matrix3f rotation_matrix = q.toRotationMatrix(); - Eigen::Vector3f lin_accel; - lin_accel.x() = linear_acceleration.x; - lin_accel.y() = linear_acceleration.y; - lin_accel.z() = linear_acceleration.z; - abs_lin_accel.x = lin_accel.x() * rotation_matrix(0, 0) + lin_accel.y() * rotation_matrix(0, 1) + lin_accel.z() * rotation_matrix(0, 2); - abs_lin_accel.y = lin_accel.x() * rotation_matrix(1, 0) + lin_accel.y() * rotation_matrix(1, 1) + lin_accel.z() * rotation_matrix(1, 2); - abs_lin_accel.z = -1.0f * (lin_accel.x() * rotation_matrix(2, 0) + lin_accel.y() * rotation_matrix(2, 1) + lin_accel.z() * rotation_matrix(2, 2)); -} - -void BNO055::get_rotation_vector() { - Eigen::Quaternion<float> q; - q.w() = abs_quaternion.w; - q.x() = abs_quaternion.x; - q.y() = abs_quaternion.y; - q.z() = abs_quaternion.z; - q.normalize(); - Eigen::Matrix3f rotation_matrix = q.toRotationMatrix(); - rot_y_vec.x = rotation_matrix(1, 0); - rot_y_vec.y = rotation_matrix(1, 1); - rot_y_vec.z = rotation_matrix(1, 2); -} - diff --git a/src/imu.cpp b/src/imu.cpp new file mode 100644 index 0000000..4a81042 --- /dev/null +++ b/src/imu.cpp @@ -0,0 +1,160 @@ +#include "imu.hpp" + +imu::imu(i2c_inst_t* inst, uint8_t addr, uint8_t id, imu_opmode_t mode) { + this->inst = inst; + this->addr = addr; + this->id = id; + this->mode = mode; +} + +void imu::reset() { + this->buffer[0] = SYS_TRIGGER; + this->buffer[1] = 0x20; // Reset system + i2c_write_blocking(this->inst, this->addr, buffer, 2, true); + sleep_ms(1000); // Wait 650ms for the sensor to reset +} + +void imu::initialize() { + sleep_ms(1000); // Wait 650ms for the sensor to reset + + uint8_t chip_id_addr = CHIP_ID; + uint8_t read_id = 0x00; + while (read_id != this->id) { + i2c_write_blocking(this->inst, this->addr, &chip_id_addr, 1, false); + i2c_read_blocking(this->inst, this->addr, &read_id, 1, false); + sleep_ms(100); + } + + // Use internal oscillator + this->buffer[0] = SYS_TRIGGER; + this->buffer[1] = 0x40; // Set to use internal oscillator + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(50); + + // Reset all interrupt status bits + this->buffer[0] = SYS_TRIGGER; + this->buffer[1] = 0x01; // Reset interrupt status + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(50); + + // Set to normal power mode + this->buffer[0] = POWER_MODE; + this->buffer[1] = 0x00; // Normal power mode + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(50); + + // Default Axis Config + this->buffer[0] = AXIS_MAP_CONFIG; + this->buffer[1] = 0x24; // P1=Z, P2=Y, P3=X + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(50); + + // Default Axis Sign + this->buffer[0] = AXIS_MAP_SIGN; + this->buffer[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(50); + + // Set units to m/s^2 + this->buffer[0] = UNIT_SELECTION; + this->buffer[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2 + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true); + sleep_ms(200); + + uint8_t sensor_offsets[19]; + sensor_offsets[0] = ACCELERATION_OFFSET_X_LSB; + sensor_offsets[1] = 0x00; + sensor_offsets[2] = 0x00; + sensor_offsets[3] = 0x00; + sensor_offsets[4] = 0x00; + sensor_offsets[5] = 0x00; + sensor_offsets[6] = 0x00; + sensor_offsets[7] = 0x00; + sensor_offsets[8] = 0x00; + sensor_offsets[9] = 0x00; + sensor_offsets[10] = 0x00; + sensor_offsets[11] = 0x00; + sensor_offsets[12] = 0x00; + sensor_offsets[13] = 0x00; + sensor_offsets[14] = 0x00; + sensor_offsets[15] = 0x00; + sensor_offsets[16] = 0x00; + sensor_offsets[17] = 0x00; + sensor_offsets[18] = 0x00; + i2c_write_blocking(this->inst, this->addr, sensor_offsets, 19, true); + sleep_ms(200); + + // The default operation mode after power-on is CONFIG + // Set to desired mode + this->buffer[0] = OPERATION_MODE; + this->buffer[1] = this->mode; // NDOF + i2c_write_blocking(this->inst, this->addr, this->buffer, 2, false); + sleep_ms(100); +} + +void imu::calibration_status(calibration_status_t* status) { + read_register(CALIBRATION_STATUS, 1, this->buffer); + status->mag = ((this->buffer[0] & 0b00000011) >> 0); + status->accel = ((this->buffer[0] & 0b00001100) >> 2); + status->gyro = ((this->buffer[0] & 0b00110000) >> 4); + status->sys = ((this->buffer[0] & 0b11000000) >> 6); +} + +void imu::linear_acceleration(Eigen::Vector3f& vec) { + read_register(LINEAR_ACCELERATION_X_LSB, 6, this->accel_buffer); + int16_t x, y, z; + x = y = z = 0; + x = ((int16_t)this->accel_buffer[0]) | (((int16_t)this->accel_buffer[1]) << 8); + y = ((int16_t)this->accel_buffer[2]) | (((int16_t)this->accel_buffer[3]) << 8); + z = ((int16_t)this->accel_buffer[4]) | (((int16_t)this->accel_buffer[5]) << 8); + vec(0) = ((float)x) / 100.0; + vec(1) = ((float)y) / 100.0; + vec(2) = ((float)z) / 100.0; +} + +void imu::quaternion(Eigen::Vector4f& vec) { + read_register(QUATERNION_W_LSB, 8, this->quat_buffer); + int16_t w, x, y, z; + w = x = y = z = 0; + w = ((int16_t)this->quat_buffer[0]) | (((int16_t)this->quat_buffer[1]) << 8); + x = ((int16_t)this->quat_buffer[2]) | (((int16_t)this->quat_buffer[3]) << 8); + y = ((int16_t)this->quat_buffer[4]) | (((int16_t)this->quat_buffer[5]) << 8); + z = ((int16_t)this->quat_buffer[6]) | (((int16_t)this->quat_buffer[7]) << 8); + vec(0) = ((float)w) / 16384.0; + vec(1) = ((float)x) / 16384.0; + vec(2) = ((float)y) / 16384.0; + vec(3) = ((float)z) / 16384.0; +} + +void imu::quaternion_euler(Eigen::Vector3f& angles, Eigen::Vector4f& quat) { + // roll (x-axis rotation) + float sinr_cosp = 2 * (quat(0) * quat(1) + quat(2) * quat(3)); + float cosr_cosp = 1 - 2 * (quat(1) * quat(1) + quat(2) * quat(2)); + angles(0) = Eigen::numext::atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + float sinp = Eigen::numext::sqrt(1 + 2 * (quat(0) * quat(2) - quat(1) * quat(3))); + float cosp = Eigen::numext::sqrt(1 - 2 * (quat(0) * quat(2) - quat(1) * quat(3))); + angles(1) = 2 * Eigen::numext::atan2(sinp, cosp) - M_PI / 2; + + // yaw (z-axis rotation) + float siny_cosp = 2 * (quat(0) * quat(3) + quat(1) * quat(2)); + float cosy_cosp = 1 - 2 * (quat(2) * quat(2) + quat(3) * quat(3)); + angles(2) = Eigen::numext::atan2(siny_cosp, cosy_cosp); +} + +uint32_t imu::expose_acceleration_buffer(uint8_t** buffer) { + *buffer = this->accel_buffer; + return sizeof(this->accel_buffer); +} + +uint32_t imu::expose_quaternion_buffer(uint8_t** buffer) { + *buffer = this->quat_buffer; + return sizeof(this->quat_buffer); +} + +void imu::read_register(uint8_t reg, size_t len, uint8_t* buffer) { + i2c_write_blocking(this->inst, this->addr, ®, 1, true); + i2c_read_blocking(this->inst, this->addr, buffer, len, false); +} + diff --git a/src/kalman_filter.cpp b/src/kalman_filter.cpp new file mode 100644 index 0000000..d4aff7a --- /dev/null +++ b/src/kalman_filter.cpp @@ -0,0 +1,76 @@ +#include "kalman_filter.hpp" + +void kalman_filter::matrix_initialize() { + state_vector.setZero(n); + state_covariance.setZero(n, n); + state_transition_M = MatrixXf::Zero(n, n); + control_input_M = MatrixXf::Zero(n, p); + I = MatrixXf::Identity(n, n); + measurement_M.setIdentity(m, n); // Setup Measurement Matrix + process_noise_covariance = MatrixXf::Zero(n, n); + measurement_covariance = MatrixXf::Zero(m, m); + + // Setup State Transition Matrix + state_transition_M << 1.0, dt, 0.0, 1.0; + + // Setup Control Input Matrix + control_input_M << 0.5 * dt * dt, dt; // (Linear Displacement Eq.) + + // Setup Process-Noise Covariance + process_noise_covariance(0,0) = 0.01; + process_noise_covariance(1,1) = 0.1; + + // Setup Measurement Covariance + measurement_covariance << 1e-12; +} + +void kalman_filter::matrix_update() { + state_transition_M(0, 1) = dt; + control_input_M(0, 0) = 0.5f * dt * dt; + control_input_M(1, 0) = dt; +} + +void kalman_filter::predict(VectorXf control_vec) { + state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec); + state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance; +} + +void kalman_filter::update(VectorXf measurement) { + // Innovation + VectorXf y = measurement - (measurement_M * state_vector); + + // Residual/Innovation Covariance + MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance; + + // Kalman Gain + MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse(); + + // Update + state_vector = state_vector + (K * y); + state_covariance = (I - (K * measurement_M)) * state_covariance; +} + +kalman_filter::kalman_filter(int state_dim, int control_dim, int measurement_dim, float dt) : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) { + matrix_initialize(); +} + +bool kalman_filter::state_initialize(VectorXf state_vec, MatrixXf state_cov) { + bool result { false }; + if (state_vec.size() == n && state_cov.rows() == n) { + state_vector = state_vec; + state_covariance = state_cov; + result = true; + } + return result; +} + +VectorXf kalman_filter::run(VectorXf control, VectorXf measurement, float _dt) { + if (control.size() == p && measurement.size() == m) { + dt = _dt; + matrix_update(); + predict(control); + update(measurement); + } + return state_vector; +} + diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp deleted file mode 100644 index 8174513..0000000 --- a/src/kalmanfilter.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include <climits> -#include "../include/kalmanfilter.hpp" - -// Private---------------------------------------------------------------------- -void KalmanFilter::matrixInit() { - - state_vector.setZero(n); - state_covariance.setZero(n, n); - state_transition_M = MatrixXf::Zero(n, n); - control_input_M = MatrixXf::Zero(n, p); - I = MatrixXf::Identity(n, n); - measurement_M.setIdentity(m, n); // Setup Measurement Matrix - process_noise_covariance = MatrixXf::Zero(n, n); - measurement_covariance = MatrixXf::Zero(m, m); - - // Setup State Transition Matrix - state_transition_M << 1.0, dt, - 0.0, 1.0; - - // Setup Control Input Matrix - control_input_M << 0.5 * std::pow(dt, 2), // (Linear Displacement Eq.) - dt; - - // Setup Process-Noise Covariance - process_noise_covariance(0,0) = 0.01; - process_noise_covariance(1,1) = 0.1; - - // Setup Measurement Covariance - measurement_covariance << 1e-12; -} - - -void KalmanFilter::updateMatrices() { - - state_transition_M(0, 1) = dt; - control_input_M(0, 0) = 0.5 * std::pow(dt, 2); - control_input_M(1, 0) = dt; -} - - -void KalmanFilter::prediction(VectorXf control_vec) { - - state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec); - state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance; -} - -void KalmanFilter::update(VectorXf measurement) { - - // Innovation - VectorXf y = measurement - (measurement_M * state_vector); - - // Residual/Innovation Covariance - MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance; - - // Kalman Gain - MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse(); - - // Update - state_vector = state_vector + (K * y); - state_covariance = (I - (K * measurement_M)) * state_covariance; -} - - - -// Public---------------------------------------------------------------------- -KalmanFilter::KalmanFilter() { - -} - - -KalmanFilter::KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt) - : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) { - - matrixInit(); -} - -bool KalmanFilter::setInitialState(VectorXf state_vec, MatrixXf state_cov) { - - if (state_vec.size() != n || state_cov.rows() != n) { - std::cout << "Error: Max State & Covariance Dimension should be " << n << std::endl; - return false; - } - - state_vector = state_vec; - state_covariance = state_cov; - return true; -} - - - - -VectorXf KalmanFilter::run(VectorXf control, VectorXf measurement, double _dt) { - - if (control.size() != p || measurement.size() != m) { - std::cout << "Error: Control Vector Size should be "<< p - << " Measurement Vector Size should be " << m << std::endl; - return state_vector; - } - - dt = _dt; - updateMatrices(); - - prediction(control); - update(measurement); - - return state_vector; -}
\ No newline at end of file diff --git a/src/logger.cpp b/src/logger.cpp deleted file mode 100644 index a857be8..0000000 --- a/src/logger.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#include "../include/logger.hpp" - -// Private---------------------------------------------------------------------- -std::string Logger::getDate() { - - t = time(nullptr); - now = localtime(&t); - return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " - + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")"; -} - -std::string Logger::getTime() { - - t = time(nullptr); - now = localtime(&t); - std::string hour = std::to_string(now->tm_hour); - std::string min = std::to_string(now->tm_min); - std::string sec = std::to_string(now->tm_sec); - //string hour = "0" + to_string(now->tm_hour); - - if (now->tm_hour < 10) { - hour = "0" + std::to_string(now->tm_hour); - } - - if (now->tm_min < 10) { - min = "0" + std::to_string(now->tm_min); - } - - if (now->tm_sec < 10) { - sec = "0" + std::to_string(now->tm_sec); - } - - return hour + ":" + min + - ":" + sec; -} - - -// Public---------------------------------------------------------------------- -Logger& Logger::Get() { - - static Logger loggerSingleton; - return loggerSingleton; -} - -//Logger Logger::loggerSingleton; - - -bool Logger::openLog(std::string _filename) { - - filename = _filename; - - if (file_open) { - return false; - } - - file.open(filename, std::ios::in | std::ios::out | std::ios::app); - - if (!file) { - return false; - } - - file_open = true; - std::string date = getDate(); - std::string timestamp = getTime(); - file << timestamp << infoTag << "Log Start---- " << date << std::endl; - - return true; -} - - -void Logger::closeLog() { - - std::string timestamp = getTime(); - file << timestamp << infoTag << "Log End----\n\n"; - - file.close(); - file_open = false; -} - - -bool Logger::log(std::string data) { - - if (!file) { - return false; - } - - if (!file_open) { - return false; - } - std::string timestamp = getTime(); - file << timestamp << infoTag << data << std::endl; - return true; -} - -bool Logger::logErr(std::string data) { - - if (!file) { - return false; - } - - if (!file_open) { - return false; - } - - std::string timestamp = getTime(); - file << timestamp << errorTag << data << std::endl; - return true; -} - - -bool Logger::printLog() { - - if (file.is_open()) { - std::cout << "Log still open. Please close Log." << std::endl; - return false; - } - - file.open(filename, std::ios::in); - - if (!file.is_open()) { - return false; - } - - std::string line; - while(getline(file, line)) { - std::cout << line << std::endl; - } - - file.close(); - - return true; -} diff --git a/src/motor.cpp b/src/motor.cpp deleted file mode 100644 index 84785a9..0000000 --- a/src/motor.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#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/read_flash.c b/src/read_flash.c deleted file mode 100644 index 71d2870..0000000 --- a/src/read_flash.c +++ /dev/null @@ -1,71 +0,0 @@ -#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/rocketUtils.cpp b/src/rocketUtils.cpp deleted file mode 100644 index 45fcfc3..0000000 --- a/src/rocketUtils.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "../include/rocketUtils.hpp" - -double deploy_percentage_to_angle(double percentage) { - - return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE; -} - - -std::string format_data(std::string prefix, double data, int precision) { - - std::stringstream stream; - stream << std::fixed << std::setprecision(precision) << data; - std::string s = stream.str(); - return prefix + s; -} - -bool led_out(Vehicle *vehicle) { - - std::ofstream file; - file.open(LED_FILENAME); - if (!file.is_open()) { - return false; - } - - file << std::to_string(vehicle->led_brightness); - file.close(); - - vehicle->led_time = time(nullptr); - vehicle->led_brightness = (vehicle->led_brightness + 1) % 2; - - return true; -} - -std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"}; -
\ No newline at end of file diff --git a/src/sensorAltimeter.cpp b/src/sensorAltimeter.cpp deleted file mode 100644 index 8ec065d..0000000 --- a/src/sensorAltimeter.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include "sensorAltimeter.hpp" - -AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) { - I2C_FILE = I2C_FILE_in; - deviceAddress = 0x60; -} - -//Startup routine copied from Adafruit library, as is most of the data getting methods -//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation) -bool AltimeterSensor::init() { - - // Vehicle *vehicle = (Vehicle *) data; - // // Do Stuff - // data = (void*) vehicle; - - //Pass file string from parent to setup function, actual I2C bus gets stored internally. - setupI2C(I2C_FILE); - - // Check a register with a hard-coded value to see if comms are working - uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI); - if (whoami != 0xC4) { - fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami); - return false; - } - - //Send device dedicated reset byte to CTRL1 Register - writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST); - //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register - while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST); - - //Set oversampling (?) and altitude mode by default - currentMode = MPL3115A2_ALTIMETER; - ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT; - writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); - - //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess - writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE | - MPL3115A2_PT_DATA_CFG_PDEFE | - MPL3115A2_PT_DATA_CFG_DREM); - - return true; -} - -//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION -void AltimeterSensor::setMode(mpl3115a2_mode_t mode) { - ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); - ctrl_reg1.bit.ALT = mode; - writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); - currentMode = mode; -} - -double AltimeterSensor::getAltitude() { - //Request new data reading - requestOneShotReading(); - //If new data is available, read it and store it to internal fields - if (isNewDataAvailable()) { - //Logger flag here for new data? - updateCurrentDataBuffer(); - } - //Return internal field, whether updated or not - return internalAltitude; -} - -double AltimeterSensor::getTemperature() { - //Request new data reading - requestOneShotReading(); - //If new data is available, read it and store it to internal fields - if (isNewDataAvailable()) { - //Logger flag here for new data? - updateCurrentDataBuffer(); - } - //Return internal field, whether updated or not - return internalTemperature; -} - -void AltimeterSensor::requestOneShotReading() { - //Request current status of oneshot reading - ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); - //If oneshot is complete, proc a new one; if it isn't, do nothing. - //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR. - if (!ctrl_reg1.bit.OST) { - // initiate one-shot measurement - ctrl_reg1.bit.OST = 1; - writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); - } -} - -bool AltimeterSensor::isNewDataAvailable() { - //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available - //There *are* registers available for exclusively temperature *or* pressure/altitude, but - //for simplicity's sake we'll use the combined one for now. - return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0); -} - -//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead -void AltimeterSensor::updateCurrentDataBuffer() { - uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0}; - readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5); - - //Pressure is no longer used, assumed rocket is only logging altitude - // uint32_t pressure; - // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 | - // uint32_t(buffer[2]); - // return double(pressure) / 6400.0; - - //Altitude Conversion - int32_t alt; - alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | - uint32_t(buffer[2]) << 8; - internalAltitude = double(alt) / 65536.0; - - int16_t t; - t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]); - internalTemperature = double(t) / 256.0; -} diff --git a/src/sensorIMU.cpp b/src/sensorIMU.cpp deleted file mode 100644 index 941ea35..0000000 --- a/src/sensorIMU.cpp +++ /dev/null @@ -1,385 +0,0 @@ -#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/servo_test.cpp b/src/servo_test.cpp deleted file mode 100644 index c5e8e6e..0000000 --- a/src/servo_test.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#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/surfaceFitModel.cpp b/src/surfaceFitModel.cpp deleted file mode 100644 index d48da49..0000000 --- a/src/surfaceFitModel.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#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); -} - - - - - - - - - diff --git a/src/unused/actuationPlan.cpp b/src/unused/actuationPlan.cpp deleted file mode 100644 index a987478..0000000 --- a/src/unused/actuationPlan.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#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 deleted file mode 100644 index 5484970..0000000 --- a/src/unused/ads.cpp +++ /dev/null @@ -1,286 +0,0 @@ -#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 deleted file mode 100644 index a857be8..0000000 --- a/src/unused/logger.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#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 deleted file mode 100644 index 84785a9..0000000 --- a/src/unused/motor.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#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 deleted file mode 100644 index 45fcfc3..0000000 --- a/src/unused/rocketUtils.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#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 deleted file mode 100644 index 8ec065d..0000000 --- a/src/unused/sensorAltimeter.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#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 deleted file mode 100644 index 941ea35..0000000 --- a/src/unused/sensorIMU.cpp +++ /dev/null @@ -1,385 +0,0 @@ -#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 deleted file mode 100644 index d48da49..0000000 --- a/src/unused/surfaceFitModel.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#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); -} - - - - - - - - - |