Add checks for whether the buffer is full and math operations

.vscode/c_cpp_properties.json

@@ -0,0 +1,15 @@
+{
+    "configurations": [
+        {
+            "name": "Mac",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "cStandard": "c17",
+            "cppStandard": "c++14",
+            "intelliSenseMode": "macos-clang-arm64"
+        }
+    ],
+    "version": 4
+}

documentation.md

@@ -219,5 +219,42 @@ Not thread-safe. Designed for single-threaded Arduino contexts. Concurrent acces
 
 Both classes are defined entirely in `src/Vectorial.h`. The library conditionally includes `Arduino.h` for `millis()`. When compiled in non-Arduino environments (e.g., host-based tests), it falls back to C++ `<chrono>` to provide a compatible `millis()` function.
 
+## Math Operations
+
+For math operations, the file `src/VectorialMath.h` can be imported. The math operations include discrete integration and differenciation via the `derive()` and `integrate()` functions which each take a TimeVector as an input and smoothens data via the `moving_mean()` and `ema()` Functions.
+
+### Example: Rocket Flight System during one frame
+
+```cpp
+#include <Vectorial.h>
+#include <VectorialMath.h>
+TimedVector<float, 5> gyroData; // Angular Velocity
+void setup() {
+  Serial.begin(9600);
+  while (!Serial) {}
+  // Actual data can come from reading the sensor, these are just sample data points.
+  gyroData.push_back(1.35);
+  delay(100);
+  gyroData.push_back(1.26);
+  delay(100);
+  gyroData.push_back(1.41);
+  delay(100);
+  gyroData.push_back(1.34);
+  delay(100);
+  gyroData.push_back(1.52);
+  // Simple moving average value of the gyro data based on last 5 values:
+  float mean5 = TimedVectorMath::moving_mean(gyroData, 5);
+  // Based on last 3 values:
+  float mean3 = TimedVectorMath::moving_mean(gyroData, 3);
+  // Exponential moving average:
+  float emaVal = TimedVectorMath::ema(gyroData, 5);
+  // Differentiate for angular acceleration:
+  float angularAccel = TimedVectorMath::derive(gyroData);
+  // Integrate for angular position:
+  float angularPosition = TimedVectorMath::integrate(gyroData, 5);
+}
+void loop() {}
+```
+
 ## License
 See the LICENSE file in the repository root.

examples/VectorialMath/VectorialMath.ino

@@ -0,0 +1,41 @@
+#include <Vectorial.h>
+#include <VectorialMath.h>
+
+TimedVector<float, 5> gyroData; // Angular Velocity
+
+void setup() {
+  Serial.begin(9600);
+  while (!Serial) {}
+  // Actual data can come from reading the sensor, these are just sample data points.
+  gyroData.push_back(1.35);
+  delay(100);
+  gyroData.push_back(1.26);
+  delay(100);
+  gyroData.push_back(1.41);
+  delay(100);
+  gyroData.push_back(1.34);
+  delay(100);
+  gyroData.push_back(1.52);
+  // Simple moving average value of the gyro data based on last 5 values:
+  float mean5 = TimedVectorMath::moving_mean(gyroData, 5);
+  Serial.print("Mean of last 5 is: ");
+  Serial.println(mean5);
+  // Based on last 3 values:
+  float mean3 = TimedVectorMath::moving_mean(gyroData, 3);
+  Serial.print("Mean of last 3 is: ");
+  Serial.println(mean3);
+  // Exponential moving average:
+  float emaVal = TimedVectorMath::ema(gyroData, 5);
+  Serial.print("EMA is: ");
+  Serial.println(emaVal);
+  // Differentiate for angular acceleration:
+  float angularAccel = TimedVectorMath::derive(gyroData);
+  Serial.print("Angular acceleration is: ");
+  Serial.println(angularAccel);
+  // Integrate for angular position:
+  float angularPosition = TimedVectorMath::integrate(gyroData, 5);
+  Serial.print("Angular position is: ");
+  Serial.println(angularPosition);
+}
+
+void loop {}

src/Vectorial.h

