From 1a3be691fbc306d5200ce94f8f42f7639850a6cb Mon Sep 17 00:00:00 2001 From: Aryan Date: Sat, 20 Jun 2026 17:28:52 +0530 Subject: [PATCH 1/3] Add checks for whether the buffer is full. --- src/Vectorial.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Vectorial.h b/src/Vectorial.h index 2b5e557..e29df1b 100644 --- a/src/Vectorial.h +++ b/src/Vectorial.h @@ -67,6 +67,10 @@ struct Vector { return vec_min(entriesAdded, N); } + bool is_full() const { + return size() == N; + } + void reset() { entriesAdded = 0; for (uint32_t i = 0; i < N; i++) { @@ -128,6 +132,10 @@ struct TimedVector { return vec_min(entriesAdded, N); } + bool is_full() const { + return size() == N; + } + void reset() { entriesAdded = 0; for (uint32_t i = 0; i < N; i++) { From 564f7e4ec0184b8cd45a6d13d0550839a592f82d Mon Sep 17 00:00:00 2001 From: Aryan Date: Sat, 20 Jun 2026 19:00:17 +0530 Subject: [PATCH 2/3] Format and add basic math operators --- .vscode/c_cpp_properties.json | 15 ++++ src/Vectorial.h | 144 +++++++++++++++------------------- src/VectorialMath.h | 67 ++++++++++++++++ src/testing/Test.h | 101 +++++++++++++++++++++--- 4 files changed, 235 insertions(+), 92 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 src/VectorialMath.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..3d488c4 --- /dev/null +++ b/.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 +} \ No newline at end of file diff --git a/src/Vectorial.h b/src/Vectorial.h index e29df1b..5b24272 100644 --- a/src/Vectorial.h +++ b/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 -struct Vector { - private: - T data[N]; - uint32_t entriesAdded = 0; +template 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,97 +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); } - bool is_full() const { - return size() == N; - } + bool is_full() const { return size() == N; } - void reset() { - entriesAdded = 0; + void reset() { + entriesAdded = 0; for (uint32_t i = 0; i < N; i++) { data[i] = T{}; } } }; -template -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 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); } - bool is_full() const { - return size() == N; - } + bool is_full() const { return size() == N; } - void reset() { - entriesAdded = 0; - for (uint32_t i = 0; i < N; i++) { - data[i] = {}; // Empty struct - } + 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 diff --git a/src/VectorialMath.h b/src/VectorialMath.h new file mode 100644 index 0000000..f25e73f --- /dev/null +++ b/src/VectorialMath.h @@ -0,0 +1,67 @@ +#ifndef VECTORIALMATH_H_ +#define VECTORIALMATH_H_ + +#include "Vectorial.h" + +#include +#include +#include +#include + +struct TimedVectorMath { + template + static float moving_mean(const TimedVector &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(sum) / elements; + } + template + static float ema(const TimedVector &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(elements) - 1; i >= 0; i--) { + if (i == static_cast(elements) - 1) { + ema = vec[static_cast(i)].value; // oldest point in window + } else { + ema = ema * (1 - alpha) + vec[static_cast(i)].value * alpha; + } + } + return ema; + } + template + static float derive(const TimedVector &vec) { + if (vec.size() < 2) { + return 0.0f; + } + return static_cast(vec[0].value - vec[1].value) * 1000 / + (vec[0].time - vec[1].time); + } + template + static float integrate(const TimedVector &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_ diff --git a/src/testing/Test.h b/src/testing/Test.h index c79ce62..b4dd3c5 100644 --- a/src/testing/Test.h +++ b/src/testing/Test.h @@ -3,11 +3,13 @@ #include #include "../Vectorial.h" +#include "../VectorialMath.h" #ifndef TEST_H #define TEST_H // Helpers +namespace { template bool ExpectEQ(T a, T b) { if (a == b) { std::cout << "PASS! " << a << " is equal to " << b @@ -19,6 +21,11 @@ template bool ExpectEQ(T a, T b) { } } +constexpr float kEpsilon = 1e-4f; +bool ExpectFloatEQ(float expected, float actual, float epsilon = kEpsilon) { + return std::fabs(expected - actual) < epsilon; +} + template bool ExpectNEQ(T a, T b) { if (a != b) { std::cout << "PASS! " << a << " is not equal to " << b @@ -32,6 +39,8 @@ template bool ExpectNEQ(T a, T b) { template void AssertEQ(T a, T b) { assert(a == b); } +} // namespace + bool SimpleVectorTest() { constexpr int kTestCount = 9; bool should_pass = true; @@ -271,7 +280,8 @@ bool TimedVectorAsVectorTest() { } if (should_pass) { - std::cout << "TimedVectorAsVectorTest: All " << kTestCount << " tests passed!\n"; + std::cout << "TimedVectorAsVectorTest: All " << kTestCount + << " tests passed!\n"; } else { std::cout << "TimedVectorAsVectorTest: Some tests failed.\n"; } @@ -288,11 +298,11 @@ bool TimedVectorTimeTest() { std::cout << "[1/" << kTestCount << "] - Time field is populated\n"; { TimedVector buffer; - #ifdef CI +#ifdef CI std::this_thread::sleep_for(std::chrono::milliseconds(2)); - #else +#else delay(2); - #endif +#endif buffer.push_back(100); should_pass = ExpectNEQ(0u, buffer[0].time) && should_pass; } @@ -301,11 +311,11 @@ bool TimedVectorTimeTest() { std::cout << "[2/" << kTestCount << "] - All entries have timestamps\n"; { TimedVector buffer; - #ifdef CI +#ifdef CI std::this_thread::sleep_for(std::chrono::milliseconds(2)); - #else +#else delay(2); - #endif +#endif buffer.push_back(1); buffer.push_back(2); buffer.push_back(3); @@ -327,14 +337,15 @@ bool TimedVectorTimeTest() { } // Test 4: get_value returns DataPoint with time - std::cout << "[4/" << kTestCount << "] - get_value returns timestamped data\n"; + std::cout << "[4/" << kTestCount + << "] - get_value returns timestamped data\n"; { TimedVector buffer; - #ifdef CI +#ifdef CI std::this_thread::sleep_for(std::chrono::milliseconds(2)); - #else +#else delay(2); - #endif +#endif buffer.push_back(999); auto result = buffer.get_value(0); should_pass = ExpectEQ(true, result.success) && should_pass; @@ -343,13 +354,77 @@ bool TimedVectorTimeTest() { } if (should_pass) { - std::cout << "TimedVectorTimeTest: All " << kTestCount << " tests passed!\n"; + std::cout << "TimedVectorTimeTest: All " << kTestCount + << " tests passed!\n"; } else { std::cout << "TimedVectorTimeTest: Some tests failed.\n"; } return should_pass; } +bool VectorialMathTest() { + constexpr int kTestCount = 4; + bool should_pass = true; + std::cout << "[1/" << kTestCount << "] - Moving_Mean Test\n"; + { + TimedVector buffer; + TimedVector::DataPoint points[5] = { + {10, 0}, {20, 1000}, {30, 2000}, {40, 3000}, {50, 4000}}; + for (auto &p : points) { + buffer.push_back(p); + } + // moving_mean over last 3: (50+40+30)/3 = 40.0 + should_pass = + ExpectEQ(40.0f, TimedVectorMath::moving_mean(buffer, 3)) && should_pass; + // moving_mean over all 5: (50+40+30+20+10)/5 = 30.0 + should_pass = + ExpectEQ(30.0f, TimedVectorMath::moving_mean(buffer, 5)) && should_pass; + } + std::cout << "[2/" << kTestCount << "] - ema Test\n"; + { + TimedVector buffer; + TimedVector::DataPoint points[5] = { + {10, 0}, {20, 1000}, {30, 2000}, {40, 3000}, {50, 4000}}; + for (auto &p : points) { + buffer.push_back(p); + } + // ema over last 3: (50+40+30)/3 = 40.0 + should_pass = + ExpectEQ(42.5f, TimedVectorMath::ema(buffer, 3)) && should_pass; + // e,a over all 5: (50+40+30+20+10)/5 = 30.0 + should_pass = + ExpectFloatEQ(33.9506f, TimedVectorMath::ema(buffer, 5)) && should_pass; + } + std::cout << "[3/" << kTestCount << "] - Derive Test\n"; + { + TimedVector buffer; + TimedVector::DataPoint points[5] = { + {10, 0}, {20, 1000}, {30, 2000}, {40, 3000}, {50, 4000}}; + for (auto &p : points) { + buffer.push_back(p); + } + // derive + should_pass = + ExpectEQ(10.0f, TimedVectorMath::derive(buffer)) && should_pass; + } + std::cout << "[4/" << kTestCount << "] - Integrate Test\n"; + { + TimedVector buffer; + TimedVector::DataPoint points[5] = { + {10, 0}, {20, 1000}, {30, 2000}, {40, 3000}, {50, 4000}}; + for (auto &p : points) { + buffer.push_back(p); + } + // integrate over last 3 + should_pass = + ExpectEQ(120.0f, TimedVectorMath::integrate(buffer, 3)) && should_pass; + // integrate over all 5 + should_pass = + ExpectEQ(140.0f, TimedVectorMath::integrate(buffer, 5)) && should_pass; + } + return should_pass; +} + void TestRunner() { std::cout << "============================================================\n"; bool success = SimpleVectorTest(); @@ -358,6 +433,8 @@ void TestRunner() { std::cout << "============================================================\n"; success = TimedVectorTimeTest() && success; std::cout << "============================================================\n"; + success = VectorialMathTest() && success; + std::cout << "============================================================\n"; assert(success); } From 6e59ca8551386c61db802d39c9e1d4ef2cb76dc1 Mon Sep 17 00:00:00 2001 From: Aryan Date: Sat, 20 Jun 2026 19:14:42 +0530 Subject: [PATCH 3/3] Add examples and documentation --- documentation.md | 37 +++++++++++++++++++++ examples/VectorialMath/VectorialMath.ino | 41 ++++++++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 examples/VectorialMath/VectorialMath.ino diff --git a/documentation.md b/documentation.md index df0ed42..532ad2c 100644 --- a/documentation.md +++ b/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++ `` 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 +#include +TimedVector 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. diff --git a/examples/VectorialMath/VectorialMath.ino b/examples/VectorialMath/VectorialMath.ino new file mode 100644 index 0000000..9350c6c --- /dev/null +++ b/examples/VectorialMath/VectorialMath.ino @@ -0,0 +1,41 @@ +#include +#include + +TimedVector 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 {}