Where to begin…. (#13)

+/- Reworked collection of altimeter related functions into altimeter class
+/- Reworked bno055 class to be imu class with minimal functionality

\- Removed external Kalman filter implementations in favor of own in house version
\- Removed any/unused files
\+ Added buffer logger for when sitting on pad for extended period of time in effort to prevent filling of flash chip
\+ Added heartbeat LED for alive status

4 files changed, 0 insertions, 613 deletions

diff --git a/include/math/imumath.h b/include/math/imumath.h
deleted file mode 100644
index 831df60..0000000
--- a/include/math/imumath.h
+++ /dev/null
@@ -1,30 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//
-//  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
deleted file mode 100644
index e71b2db..0000000
--- a/include/math/matrix.h
+++ /dev/null
@@ -1,185 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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 <stdint.h>
-#include <string.h>
-
-#include "vector.h"
-
-namespace imu {
-
-template <uint8_t N> 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<N> row_to_vector(int i) const {
-    Vector<N> ret;
-    for (int j = 0; j < N; j++) {
-      ret[j] = cell(i, j);
-    }
-    return ret;
-  }
-
-  Vector<N> col_to_vector(int j) const {
-    Vector<N> ret;
-    for (int i = 0; i < N; i++) {
-      ret[i] = cell(i, j);
-    }
-    return ret;
-  }
-
-  void vector_to_row(const Vector<N> &v, int i) {
-    for (int j = 0; j < N; j++) {
-      cell(i, j) = v[j];
-    }
-  }
-
-  void vector_to_col(const Vector<N> &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<N> 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<N - 1> minor_matrix(int row, int col) const {
-    Matrix<N - 1> 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
deleted file mode 100644
index c5b907a..0000000
--- a/include/math/quaternion.h
+++ /dev/null
@@ -1,214 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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 <math.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <string.h>
-
-#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
deleted file mode 100644
index 10345c3..0000000
--- a/include/math/vector.h
+++ /dev/null
@@ -1,184 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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 <math.h>
-#include <stdint.h>
-#include <string.h>
-
-namespace imu {
-
-template <uint8_t N> 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<N> &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