Worked on acceleration calculation, Moved all unit conversion constants to Core - Units, and split encoder into inc/src instead of lib

Tried to put units in commonconfig but that messes up the import order.

Core: Moved Units to it's own header to avoid import order issues

Author: BobSaidHi

lib/Encoder/Encoder.hpp include/Encoder.hpplib/Encoder/Encoder.hpp renamed to include/Encoder.hpp

@@ -12,8 +12,8 @@
 #include <etl/to_string.h>
 
 // Custom Includes
-#include "../../include/NacelleConfig.hpp"
 #include "AS5600.h"
+#include "NacelleConfig.hpp"
 
 /**
  * @namespace Encoder

include/NacelleConfig.hpp

@@ -6,7 +6,9 @@
 static_assert(__cplusplus >= 202302L, "C++23 standard or later required.");
 
 // Imports
+#include "2026Core/Units.hpp"
 #include <cstdint>
+
 #define ESP32C5 1
 #define BOARD ESP32C5
 #if not(BOARD == ESP32C5)
@@ -68,13 +70,6 @@ namespace FR_FIREBEETLE2_ESP32C6 {
     constexpr uint_fast8_t LPUART_RX_PIN = LP_RX;
 } // namespace FR_FIREBEETLE2_ESP32C6
 
-// MARK: Constants
-namespace CONSTS {
-    constexpr uint_fast32_t MILLIS_PER_SEC = 1000;
-    constexpr uint_fast32_t SECS_PER_MIN = 60;
-    constexpr uint_fast16_t uVOLTS_PER_VOLT = 1'000'000;
-} // namespace CONSTS
-
 // Mark: Application
 namespace LED {
     /**
@@ -85,14 +80,14 @@ namespace LED {
     // constexpr uint_fast32_t BLINK_OFF_SECS = 3;
     constexpr uint_fast32_t BLINK_OFF_MILLIS = 3500;
     // constexpr uint_fast32_t LED_BLINK_ON_SEC = 1;
-    constexpr uint_fast32_t BLINK_ON_MILLIS = CONSTS::MILLIS_PER_SEC / 3;
+    constexpr uint_fast32_t BLINK_ON_MILLIS = UNITS::MILLIS_PER_SEC / 3;
 } // namespace LED
 
 namespace ENCODER {
     constexpr uint_fast16_t START_RUN_2_RPM = 200; // CONFIG
     constexpr uint_fast16_t TARGET_RPM = 2200;     // CONFIG
     constexpr uint_fast16_t MAX_RPM = 3000;        // CONFIG
-    constexpr uint_fast8_t MAX_RPS = MAX_RPM / CONSTS::SECS_PER_MIN;
+    constexpr uint_fast8_t MAX_RPS = MAX_RPM / UNITS::SECS_PER_MIN;
     constexpr uint_fast16_t MIN_T_mS_PER_REV = 1000 / MAX_RPS;
     constexpr uint_fast32_t OPTIMAL_SAMPLE_TIME_mS = MIN_T_mS_PER_REV / 4;
     // static_assert(SAMPLE_DELAY_MS == OPTIMAL_SAMPLE_TIME_mS,
@@ -125,7 +120,7 @@ namespace ENCODER {
     constexpr uint_fast16_t MAX_RATED_RPM = 3500;           // CONFIG
     constexpr uint_fast16_t MOTOR_kV_RPMPV = 107;
     constexpr uint_fast16_t MOTOR_IkV_RPSPuV =
-        CONSTS::uVOLTS_PER_VOLT * CONSTS::SECS_PER_MIN / MOTOR_kV_RPMPV;
+        UNITS::uVOLTS_PER_VOLT * UNITS::SECS_PER_MIN / MOTOR_kV_RPMPV;
 } // namespace ENCODER
 
 namespace PITCHING {
@@ -188,8 +183,8 @@ namespace RUN {
     // todo: What was this for?
     // constexpr uint_fast32_t SLEEP_TIME_MINS = 10;
     // constexpr uint_fast32_t SLEEP_TIME_SECS = SLEEP_TIME_MINS *
-    // CONSTS::SECS_PER_MIN; constexpr uint_fast32_t SLEEP_TIME_MILLIS =
-    //     SLEEP_TIME_SECS * CONSTS::MILLIS_PER_SEC;
+    // UNITS::SECS_PER_MIN; constexpr uint_fast32_t SLEEP_TIME_MILLIS =
+    //     SLEEP_TIME_SECS * UNITS::MILLIS_PER_SEC;
 } // namespace RUN
 
 namespace TELNET {

lib/2026-Controls_Core

@@ -122,9 +122,10 @@ namespace Encoder {
         (void)logString.append(", AGC: ");
 
         etl::format_spec decFormatB;
-        (void)decFormatB.width(3).fill('0');                       // [3 chars]
-        etl::to_string(as5600.readAGC(), logString, decFormatB, true); // 3 chars
-        (void)logString.append(", MD: ");                          // 6 chars
+        (void)decFormatB.width(3).fill('0'); // [3 chars]
+        etl::to_string(as5600.readAGC(), logString, decFormatB,
+                       true);             // 3 chars
+        (void)logString.append(", MD: "); // 6 chars
 
         boolFormatA.binary().width(1).fill('0'); // [1 char]
         etl::to_string(as5600.detectMagnet(), logString, decFormatB,

lib/Encoder/Encoder.cpp src/Encoder.cpplib/Encoder/Encoder.cpp renamed to src/Encoder.cpp

@@ -12,6 +12,7 @@
 
 // Project Includes
 #include "2026Core/CommonConfig.hpp" // Include after NacelleConfig due to macro precednece
+#include "2026Core/Units.hpp"
 #include "ActuonixL12.hpp"
 #include "Encoder.hpp"
 #include "NacelleComms.hpp"
@@ -310,37 +311,57 @@ vTaskUpdateFSM([[maybe_unused]] void *pvParameters) { // NOSONAR
 [[noreturn]] void
 vTaskPollSensors([[maybe_unused]] void *pvParameters) { // NOSONAR
     while (true) {
+        // Save last wake time to ensure consistent timing
         static TickType_t xLastWakeTime = xTaskGetTickCount();
-        static unsigned long previousTime = 0;
-        static etl::circular_buffer<int_fast32_t, 20> accelSamples;
-        static int_fast32_t accelSum = 0;
-        unsigned long currentTime = micros();
 
         // Poll encoder & update stored value
-        // todo: back off and retry if not working
-        int_fast32_t lastRPM = nacelle.currentRPM;        // Save
-        Encoder::getRpmMovingAverage(nacelle.currentRPM); // Update
-        int_fast32_t deltaRPM = nacelle.currentRPM - lastRPM;
-        // ESP_LOGD(TAG, "deltaRPM: %ld", deltaRPM);
-        int_fast32_t deltaTime_us = currentTime - previousTime;
-        constexpr unsigned long m_TO_BASE = 1000;
-        constexpr unsigned long u_TO_m = 1000;
-        constexpr unsigned long u_TO_BASE = m_TO_BASE * u_TO_m;
-        int_fast32_t accelSample_RPMPS = static_cast<int_fast32_t>(deltaRPM) *
-                                         static_cast<int_fast32_t>(u_TO_BASE) /
-                                         deltaTime_us;
+        // TODO: back off and retry if not working
+        Encoder::getRpmMovingAverage(nacelle.currentRPM); // Update RPM
+
+        // Reduce the update speed of acceleration to reduce noise
+        static uint_fast16_t RPMSamplesTaken = 0;
+
+        // Wait until the filter history has been refreshed
+        if (RPMSamplesTaken >= ENCODER::FILTER_HISTORY_SIZE) {
+            static unsigned long previousTime = 0;
+            // The first deltaRPM will be 0 instead of (current - 0)
+            static int_fast32_t lastRPM = nacelle.currentRPM;
+
+            // Calculate deltas
+            unsigned long currentTime = micros();
+            int_fast32_t deltaRPM = nacelle.currentRPM - lastRPM; // dY
+            // ESP_LOGD(TAG, "deltaRPM: %ld", deltaRPM);
+            int_fast32_t deltaTime_us = currentTime - previousTime; // dT
+
+            // Calculate acceleration and perform unit conversion
+            // (RPM / t us) * (10^6 us / 1 s) = (RPM / 1 s)
+            int_fast32_t accelSample_RPMPS =
+                deltaRPM * static_cast<int_fast32_t>(UNITS::MICROS_PER_SEC) /
+                deltaTime_us;
+
+            // Filter - moving average
+            static etl::circular_buffer<int_fast32_t,
+                                        ENCODER::FILTER_HISTORY_SIZE>
+                accelSamples;
+            static int_fast32_t accelSum = 0;
+            if (accelSamples.full()) {
+                accelSum -= accelSamples.front();
+                accelSamples.pop();
+            }
+            accelSamples.push(accelSample_RPMPS);
+            accelSum += accelSample_RPMPS;
 
-        if (accelSamples.full()) {
-            accelSum -= accelSamples.front();
-            accelSamples.pop();
-        }
-        accelSamples.push(accelSample_RPMPS);
-        accelSum += accelSample_RPMPS;
+            nacelle.angularAccel_RPMPS =
+                accelSum / static_cast<int_fast32_t>(accelSamples.size());
+
+            // Save current time
+            previousTime = currentTime;
 
-        nacelle.angularAccel_RPMPS = static_cast<int_fast32_t>(
-            accelSum / static_cast<int_fast32_t>(accelSamples.size()));
-        previousTime = currentTime;
+            // Reset counter
+            RPMSamplesTaken = 0;
+        }
 
+        // Run at a fixed frequency
         BaseType_t xWasDelayed = xTaskDelayUntil(
             &xLastWakeTime,
             pdMS_TO_TICKS(RUN::TASK_INTERVALS::TI_POLL_SENSORS_mS));