compiles. to be tested

hytech-racing · May 13, 2024 · c76391e · c76391e
1 parent 6df7c59
commit c76391e
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h
@@ -49,6 +49,8 @@ class SteeringSystem
      * 1 : secondary sensor filter
      */
     Filter_IIR<float> steeringFilters_[NUM_SENSORS];
+    float filteredAnglePrimary_;
+    float filteredAngleSecondary_;
 public:
     SteeringSystem(SteeringEncoderInterface *primarySensor)
     : primarySensor_(primarySensor)

diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp
@@ -13,19 +13,18 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         primaryConversion_ = primarySensor_->convert();
 
         // Filter both sensor angle readings
-        steeringFilters_[0].filtered_result(primaryConversion_.raw);
-        steeringFilters_[1].filtered_result(intake.secondaryConversion.raw);
-
+        filteredAnglePrimary_ = steeringFilters_[0].filtered_result(primaryConversion_.angle);
+        filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion);        
 
         // Both sensors are nominal and agree
         if (
             (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL)
             && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD)
-            && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_WARN_THRESHOLD)
+            && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_WARN_THRESHOLD)
         )
         {
             steeringData_ = {
-                .angle = primaryConversion_.angle,
+                .angle = filteredAnglePrimary_,
                 .status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL
             };
         }
@@ -34,11 +33,11 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         else if (
             (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL)
             || (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED)
-            || ((std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_ERROR_THRESHOLD))
+            || ((std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD))
         )
         {
             steeringData_ = {
-                .angle = primaryConversion_.angle,
+                .angle = filteredAnglePrimary_,
                 .status = SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL
             };
         }
@@ -49,7 +48,7 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         )
         {
             steeringData_ = {
-                .angle = intake.secondaryConversion.conversion,
+                .angle = filteredAngleSecondary_,
                 .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED
             };
         }

diff --git a/platformio.ini b/platformio.ini
@@ -50,7 +50,7 @@ lib_deps =
     Nanopb
     https://github.com/vjmuzik/NativeEthernet.git
     https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz
-    https://github.com/hytech-racing/shared_firmware_interfaces.git
+    https://github.com/hytech-racing/shared_firmware_interfaces.git#feature/parameterize-filter-class
     https://github.com/hytech-racing/shared_firmware_systems.git
     https://github.com/RCMast3r/spi_libs
     https://github.com/tonton81/FlexCAN_T4