From 90c4d94b13472114daab71d3e368660224423c90 Mon Sep 17 00:00:00 2001 From: Dawsyn Schraiber <32221234+dawsynth@users.noreply.github.com> Date: Thu, 9 May 2024 01:20:17 -0400 Subject: 02/24/2024 Test Launch Version (BB Black) (#11) * Adding a 90% completed, compilable but untested ADS * Made basic changes to actuator & sensor. Also added motor class * Removed unnecessary .cpp files * Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions * Relocated 'main' to 'active-drag-system.cpp'. Added more info to README * Removed main.cpp * Added more details to README * Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace * Started writing the test cases * Updated the .gitignore file * Removed some files that should be gitignored * Up to date with Jazz's pull request * Test Launch Branch Created; PRU Servo Control with Test Program * Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION] Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly. * Partial implementation of MPL sensor Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES. * *Hypothetically* complete MPL implementation NEEDS HARDWARE TESTING * IMU Header and init() method implementation Needs like, all data handling still lol * Hypothetically functional (Definitely won't compile) * We ball? --------- Co-authored-by: Jazz Jackson Co-authored-by: Cian Capacci <46614295+BeeGuyDude@users.noreply.github.com> --- include/actuationPlan.hpp | 36 +++++ include/actuator.hpp | 30 +++++ include/ads.hpp | 66 ++++++++++ include/kalmanfilter.hpp | 98 ++++++++++++++ include/logger.hpp | 83 ++++++++++++ include/math/imumath.h | 30 +++++ include/math/matrix.h | 185 ++++++++++++++++++++++++++ include/math/quaternion.h | 214 ++++++++++++++++++++++++++++++ include/math/vector.h | 184 ++++++++++++++++++++++++++ include/motor.hpp | 36 +++++ include/rocketUtils.hpp | 121 +++++++++++++++++ include/sensorAltimeter.hpp | 165 +++++++++++++++++++++++ include/sensorI2C.hpp | 105 +++++++++++++++ include/sensorIMU.hpp | 312 ++++++++++++++++++++++++++++++++++++++++++++ include/surfaceFitModel.hpp | 32 +++++ 15 files changed, 1697 insertions(+) create mode 100644 include/actuationPlan.hpp create mode 100644 include/actuator.hpp create mode 100644 include/ads.hpp create mode 100644 include/kalmanfilter.hpp create mode 100644 include/logger.hpp create mode 100644 include/math/imumath.h create mode 100644 include/math/matrix.h create mode 100644 include/math/quaternion.h create mode 100644 include/math/vector.h create mode 100644 include/motor.hpp create mode 100644 include/rocketUtils.hpp create mode 100644 include/sensorAltimeter.hpp create mode 100644 include/sensorI2C.hpp create mode 100644 include/sensorIMU.hpp create mode 100644 include/surfaceFitModel.hpp (limited to 'include') diff --git a/include/actuationPlan.hpp b/include/actuationPlan.hpp new file mode 100644 index 0000000..8d1ef26 --- /dev/null +++ b/include/actuationPlan.hpp @@ -0,0 +1,36 @@ +#pragma once +#include +#include +#include "surfaceFitModel.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" + +class ActuationPlan { + + private: + SurfaceFitModel sFitModel; + + public: + + /** + * @brief Construct a new Actuation Plan object + * + */ + ActuationPlan(); + + /** + * @brief Construct a new Actuation Plan object + * + * @param sFitModel + */ + ActuationPlan(SurfaceFitModel sFitModel); + + /** + * @brief Run the Fin Actuation Plan. + * Adjusts the fin angle values depending on the current vehicle state during the launch + * + * @param rocket Provides current rocket status and hold updated fin angle value. + */ + void runPlan(Vehicle& rocket); +}; \ No newline at end of file diff --git a/include/actuator.hpp b/include/actuator.hpp new file mode 100644 index 0000000..e7c4120 --- /dev/null +++ b/include/actuator.hpp @@ -0,0 +1,30 @@ +#pragma once + + +class Actuator { + + private: + + + public: + + /** + * @brief Initialize the actuator. + * + * @param data Data for initializing the actuator + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) = 0; + + /** + * @brief Pass data to the actuator. + * + * @param data Data to sent to the actuator + * + * @return true Actuator write Success + * @return false Actuator write Failure + */ + virtual bool writeData(void* data) = 0; +}; \ No newline at end of file diff --git a/include/ads.hpp b/include/ads.hpp new file mode 100644 index 0000000..b16c4fb --- /dev/null +++ b/include/ads.hpp @@ -0,0 +1,66 @@ +#pragma once +#include +#include +#include +#include +#include +#include "kalmanfilter.hpp" +#include "logger.hpp" +#include "actuationPlan.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" +#include "motor.hpp" +#include "eigen3/Eigen/Dense" + +using namespace Eigen; + +class ADS { + + private: + + KalmanFilter kf; + ActuationPlan plan; + + IMUSensor imu; + AltimeterSensor altimeter; + Motor motor; + Vehicle rocket; + + /** + * @brief Logs a summary of all pertinent current rocket data + * (e.g. Altitude, Velocity, Acceleration) + */ + virtual void logSummary(); + + /** + * @brief Performs a routine to calculate the average altitude + * while the vehicle is waiting on the pad. + */ + virtual void updateOnPadAltitude(); + + /** + * @brief Update the vehicle with the current sensor (IMU & Altimeter) readings + */ + virtual void updateSensorData(); + + /** + * @brief Update the rocket state based on its current telemetry readings. + * Also Log pertinent telemetry and rocket state data + */ + virtual void updateRocketState(); + + public: + + /** + * @brief Construct a new ADS object + * + * @param plan The Actuation Plan for the Rocket + */ + ADS(ActuationPlan plan); + + /** + * @brief Run the full active drag system from launch to landing. + */ + virtual void run(); +}; \ No newline at end of file diff --git a/include/kalmanfilter.hpp b/include/kalmanfilter.hpp new file mode 100644 index 0000000..2c8398c --- /dev/null +++ b/include/kalmanfilter.hpp @@ -0,0 +1,98 @@ +#pragma once +#include "eigen3/Eigen/Dense" +#include +#include + +using namespace Eigen; + +class KalmanFilter { + + private: + + VectorXf state_vector; // x + MatrixXf state_covariance; // P + + MatrixXf state_transition_M; // F + MatrixXf control_input_M; // B + MatrixXf measurement_M; // H + + MatrixXf process_noise_covariance; // Q + MatrixXf measurement_covariance; // R + + MatrixXf I; // Identity + + int n; // State Vector Dimension + int p; // Control Vector Dimension + int m; // Measurement Vector Dimension + + double dt; // timestep + + /** + * @brief Initialize all necessary matrices. + * + */ + void matrixInit(); + + /** + * @brief Update any existing variable elements in your State Transition + * & Control Input matrices. + * + */ + void updateMatrices(); + + /** + * @brief Predict current State Vector & State Covariance + * given the current control input. + * + * @param control_vec The control input to be applied to the + * previous State Vector + */ + void prediction(VectorXf control_vec); + + /** + * @brief Correct the State Vector & State Covariance predictions + * given a related current measurement. + * + * @param measurement Current measurement + */ + void update(VectorXf measurement); + + public: + + KalmanFilter(); + + /** + * @brief Construct a new Kalman Filter object + * Set the sizes of the Filter's user inputs + * + * @param state_dim State Vector Dimension. i.e. dim(x) + * @param control_dim Control/Input Vector Dimension. i.e. dim(u) + * @param measurement_dim Measurement Vector Dimension. i.e. dim(z) + * @param dt timestep + */ + KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt); + + + /** + * @brief Optional function to set a custom initial state for the Filter. + * If not called, State Vector & State Covariance are zero-initialized + * + * @param state_vec Initial State Vector + * @param state_cov Initial State Covariance + * + * @return Whether state initialization was successful + */ + bool setInitialState(VectorXf state_vec, MatrixXf state_cov); + + /** + * @brief Perform Kalman Filter operation with given control input vector + * and measurement. + * + * @param control current control command + * @param measurement current measurement + * @param dt timestep + * + * @return Filtered state vector + */ + VectorXf run(VectorXf control, VectorXf measurement, double _dt); +}; \ No newline at end of file diff --git a/include/logger.hpp b/include/logger.hpp new file mode 100644 index 0000000..90fa05a --- /dev/null +++ b/include/logger.hpp @@ -0,0 +1,83 @@ +#pragma once +#include +#include +#include + +class Logger { + + private: + std::fstream file; + std::string filename; + time_t t; + tm* now; + bool file_open; + + std::string infoTag = "-> [INFO]: "; + std::string errorTag = "-> [ERROR]: "; + std::string months[12] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sept", "Oct", "Nov", "Dec"}; + std::string days[7] = {"Sun", "Mon", "Tues", "Wed", "Thur", "Frid", "Sat"}; + + /** + * @brief Create formatted current-date tag + * + * @return string + */ + std::string getDate(); + + /** + * @brief Create formatted current-time tag + * + * @return string + */ + std::string getTime(); + + + public: + + /** + * @brief + * + */ + static Logger& Get(); + + /** + * @brief Open a Log File for writing + * + * @param _filename Name of file to open + * @return true Successful Open + * @return false Unsuccessful Open + */ + bool openLog(std::string _filename); + + /** + * @brief Close the Log File + * + */ + void closeLog(); + + /** + * @brief Write data to Log file + * + * @param data Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool log(std::string data); + + /** + * @brief Write error data to Log file + * + * @param data Error Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool logErr(std::string data); + + /** + * @brief Print Log Data to Terminal + * + * @return true Successful Print + * @return false Unsuccessful Print + */ + bool printLog(); +}; \ No newline at end of file diff --git a/include/math/imumath.h b/include/math/imumath.h new file mode 100644 index 0000000..831df60 --- /dev/null +++ b/include/math/imumath.h @@ -0,0 +1,30 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_H +#define IMUMATH_H + +#include "matrix.h" +#include "quaternion.h" +#include "vector.h" + +#endif \ No newline at end of file diff --git a/include/math/matrix.h b/include/math/matrix.h new file mode 100644 index 0000000..e71b2db --- /dev/null +++ b/include/math/matrix.h @@ -0,0 +1,185 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_MATRIX_HPP +#define IMUMATH_MATRIX_HPP + +#include +#include + +#include "vector.h" + +namespace imu { + +template class Matrix { +public: + Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); } + + Matrix(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + } + + ~Matrix() {} + + Matrix &operator=(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + return *this; + } + + Vector row_to_vector(int i) const { + Vector ret; + for (int j = 0; j < N; j++) { + ret[j] = cell(i, j); + } + return ret; + } + + Vector col_to_vector(int j) const { + Vector ret; + for (int i = 0; i < N; i++) { + ret[i] = cell(i, j); + } + return ret; + } + + void vector_to_row(const Vector &v, int i) { + for (int j = 0; j < N; j++) { + cell(i, j) = v[j]; + } + } + + void vector_to_col(const Vector &v, int j) { + for (int i = 0; i < N; i++) { + cell(i, j) = v[i]; + } + } + + double operator()(int i, int j) const { return cell(i, j); } + double &operator()(int i, int j) { return cell(i, j); } + + double cell(int i, int j) const { return _cell_data[i * N + j]; } + double &cell(int i, int j) { return _cell_data[i * N + j]; } + + Matrix operator+(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij]; + } + return ret; + } + + Matrix operator-(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij]; + } + return ret; + } + + Matrix operator*(double scalar) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] * scalar; + } + return ret; + } + + Matrix operator*(const Matrix &m) const { + Matrix ret; + for (int i = 0; i < N; i++) { + Vector row = row_to_vector(i); + for (int j = 0; j < N; j++) { + ret(i, j) = row.dot(m.col_to_vector(j)); + } + } + return ret; + } + + Matrix transpose() const { + Matrix ret; + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(j, i) = cell(i, j); + } + } + return ret; + } + + Matrix minor_matrix(int row, int col) const { + Matrix ret; + for (int i = 0, im = 0; i < N; i++) { + if (i == row) + continue; + + for (int j = 0, jm = 0; j < N; j++) { + if (j != col) { + ret(im, jm++) = cell(i, j); + } + } + im++; + } + return ret; + } + + double determinant() const { + // specialization for N == 1 given below this class + double det = 0.0, sign = 1.0; + for (int i = 0; i < N; ++i, sign = -sign) + det += sign * cell(0, i) * minor_matrix(0, i).determinant(); + return det; + } + + Matrix invert() const { + Matrix ret; + double det = determinant(); + + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(i, j) = minor_matrix(j, i).determinant() / det; + if ((i + j) % 2 == 1) + ret(i, j) = -ret(i, j); + } + } + return ret; + } + + double trace() const { + double tr = 0.0; + for (int i = 0; i < N; ++i) + tr += cell(i, i); + return tr; + } + +private: + double _cell_data[N * N]; +}; + +template <> inline double Matrix<1>::determinant() const { return cell(0, 0); } + +}; // namespace imu + +#endif \ No newline at end of file diff --git a/include/math/quaternion.h b/include/math/quaternion.h new file mode 100644 index 0000000..c5b907a --- /dev/null +++ b/include/math/quaternion.h @@ -0,0 +1,214 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_QUATERNION_HPP +#define IMUMATH_QUATERNION_HPP + +#include +#include +#include +#include + +#include "matrix.h" + +namespace imu { + +class Quaternion { +public: + Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} + + Quaternion(double w, double x, double y, double z) + : _w(w), _x(x), _y(y), _z(z) {} + + Quaternion(double w, Vector<3> vec) + : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} + + double &w() { return _w; } + double &x() { return _x; } + double &y() { return _y; } + double &z() { return _z; } + + double w() const { return _w; } + double x() const { return _x; } + double y() const { return _y; } + double z() const { return _z; } + + double magnitude() const { + return sqrt(_w * _w + _x * _x + _y * _y + _z * _z); + } + + void normalize() { + double mag = magnitude(); + *this = this->scale(1 / mag); + } + + Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); } + + void fromAxisAngle(const Vector<3> &axis, double theta) { + _w = cos(theta / 2); + // only need to calculate sine of half theta once + double sht = sin(theta / 2); + _x = axis.x() * sht; + _y = axis.y() * sht; + _z = axis.z() * sht; + } + + void fromMatrix(const Matrix<3> &m) { + double tr = m.trace(); + + double S; + if (tr > 0) { + S = sqrt(tr + 1.0) * 2; + _w = 0.25 * S; + _x = (m(2, 1) - m(1, 2)) / S; + _y = (m(0, 2) - m(2, 0)) / S; + _z = (m(1, 0) - m(0, 1)) / S; + } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { + S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; + _w = (m(2, 1) - m(1, 2)) / S; + _x = 0.25 * S; + _y = (m(0, 1) + m(1, 0)) / S; + _z = (m(0, 2) + m(2, 0)) / S; + } else if (m(1, 1) > m(2, 2)) { + S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; + _w = (m(0, 2) - m(2, 0)) / S; + _x = (m(0, 1) + m(1, 0)) / S; + _y = 0.25 * S; + _z = (m(1, 2) + m(2, 1)) / S; + } else { + S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; + _w = (m(1, 0) - m(0, 1)) / S; + _x = (m(0, 2) + m(2, 0)) / S; + _y = (m(1, 2) + m(2, 1)) / S; + _z = 0.25 * S; + } + } + + void toAxisAngle(Vector<3> &axis, double &angle) const { + double sqw = sqrt(1 - _w * _w); + if (sqw == 0) // it's a singularity and divide by zero, avoid + return; + + angle = 2 * acos(_w); + axis.x() = _x / sqw; + axis.y() = _y / sqw; + axis.z() = _z / sqw; + } + + Matrix<3> toMatrix() const { + Matrix<3> ret; + ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z; + ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z; + ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y; + + ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z; + ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z; + ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x; + + ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y; + ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x; + ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y; + return ret; + } + + // Returns euler angles that represent the quaternion. Angles are + // returned in rotation order and right-handed about the specified + // axes: + // + // v[0] is applied 1st about z (ie, roll) + // v[1] is applied 2nd about y (ie, pitch) + // v[2] is applied 3rd about x (ie, yaw) + // + // Note that this means result.x() is not a rotation about x; + // similarly for result.z(). + // + Vector<3> toEuler() const { + Vector<3> ret; + double sqw = _w * _w; + double sqx = _x * _x; + double sqy = _y * _y; + double sqz = _z * _z; + + ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)); + ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)); + ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)); + + return ret; + } + + Vector<3> toAngularVelocity(double dt) const { + Vector<3> ret; + Quaternion one(1.0, 0.0, 0.0, 0.0); + Quaternion delta = one - *this; + Quaternion r = (delta / dt); + r = r * 2; + r = r * one; + + ret.x() = r.x(); + ret.y() = r.y(); + ret.z() = r.z(); + return ret; + } + + Vector<3> rotateVector(const Vector<2> &v) const { + return rotateVector(Vector<3>(v.x(), v.y())); + } + + Vector<3> rotateVector(const Vector<3> &v) const { + Vector<3> qv(_x, _y, _z); + Vector<3> t = qv.cross(v) * 2.0; + return v + t * _w + qv.cross(t); + } + + Quaternion operator*(const Quaternion &q) const { + return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z, + _w * q._x + _x * q._w + _y * q._z - _z * q._y, + _w * q._y - _x * q._z + _y * q._w + _z * q._x, + _w * q._z + _x * q._y - _y * q._x + _z * q._w); + } + + Quaternion operator+(const Quaternion &q) const { + return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); + } + + Quaternion operator-(const Quaternion &q) const { + return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); + } + + Quaternion operator/(double scalar) const { + return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); + } + + Quaternion operator*(double scalar) const { return scale(scalar); } + + Quaternion scale(double scalar) const { + return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); + } + +private: + double _w, _x, _y, _z; +}; + +} // namespace imu + +#endif \ No newline at end of file diff --git a/include/math/vector.h b/include/math/vector.h new file mode 100644 index 0000000..10345c3 --- /dev/null +++ b/include/math/vector.h @@ -0,0 +1,184 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. + +#ifndef IMUMATH_VECTOR_HPP +#define IMUMATH_VECTOR_HPP + +#include +#include +#include + +namespace imu { + +template class Vector { +public: + Vector() { memset(p_vec, 0, sizeof(double) * N); } + + Vector(double a) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + } + + Vector(double a, double b) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + } + + Vector(double a, double b, double c) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + } + + Vector(double a, double b, double c, double d) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + p_vec[3] = d; + } + + Vector(const Vector &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + } + + ~Vector() {} + + uint8_t n() { return N; } + + double magnitude() const { + double res = 0; + for (int i = 0; i < N; i++) + res += p_vec[i] * p_vec[i]; + + return sqrt(res); + } + + void normalize() { + double mag = magnitude(); + if (isnan(mag) || mag == 0.0) + return; + + for (int i = 0; i < N; i++) + p_vec[i] /= mag; + } + + double dot(const Vector &v) const { + double ret = 0; + for (int i = 0; i < N; i++) + ret += p_vec[i] * v.p_vec[i]; + + return ret; + } + + // The cross product is only valid for vectors with 3 dimensions, + // with the exception of higher dimensional stuff that is beyond + // the intended scope of this library. + // Only a definition for N==3 is given below this class, using + // cross() with another value for N will result in a link error. + Vector cross(const Vector &v) const; + + Vector scale(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] * scalar; + return ret; + } + + Vector invert() const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = -p_vec[i]; + return ret; + } + + Vector &operator=(const Vector &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + return *this; + } + + double &operator[](int n) { return p_vec[n]; } + + double operator[](int n) const { return p_vec[n]; } + + double &operator()(int n) { return p_vec[n]; } + + double operator()(int n) const { return p_vec[n]; } + + Vector operator+(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] + v.p_vec[i]; + return ret; + } + + Vector operator-(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] - v.p_vec[i]; + return ret; + } + + Vector operator*(double scalar) const { return scale(scalar); } + + Vector operator/(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] / scalar; + return ret; + } + + void toDegrees() { + for (int i = 0; i < N; i++) + p_vec[i] *= 57.2957795131; // 180/pi + } + + void toRadians() { + for (int i = 0; i < N; i++) + p_vec[i] *= 0.01745329251; // pi/180 + } + + double &x() { return p_vec[0]; } + double &y() { return p_vec[1]; } + double &z() { return p_vec[2]; } + double x() const { return p_vec[0]; } + double y() const { return p_vec[1]; } + double z() const { return p_vec[2]; } + +private: + double p_vec[N]; +}; + +template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { + return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], + p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], + p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); +} + +} // namespace imu + +#endif \ No newline at end of file diff --git a/include/motor.hpp b/include/motor.hpp new file mode 100644 index 0000000..cb95b6a --- /dev/null +++ b/include/motor.hpp @@ -0,0 +1,36 @@ +#pragma once +#include "actuator.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" + +class Motor : public Actuator { + + private: + + + public: + + /** + * @brief Construct a new Motor object + * + */ + Motor(); + + /** + * @brief Initialize the motor. + * + * @param data Data for initializing the motor + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) override; + + /** + * @brief Write data to the motor. + * + * @param data Data to be writen to the motor + * @return true Motor write Success + * @return false Motor write Failure + */ + virtual bool writeData(void* data) override; +}; \ No newline at end of file diff --git a/include/rocketUtils.hpp b/include/rocketUtils.hpp new file mode 100644 index 0000000..65b9cea --- /dev/null +++ b/include/rocketUtils.hpp @@ -0,0 +1,121 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +// Deployment angle limits +#define MAX_ANGLE 100 +#define MIN_ANGLE 120 + +// Altimeter initialization count limit +#define COUNT_LIMIT 50 + +// Constants +#define G_0 9.8066 + +// Threshold limits +#define BOOST_ACCEL_THRESH 3 +#define BOOST_HEIGHT_THRESH 20 +#define GLIDE_ACCEL_THRESH 0.5 + +#define ALTI_DEPL_THRESHOLD 0.5 +#define RATE_ALTI_DEPL 1/50 +#define FSM_DONE_SURFACE_ALTITUDE 200 +#define APOGEE_FSM_CHANGE 3 + +#define INIT_DEPLOYMENT 0 + +#define TIME_BO 8 +#define TIME_APO 25 +#define TIME_END 120 + +#define PAD_PRESSURE 102250 + +#define DUTY_MAX 14.5 +#define DUTY_MIN 3 + +#define LAUNCH_DATE "4-15-2023" +#define LOG_FILENAME "DataLog_" LAUNCH_DATE ".txt" + +#define LED_GAP_TIME 0.5 +#define LED_ONE_PATH "/sys/class/leds/beaglebone:green:usr1" +#define LED_BRIGHTNESS_FILE "brightness" +#define LED_FILENAME LED_ONE_PATH LED_BRIGHTNESS_FILE + +#define TEST_MODE false + + +enum VehicleState {ON_PAD, BOOST, GLIDE, APOGEE, DONE}; +extern std::string state_for_log[5]; + +struct Vehicle { + + int status; + + std::vector acceleration; + std::vector linear_acceleration; + + double apogee_altitude; + double previous_altitude; + double current_altitude; + double filtered_altitude; + + double filtered_velocity; + + double deployment_angle; + + bool imuInitFail; + bool imuReadFail; + bool altiInitFail; + bool altiReadFail; + + double ON_PAD_altitude; + bool ON_PAD_fail; + + double duty_span; + + double dt; + + int led_brightness; + + time_t start_time; + time_t fail_time; // For failure termination + time_t liftoff_time; + time_t relog_time; + time_t deploy_time; // NOT INITIALIZED YET + time_t loop_time; + time_t led_time; +}; + +/** + * @brief Convert fin deployment percentage to fin rotation angle + * + * @param percentage Fin deployment percentage + * @return double + */ +double deploy_percentage_to_angle(double percentage); + +/** + * @brief Set the decimal precision of the given data, and return it + * as a formatted string with a prefix containing a relevant description of the data. + * + * @param prefix Identifying or clarifying information about the loggef data + * @param data Data to Log + * @param precision The decimal precision value for the data + * + * @return A string with the formatted data. + */ +std::string format_data(std::string prefix, double data, int precision); + +/** + * @brief Blink Beaglebone LED 1 + * + * @param vehicle Holds settings pertinent to the Beaglebone LED + * @return true Successful Blink + * @return false Unsuccessful Blink + */ +bool led_out(Vehicle *vehicle); diff --git a/include/sensorAltimeter.hpp b/include/sensorAltimeter.hpp new file mode 100644 index 0000000..aa3a3cd --- /dev/null +++ b/include/sensorAltimeter.hpp @@ -0,0 +1,165 @@ +#pragma once +#include "sensorI2C.hpp" +#include + +/* +Header file for driver for MPL3115a2 breakout, based heavily on https://github.com/adafruit/Adafruit_MPL3115A2_Library/tree/master +Based on register declarations found https://cdn-shop.adafruit.com/datasheets/1893_datasheet.pdf +Designed using subclass for I2C handler for Beaglebone Black +*/ + +class AltimeterSensor : public sensorI2C { + + + private: + double internalTemperature = 0; + double internalAltitude = 0; + + //ENUMS COPIED DIRECTLY FROM ADAFRUIT IMPLEMENTATION + /** MPL3115A2 registers **/ + enum { + MPL3115A2_REGISTER_STATUS = (0x00), + + MPL3115A2_REGISTER_PRESSURE_MSB = (0x01), + MPL3115A2_REGISTER_PRESSURE_CSB = (0x02), + MPL3115A2_REGISTER_PRESSURE_LSB = (0x03), + + MPL3115A2_REGISTER_TEMP_MSB = (0x04), + MPL3115A2_REGISTER_TEMP_LSB = (0x05), + + MPL3115A2_REGISTER_DR_STATUS = (0x06), + + MPL3115A2_OUT_P_DELTA_MSB = (0x07), + MPL3115A2_OUT_P_DELTA_CSB = (0x08), + MPL3115A2_OUT_P_DELTA_LSB = (0x09), + + MPL3115A2_OUT_T_DELTA_MSB = (0x0A), + MPL3115A2_OUT_T_DELTA_LSB = (0x0B), + + MPL3115A2_WHOAMI = (0x0C), + //This is hard-coded in the device from the factory + MPL3115A2_WHOAMI_EXPECTED = (0xC4), + + MPL3115A2_BAR_IN_MSB = (0x14), + MPL3115A2_BAR_IN_LSB = (0x15), + + MPL3115A2_OFF_H = (0x2D), + }; + + /** MPL3115A2 status register BITS **/ + enum { + MPL3115A2_REGISTER_STATUS_TDR = 0x02, + MPL3115A2_REGISTER_STATUS_PDR = 0x04, + MPL3115A2_REGISTER_STATUS_PTDR = 0x08, + }; + + /** MPL3115A2 PT DATA register BITS **/ + enum { + MPL3115A2_PT_DATA_CFG = 0x13, + MPL3115A2_PT_DATA_CFG_TDEFE = 0x01, + MPL3115A2_PT_DATA_CFG_PDEFE = 0x02, + MPL3115A2_PT_DATA_CFG_DREM = 0x04, + }; + + /** MPL3115A2 control registers **/ + enum { + MPL3115A2_CTRL_REG1 = (0x26), + MPL3115A2_CTRL_REG2 = (0x27), + MPL3115A2_CTRL_REG3 = (0x28), + MPL3115A2_CTRL_REG4 = (0x29), + MPL3115A2_CTRL_REG5 = (0x2A), + }; + + /** MPL3115A2 control register BITS **/ + enum { + MPL3115A2_CTRL_REG1_SBYB = 0x01, + MPL3115A2_CTRL_REG1_OST = 0x02, + MPL3115A2_CTRL_REG1_RST = 0x04, + MPL3115A2_CTRL_REG1_RAW = 0x40, + MPL3115A2_CTRL_REG1_ALT = 0x80, + MPL3115A2_CTRL_REG1_BAR = 0x00, + }; + + /** MPL3115A2 oversample values **/ + enum { + MPL3115A2_CTRL_REG1_OS1 = 0x00, + MPL3115A2_CTRL_REG1_OS2 = 0x08, + MPL3115A2_CTRL_REG1_OS4 = 0x10, + MPL3115A2_CTRL_REG1_OS8 = 0x18, + MPL3115A2_CTRL_REG1_OS16 = 0x20, + MPL3115A2_CTRL_REG1_OS32 = 0x28, + MPL3115A2_CTRL_REG1_OS64 = 0x30, + MPL3115A2_CTRL_REG1_OS128 = 0x38, + }; + + /** MPL3115A2 measurement modes **/ + typedef enum { + MPL3115A2_BAROMETER = 0, + MPL3115A2_ALTIMETER, + } mpl3115a2_mode_t; + + /** MPL3115A2 measurement types **/ + typedef enum { + MPL3115A2_PRESSURE, + MPL3115A2_ALTITUDE, + MPL3115A2_TEMPERATURE, + } mpl3115a2_meas_t; + + //This never actually gets used, and I can't find anything in the datasheet about it?? + #define MPL3115A2_REGISTER_STARTCONVERSION (0x12) ///< start conversion + + //Store current operating mode, sent to device during startup procedure + //This is why an enum is used rather than raw #define statements + mpl3115a2_mode_t currentMode; + + //Struct for storing ctrl register contents, copied from adafruit implementation + typedef union { + struct { + uint8_t SBYB : 1; + uint8_t OST : 1; + uint8_t RST : 1; + uint8_t OS : 3; + uint8_t RAW : 1; + uint8_t ALT : 1; + } bit; + uint8_t reg; + } CTRL_REG_1_STRUCT; + //Create instance of this register config to use during device startup and operation + CTRL_REG_1_STRUCT ctrl_reg1; + + public: + + /** + * @brief Construct a new Altimeter Sensor object + * + */ + AltimeterSensor(std::string I2C_FILE); + + /** + * @brief Initialize the Altimeter + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data getters and setters + // double getPressure(); + double getAltitude(); + double getTemperature(); + double setSeaLevelPressure(double pressure); + + //Data and mode handlers + //Use altimeter mode by default as this is what rocket logger wants + void setMode(mpl3115a2_mode_t mode = MPL3115A2_ALTIMETER); + void requestOneShotReading(); + bool isNewDataAvailable(); + void updateCurrentDataBuffer(); + + +}; + + + + diff --git a/include/sensorI2C.hpp b/include/sensorI2C.hpp new file mode 100644 index 0000000..bd497a5 --- /dev/null +++ b/include/sensorI2C.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class sensorI2C { + + + //Predominantly used for I2C handler functions; implement high-level functions in sensor classes + //Implemented as a combination of Jazz' implementation and Derek Malloy's code + public: + + //Initial single byte write, used at beginning of read operation + //Returns 0 if successful, -1 if not + int initialWrite(unsigned char registerAddress) { + unsigned char *convertedAddressBuffer = new unsigned char[1]; + convertedAddressBuffer[0] = registerAddress; + //Expect 1 byte response from 1 byte write + if (write(i2c_bus, convertedAddressBuffer, 1) != 1) { + fprintf(stderr, "ERROR DOING INITIAL READ TRANSACTION WRITE TO REGISTER %x FOR DEVICE %x\n", registerAddress, deviceAddress); + return 0; + } + return 1; + } + + int writeRegister(unsigned char registerAddress, unsigned char value) { + //initialWrite() not used here because it's easier to just pack it into one buffer for file writing + unsigned char writeBuffer[2]; + writeBuffer[0] = registerAddress; + writeBuffer[1] = value; + + //Expect 2 byte output + if (write(i2c_bus, writeBuffer, 2) != 2) { + //These error messages are kind of obtuse but I'd rather have too much information than not enough + fprintf(stderr, "ERROR WRITING %x TO REGISTER %x ON DEVICE %x\n", value, registerAddress, deviceAddress); + return -1; + } + return 0; + } + + //Could probably be uint8_t but Derek Malloy does it with unsigned chars and that's what worked during testing so I don't want to touch it + unsigned char readSingleRegister(unsigned char registerAddress) { + printf("reg addr: %X\n", registerAddress); + + initialWrite(registerAddress); + unsigned char* readBuffer = new unsigned char[1]; + if (read(i2c_bus, readBuffer, 1) != 1){ + fprintf(stderr, "FAILED TO READ VALUE FROM REGISTER %x ON DEVICE %x\n", registerAddress, deviceAddress); + return -1; + } + printf("readBuffer: %X\n", readBuffer[0]); + return readBuffer[0]; + } + + unsigned char* readMultipleRegisters(unsigned char startingRegisterAddress, int numberOfRegisters) { + initialWrite(startingRegisterAddress); + unsigned char* readBuffer = new unsigned char[numberOfRegisters]; + if (read(i2c_bus, readBuffer, numberOfRegisters) != numberOfRegisters) { + fprintf(stderr, "ERROR TRYING TO READ %d REGISTERS STARTING AT ADDRESS %x ON DEVICE %x\n", + numberOfRegisters, startingRegisterAddress, deviceAddress); + } + return readBuffer; + } + + + //Intakes device address and file + //Private because IT'S ASSUMED PROGRAMMER WILL CALL THIS METHOD DURING INIT() ROUTINE + int setupI2C(std::string I2C_FILE) { + // Open i2c driver file + i2c_bus = open("/dev/i2c-2", O_RDWR); + if(i2c_bus < 0){ + fprintf(stderr, "FAILED TO OPEN I2C BUS USING FILE %s\n", "/dev/i2c-2"); + close(i2c_bus); + return -1; + } + + // Identify slave device address (MODIFIED FROM INITIAL IMPLEMENTATION, USES INTERNAL deviceAddress INSTEAD OF PARAMETER) + if(ioctl(i2c_bus, I2C_SLAVE, deviceAddress) < 0){ + fprintf(stderr, "FAILED TO CONNECT TO DEVICE AT ADDRESS %x VIA I2C\n", deviceAddress); + close(i2c_bus); + return -1; + } + return 0; + } + + /** + * @brief Initialize the sensor. + * + * @param data Data for initializing the sensor + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init() = 0; + + unsigned char deviceAddress; + int i2c_bus; + std::string I2C_FILE; +}; diff --git a/include/sensorIMU.hpp b/include/sensorIMU.hpp new file mode 100644 index 0000000..caddbe4 --- /dev/null +++ b/include/sensorIMU.hpp @@ -0,0 +1,312 @@ +#pragma once +#include +#include "sensorI2C.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" +#include "imumath.h" +#include + +struct IMUData { + std::vector acceleration[3]; + std::vector linear_acceleration[3]; +}; + +//Register addresses and data structs copied from Adafruit implementation +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} adafruit_bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} adafruit_bno055_opmode_t; +class IMUSensor : public sensorI2C { + + private: + deviceAddress = BNO055_ADDRESS_A; + + adafruit_bno055_opmode_t default_mode = OPERATION_MODE_NDOF; + /** BNO055 Registers **/ + typedef enum { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR = 0X07, + + /* PAGE0 REGISTER DEFINITION START*/ + BNO055_CHIP_ID_ADDR = 0x00, + BNO055_ACCEL_REV_ID_ADDR = 0x01, + BNO055_MAG_REV_ID_ADDR = 0x02, + BNO055_GYRO_REV_ID_ADDR = 0x03, + BNO055_SW_REV_ID_LSB_ADDR = 0x04, + BNO055_SW_REV_ID_MSB_ADDR = 0x05, + BNO055_BL_REV_ID_ADDR = 0X06, + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, + BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, + BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, + BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, + BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, + BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, + BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, + BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, + BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, + BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, + BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, + BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, + BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, + BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, + BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, + BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR = 0X1A, + BNO055_EULER_H_MSB_ADDR = 0X1B, + BNO055_EULER_R_LSB_ADDR = 0X1C, + BNO055_EULER_R_MSB_ADDR = 0X1D, + BNO055_EULER_P_LSB_ADDR = 0X1E, + BNO055_EULER_P_MSB_ADDR = 0X1F, + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, + BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, + BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, + BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, + BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, + BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, + BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, + BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, + BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, + BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, + BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, + BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, + BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, + BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, + BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, + BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, + BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, + BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, + + /* Temperature data register */ + BNO055_TEMP_ADDR = 0X34, + + /* Status registers */ + BNO055_CALIB_STAT_ADDR = 0X35, + BNO055_SELFTEST_RESULT_ADDR = 0X36, + BNO055_INTR_STAT_ADDR = 0X37, + + BNO055_SYS_CLK_STAT_ADDR = 0X38, + BNO055_SYS_STAT_ADDR = 0X39, + BNO055_SYS_ERR_ADDR = 0X3A, + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR = 0X3B, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A + } adafruit_bno055_reg_t; + + /** BNO055 power settings */ + typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 + } adafruit_bno055_powermode_t; + + /** Remap settings **/ + typedef enum { + REMAP_CONFIG_P0 = 0x21, + REMAP_CONFIG_P1 = 0x24, // default + REMAP_CONFIG_P2 = 0x24, + REMAP_CONFIG_P3 = 0x21, + REMAP_CONFIG_P4 = 0x24, + REMAP_CONFIG_P5 = 0x21, + REMAP_CONFIG_P6 = 0x21, + REMAP_CONFIG_P7 = 0x24 + } adafruit_bno055_axis_remap_config_t; + + /** Remap Signs **/ + typedef enum { + REMAP_SIGN_P0 = 0x04, + REMAP_SIGN_P1 = 0x00, // default + REMAP_SIGN_P2 = 0x06, + REMAP_SIGN_P3 = 0x02, + REMAP_SIGN_P4 = 0x03, + REMAP_SIGN_P5 = 0x01, + REMAP_SIGN_P6 = 0x07, + REMAP_SIGN_P7 = 0x05 + } adafruit_bno055_axis_remap_sign_t; + + /** A structure to represent revisions **/ + typedef struct { + uint8_t accel_rev; /**< acceleration rev */ + uint8_t mag_rev; /**< magnetometer rev */ + uint8_t gyro_rev; /**< gyroscrope rev */ + uint16_t sw_rev; /**< SW rev */ + uint8_t bl_rev; /**< bootloader rev */ + } adafruit_bno055_rev_info_t; + + /** Vector Mappings **/ + typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR + } adafruit_vector_type_t; + + int32_t _sensorID; + adafruit_bno055_opmode_t currentMode; + + public: + /** + * @brief Construct a new IMUSensor object + * + */ + IMUSensor(std::string I2C_FILE); + + /** + * @brief Inititlize the IMU + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data handlers, copied from adafruit implementation (not all of them) + void setModeTemp(adafruit_bno055_opmode_t mode); + void setModeHard(adafruit_bno055_opmode_t mode); + adafruit_bno055_opmode_t getMode(); + + imu::Vector<3> getVector(adafruit_vector_type_t vector_type); + imu::Quaternion getQuat(); + int8_t getTemp(); + + //Configuration and status getters/setters + void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode); + void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign); + void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, + uint8_t *system_error); + void getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, + uint8_t *mag); + + /* Functions to deal with raw calibration data */ + bool getSensorOffsets(uint8_t *calibData); + bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type); + void setSensorOffsets(const uint8_t *calibData); + void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); + bool isFullyCalibrated(); + + /* Power managments functions */ + void enterSuspendMode(); + void enterNormalMode(); + +}; \ No newline at end of file diff --git a/include/surfaceFitModel.hpp b/include/surfaceFitModel.hpp new file mode 100644 index 0000000..303633f --- /dev/null +++ b/include/surfaceFitModel.hpp @@ -0,0 +1,32 @@ +#pragma once +#include +#include "eigen3/Eigen/Dense" + + +using namespace Eigen; + +#define X_DEGREE 4 // Highest x-degree of current Surface Fit Model +#define Y_DEGREE 3 // Highest y-degree of current Surface Fit Model + +class SurfaceFitModel { + + private: + MatrixXd p; // Polynomial values + + public: + + /** + * @brief Construct a new Surface Fit Model object + * + */ + SurfaceFitModel(); + + /** + * @brief + * + * @param x + * @param y + * @return double + */ + double getFit(double x, double y); +}; -- cgit v1.2.3