@@ -17,24 +17,20 @@ inline uint32_t millis() {
 #endif
 
 enum appendType { cycle, ignore };
-inline uint32_t vec_min(uint32_t a, uint32_t b) {
-  return (a < b) ? a : b;
-}
+inline uint32_t vec_min(uint32_t a, uint32_t b) { return (a < b) ? a : b; }
 
-template <typename T, uint32_t N>
-struct Vector {
-  private:
-      T data[N];
-      uint32_t entriesAdded = 0;
+template <typename T, uint32_t N> struct Vector {
+private:
+  T data[N];
+  uint32_t entriesAdded = 0;
 
-  public:
+public:
   void push_back(T element, appendType type = appendType::cycle) {
     if (type == cycle) {
-        uint32_t latest = entriesAdded % N;
-        data[latest] = element;
-        entriesAdded++;
-    }
-    else if (type == ignore) {
+      uint32_t latest = entriesAdded % N;
+      data[latest] = element;
+      entriesAdded++;
+    } else if (type == ignore) {
       if (entriesAdded < N) {
         data[entriesAdded] = element;
         entriesAdded++;
@@ -43,7 +39,7 @@ struct Vector {
   }
 
   T operator[](uint32_t idx) const {
-    return data[(entriesAdded + 2*N - 1 - idx) % N]; 
+    return data[(entriesAdded + 2 * N - 1 - idx) % N];
   }
 
   struct VectorReturnObject {
@@ -55,89 +51,85 @@ struct Vector {
     VectorReturnObject ret_value;
     if (idx >= entriesAdded) {
       return ret_value;
-    }
-    else {
+    } else {
       ret_value.success = true;
       ret_value.value = (*this)[idx];
       return ret_value;
     }
   }
 
-  uint32_t size() const {
-    return vec_min(entriesAdded, N);
-  }
+  uint32_t size() const { return vec_min(entriesAdded, N); }
 
-  void reset() { 
-    entriesAdded = 0; 
+  bool is_full() const { return size() == N; }
+
+  void reset() {
+    entriesAdded = 0;
     for (uint32_t i = 0; i < N; i++) {
       data[i] = T{};
     }
   }
 };
 
-template <typename T, uint32_t N>
-struct TimedVector {
-  public:
-    struct DataPoint {
-      T value;
-      uint32_t time;
-    };
-
-    void push_back(DataPoint element, appendType type = appendType::cycle) {
-      if (type == cycle) {
-          uint32_t latest = entriesAdded % N;
-          data[latest] = element;
-          entriesAdded++;
-      }
-      else if (type == ignore) {
-        if (entriesAdded < N) {
-          data[entriesAdded] = element;
-          entriesAdded++;
-        }
+template <typename T, uint32_t N> struct TimedVector {
+public:
+  struct DataPoint {
+    T value;
+    uint32_t time;
+  };
+
+  void push_back(DataPoint element, appendType type = appendType::cycle) {
+    if (type == cycle) {
+      uint32_t latest = entriesAdded % N;
+      data[latest] = element;
+      entriesAdded++;
+    } else if (type == ignore) {
+      if (entriesAdded < N) {
+        data[entriesAdded] = element;
+        entriesAdded++;
       }
     }
+  }
 
-    // Default to using builtin millis() arduino function 
-    // if push_back with element only.
-    void push_back(T element, appendType type = appendType::cycle) {
-      push_back({element, (uint32_t)millis()}, type);
-    }
+  // Default to using builtin millis() arduino function
+  // if push_back with element only.
+  void push_back(T element, appendType type = appendType::cycle) {
+    push_back({element, (uint32_t)millis()}, type);
+  }
 
-    DataPoint operator[](uint32_t idx) const {
-      return data[(entriesAdded + 2*N - 1 - idx) % N]; 
-    }
+  DataPoint operator[](uint32_t idx) const {
+    return data[(entriesAdded + 2 * N - 1 - idx) % N];
+  }
 
-    struct VectorReturnObject {
-      bool success = false;
-      DataPoint value;
-    };
+  struct VectorReturnObject {
+    bool success = false;
+    DataPoint value;
+  };
 
-    VectorReturnObject get_value(uint32_t idx) const {
-      VectorReturnObject ret_value;
-      if (idx >= entriesAdded) {
-        return ret_value;
-      }
-      else {
-        ret_value.success = true;
-        ret_value.value = (*this)[idx];
-        return ret_value;
-      }
+  VectorReturnObject get_value(uint32_t idx) const {
+    VectorReturnObject ret_value;
+    if (idx >= entriesAdded) {
+      return ret_value;
+    } else {
+      ret_value.success = true;
+      ret_value.value = (*this)[idx];
+      return ret_value;
     }
+  }
 
-    uint32_t size() const {
-      return vec_min(entriesAdded, N);
-    }
+  uint32_t size() const { return vec_min(entriesAdded, N); }
 
-    void reset() { 
-      entriesAdded = 0; 
-      for (uint32_t i = 0; i < N; i++) {
-        data[i] = {}; // Empty struct
-      }
+  bool is_full() const { return size() == N; }
+
+  void reset() {
+    entriesAdded = 0;
+    for (uint32_t i = 0; i < N; i++) {
+      data[i] = {}; // Empty struct
     }
+  }
 
-  private:
-    DataPoint data[N];
-    uint32_t entriesAdded = 0;
+private:
+  DataPoint data[N];
+  uint32_t entriesAdded = 0;
 };
 
 #endif

src/VectorialMath.h

@@ -0,0 +1,67 @@
+#ifndef VECTORIALMATH_H_
+#define VECTORIALMATH_H_
+
+#include "Vectorial.h"
+
+#include <algorithm>
+#include <cassert>
+#include <cstdint>
+#include <math.h>
+
+struct TimedVectorMath {
+  template <typename T, uint32_t N>
+  static float moving_mean(const TimedVector<T, N> &vec, uint32_t elements) {
+    assert(elements <= N);
+    assert(elements > 0);
+    T sum = 0;
+    for (uint32_t i = 0; i < elements; i++) {
+      sum += vec[i].value;
+    }
+    return static_cast<float>(sum) / elements;
+  }
+  template <typename T, uint32_t N>
+  static float ema(const TimedVector<T, N> &vec, uint32_t elements,
+                   float alpha = 0) {
+    assert(elements <= N);
+    assert(elements > 0);
+    float ema = 0;
+    if (alpha == 0) {
+      alpha = 2.0f / (1 + elements);
+    }
+    for (int64_t i = static_cast<int64_t>(elements) - 1; i >= 0; i--) {
+      if (i == static_cast<int64_t>(elements) - 1) {
+        ema = vec[static_cast<uint32_t>(i)].value; // oldest point in window
+      } else {
+        ema = ema * (1 - alpha) + vec[static_cast<uint32_t>(i)].value * alpha;
+      }
+    }
+    return ema;
+  }
+  template <typename T, uint32_t N>
+  static float derive(const TimedVector<T, N> &vec) {
+    if (vec.size() < 2) {
+      return 0.0f;
+    }
+    return static_cast<float>(vec[0].value - vec[1].value) * 1000 /
+           (vec[0].time - vec[1].time);
+  }
+  template <typename T, uint32_t N>
+  static float integrate(const TimedVector<T, N> &vec, uint32_t elements) {
+    // When the window covers the full buffer, there's no vec[N] to pair with
+    // the oldest point — estimate its contribution standalone instead of
+    // dropping it. Also serves as built-in anti-windup.
+    assert(elements <= N);
+    assert(elements > 0);
+    float integral = 0;
+    if (elements == N) {
+      integral = vec[elements - 1].value * vec[elements - 1].time *
+                 (0.001f); // Estimate of state lost before timed vector.
+    }
+    for (uint32_t i = 0; i < std::min(elements, N - 1); i++) {
+      integral += vec[i].value * (0.001f) * (vec[i].time - vec[i + 1].time);
+    }
+    return integral;
+  }
+};
+
+#endif // VECTORIALMATH_H_