Squashed commit of the following:

commit 0b68870 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 17:33:24 2024 -0400 Squashed commit of the following: commit 07faefa Merge: f149cdc bb03837 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 17:29:16 2024 -0400 Merge branch 'master' into CASE_testbranch commit f149cdc Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 17:06:46 2024 -0400 Reapply "bad" This reverts commit 2a253cd. commit e8cbee7 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 17:06:35 2024 -0400 Reapply "Update main.cpp" This reverts commit 50de74c. commit 3d1aab3 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 17:06:23 2024 -0400 Reapply "Merge branch 'master' into CASE_testbranch" This reverts commit e062970. commit e062970 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:37:16 2024 -0400 Revert "Merge branch 'master' into CASE_testbranch" This reverts commit df74595. commit 50de74c Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:37:07 2024 -0400 Revert "Update main.cpp" This reverts commit dfa4f7a. commit 2a253cd Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:36:55 2024 -0400 Revert "bad" This reverts commit 95c4618. commit 95c4618 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:36:42 2024 -0400 bad commit dfa4f7a Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:24:22 2024 -0400 Update main.cpp commit df74595 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:23:20 2024 -0400 Merge branch 'master' into CASE_testbranch commit bb03837 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b56423 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 40dd816 Author: CL16gtgh <ryang359@gatech.edu> Date: Tue May 21 18:05:55 2024 -0400 Revert "Added Derate Factor code to AMS interface" This reverts commit 5262fe8. commit 760bd8f Author: CL16gtgh <ryang359@gatech.edu> Date: Tue May 21 18:04:56 2024 -0400 Revert "added derate factor to case and loadcell" This reverts commit 5d9761b. commit 5d9761b Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Tue May 21 00:07:20 2024 -0700 added derate factor to case and loadcell commit 5262fe8 Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 23:53:20 2024 -0700 Added Derate Factor code to AMS interface commit 23be5cd Merge: 8ac101b 1416d3e Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 22:54:56 2024 -0700 Merge branch 'master' of https://github.com/hytech-racing/MCU commit 1416d3e Author: Ben Hall <rcmast3r1@gmail.com> Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b Author: Ben Hall <rcmast3r1@gmail.com> Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit 28d5e72 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:17:14 2024 -0400 Revert "Squashed commit of the following:" This reverts commit 25136c7. commit 25136c7 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 16:15:59 2024 -0400 Squashed commit of the following: commit bb03837 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b56423 Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 23be5cd Merge: 8ac101b 1416d3e Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 22:54:56 2024 -0700 Merge branch 'master' of https://github.com/hytech-racing/MCU commit 1416d3e Author: Ben Hall <rcmast3r1@gmail.com> Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b Author: Ben Hall <rcmast3r1@gmail.com> Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit ccd205a Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 15:50:25 2024 -0400 finale commit d0c523e Author: CL16gtgh <ryang359@gatech.edu> Date: Wed May 22 14:57:58 2024 -0400 read more values commit b528dfc Merge: f4e69ef 207b299 Author: CL16gtgh <ryang359@gatech.edu> Date: Mon May 20 21:26:55 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit f4e69ef Author: CL16gtgh <ryang359@gatech.edu> Date: Mon May 20 01:16:52 2024 -0400 Added comments to steering test for readability commit c1710c2 Author: CL16gtgh <ryang359@gatech.edu> Date: Sat May 18 22:01:15 2024 -0400 Recalibrated bottom sensor and wheel spun commit 04524b7 Author: CL16gtgh <ryang359@gatech.edu> Date: Sat May 18 19:02:42 2024 -0400 Minor adjustment commit 455aab3 Author: CL16gtgh <ryang359@gatech.edu> Date: Sat May 18 18:45:53 2024 -0400 Adjusted steering system test for iir filter commit 0c6639f Author: CL16gtgh <ryang359@gatech.edu> Date: Sat May 18 14:38:51 2024 -0400 Added non zero filer alpha commit 7ab723e Merge: e7dceb7 04ad455 Author: CL16gtgh <ryang359@gatech.edu> Date: Fri May 17 23:49:31 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit e7dceb7 Author: CL16gtgh <ryang359@gatech.edu> Date: Sun May 12 22:06:24 2024 -0400 configuration for testing commit c76391e Author: CL16gtgh <ryang359@gatech.edu> Date: Sun May 12 20:02:55 2024 -0400 compiles. to be tested commit 6df7c59 Author: CL16gtgh <ryang359@gatech.edu> Date: Sun May 12 00:52:58 2024 -0400 Integrating filter into steering system
hytech-racing · May 25, 2024 · fd0885f · fd0885f
1 parent 07faefa
commit fd0885f
Show file tree

Hide file tree

Showing 7 changed files with 205 additions and 51 deletions.
diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h
@@ -99,9 +99,10 @@ const float LOADCELL_RR_OFFSET = 23.761 / LOADCELL_RR_SCALE;
 
 // Steering parameters
 const float PRIMARY_STEERING_SENSE_OFFSET = 0.0; // units are degrees
-const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 812;
-const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3179;
-const int SECONDARY_STEERING_SENSE_CENTER = 1970;
-const float STEERING_RANGE_DEGREES = 256.05f;
+const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 785;    // 794    // 812    // 130 deg
+const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3087;    // 3075    // 3179    // 134 deg
+const int SECONDARY_STEERING_SENSE_CENTER = 1945;    // 1960    // 1970
+const float STEERING_RANGE_DEGREES = 257.0f;    // 253.0f    // 256.05f   // 134+130-7(slop)
+const float STEERING_IIR_ALPHA = 0.7f;  // shaves off around 1 deg of max discrepancy
 
 #endif /* __MCU15_H__ */
diff --git a/lib/mock_interfaces/Filter_IIR.h b/lib/mock_interfaces/Filter_IIR.h
@@ -0,0 +1,69 @@
+/* IIR digital low pass filter */
+#ifndef __FILTER_IIR__
+#define __FILTER_IIR__
+
+#include <stdint.h>
+
+#define DEFAULT_ALPHA 0.0
+
+template <typename dataType>
+class Filter_IIR
+{
+
+public:
+    /**
+     * Constructors
+     */
+    Filter_IIR(float alpha, dataType init_val=0) {
+        set_alpha(alpha);
+        prev_reading = init_val;
+    }
+    Filter_IIR() {
+        Filter_IIR(DEFAULT_ALPHA);
+    }
+
+    void set_alpha(float alpha);
+    dataType get_prev_reading() const {return prev_reading;}
+
+    dataType filtered_result(dataType new_val);
+
+private:    
+    float alpha;
+    dataType prev_reading;
+};
+
+template <typename dataType>
+void Filter_IIR<dataType>::set_alpha(float alpha) {
+    if (alpha > 1.0) {
+        this->alpha = 1.0;
+    }
+    else if (alpha < 0.0) {
+        this->alpha = 0.0;
+    }
+    else
+    {
+        this->alpha = alpha;
+    }
+}
+
+template <typename dataType>
+dataType Filter_IIR<dataType>::filtered_result(dataType new_val) {
+    prev_reading = (1 - alpha) * new_val + alpha * prev_reading;
+
+    return prev_reading;
+}
+
+template <typename dataType, int N>
+class FilterIIRMulti
+{
+protected:
+    Filter_IIR<dataType> filter_channels_[N];
+public:
+    virtual void setAlphas(int channel, float alpha) 
+    {
+    filter_channels_[channel].set_alpha(alpha);
+    }
+};
+
+#endif
+#pragma once
diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h
@@ -3,14 +3,17 @@
 
 #include "SteeringEncoderInterface.h"
 #include "AnalogSensorsInterface.h"
+#include "Filter_IIR.h"
 #include "SysClock.h"
 // Digital Encoder = Primary Sensor
 // Analog Encoder = Secondary Sensor
 
 // Definitions
 // TODO: evalaute reasonable thresholds for agreement
-#define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by x degrees before output is considered erroneous
+#define STEERING_DIVERGENCE_ERROR_THRESHOLD (16.0) // Steering sensors can disagree by x degrees before output is considered erroneous
 #define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge x degrees
+#define NUM_SENSORS 2
+#define DEFAULT_STEERING_ALPHA (0.0)
 
 // Enums
 enum class SteeringSystemStatus_e
@@ -40,11 +43,32 @@ class SteeringSystem
     SteeringEncoderInterface *primarySensor_;
     SteeringEncoderConversion_s primaryConversion_;
     SteeringSystemData_s steeringData_;
+
+    /**
+     * Utility digital IIR filters
+     * 0 : primary sensor filter
+     * 1 : secondary sensor filter
+     */
+    Filter_IIR<float> steeringFilters_[NUM_SENSORS];
+    float filteredAnglePrimary_;
+    float filteredAngleSecondary_;
 public:
     SteeringSystem(SteeringEncoderInterface *primarySensor)
-    : primarySensor_(primarySensor)
+    : SteeringSystem(primarySensor, DEFAULT_STEERING_ALPHA)
+    {}
+
+    SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlpha)
+    : SteeringSystem(primarySensor, filterAlpha, filterAlpha)
     {}
 
+    SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlphaPrimary, float filterAlphaSecondary)
+    : primarySensor_(primarySensor)
+    {
+        steeringFilters_[0] = Filter_IIR<float>(filterAlphaPrimary);
+        steeringFilters_[1] = Filter_IIR<float>(filterAlphaSecondary);
+
+    }
+
     /// @brief Computes steering angle and status of the steering system.
     /// @param secondaryAngle The computed steering angle as reported by the secondary steering sensor.
     /// @return SteeringSystemOutput_s contains steering angle and SteeringSystemStatus_e

diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp
@@ -12,15 +12,19 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         primarySensor_->sample();
         primaryConversion_ = primarySensor_->convert();
 
+        // Filter both sensor angle readings
+        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
             };
         }
@@ -29,11 +33,11 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         else if (
             (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL)
             || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (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
             };
         }
@@ -44,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#af96a63
     https://github.com/RCMast3r/spi_libs#2214fee
     https://github.com/tonton81/FlexCAN_T4#b928bcb

diff --git a/src/main.cpp b/src/main.cpp
@@ -142,7 +142,7 @@ struct inverters
 // */
 
 SysClock sys_clock;
-SteeringSystem steering_system(&steering1);
+SteeringSystem steering_system(&steering1, STEERING_IIR_ALPHA);
 BuzzerController buzzer(BUZZER_ON_INTERVAL);
 
 SafetySystem safety_system(&ams_interface, &wd_interface);