From fd0885fe3a8ae2df1174fa45ef04c07f072c0856 Mon Sep 17 00:00:00 2001 From: CL16gtgh Date: Fri, 24 May 2024 21:28:01 -0400 Subject: [PATCH] Squashed commit of the following: commit 0b688704841d2fa14da35a5fa5159061fa30a375 Author: CL16gtgh Date: Wed May 22 17:33:24 2024 -0400 Squashed commit of the following: commit 07faefaba316f8a090ab1d0191dd06c95f139e82 Merge: f149cdcc bb038370 Author: CL16gtgh Date: Wed May 22 17:29:16 2024 -0400 Merge branch 'master' into CASE_testbranch commit f149cdcc7d914b543d060fb815c316e24461bb95 Author: CL16gtgh Date: Wed May 22 17:06:46 2024 -0400 Reapply "bad" This reverts commit 2a253cde26abbe9648e8cf12feb2ffedcb2c630d. commit e8cbee792d0dbe17f318fcc82e0d5b3bef443b1d Author: CL16gtgh Date: Wed May 22 17:06:35 2024 -0400 Reapply "Update main.cpp" This reverts commit 50de74cb8fdf54dfb73dd047d41d2618a97a3503. commit 3d1aab350837aa61484f96b4e33bf0752168c37b Author: CL16gtgh Date: Wed May 22 17:06:23 2024 -0400 Reapply "Merge branch 'master' into CASE_testbranch" This reverts commit e062970d4409bd761214e392f27842b7e6870ec2. commit e062970d4409bd761214e392f27842b7e6870ec2 Author: CL16gtgh Date: Wed May 22 16:37:16 2024 -0400 Revert "Merge branch 'master' into CASE_testbranch" This reverts commit df74595acc64ee569cfd9377f0f344c784be3323. commit 50de74cb8fdf54dfb73dd047d41d2618a97a3503 Author: CL16gtgh Date: Wed May 22 16:37:07 2024 -0400 Revert "Update main.cpp" This reverts commit dfa4f7a3cf177dcaee65c59cd3a406c253359288. commit 2a253cde26abbe9648e8cf12feb2ffedcb2c630d Author: CL16gtgh Date: Wed May 22 16:36:55 2024 -0400 Revert "bad" This reverts commit 95c4618988317737ebc4a31afa6c6d5ee3a2e27b. commit 95c4618988317737ebc4a31afa6c6d5ee3a2e27b Author: CL16gtgh Date: Wed May 22 16:36:42 2024 -0400 bad commit dfa4f7a3cf177dcaee65c59cd3a406c253359288 Author: CL16gtgh Date: Wed May 22 16:24:22 2024 -0400 Update main.cpp commit df74595acc64ee569cfd9377f0f344c784be3323 Author: CL16gtgh Date: Wed May 22 16:23:20 2024 -0400 Merge branch 'master' into CASE_testbranch commit bb038370a0f0c13cd0e1e4bcb25e634b6ee7feed Author: CL16gtgh Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b5642365faf8db118b26bba7cd2324b8d8ffa7f Author: CL16gtgh Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 40dd816373d34d825800ed9cf16a17fd0994ee7d Author: CL16gtgh Date: Tue May 21 18:05:55 2024 -0400 Revert "Added Derate Factor code to AMS interface" This reverts commit 5262fe863b9df002e12565476522d49171ff36b1. commit 760bd8f0acf3683092601822cb4682c4eb286568 Author: CL16gtgh Date: Tue May 21 18:04:56 2024 -0400 Revert "added derate factor to case and loadcell" This reverts commit 5d9761b97bdf00b7dc1c1b5c5e89d01f0a533253. commit 5d9761b97bdf00b7dc1c1b5c5e89d01f0a533253 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 5262fe863b9df002e12565476522d49171ff36b1 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 23be5cd4e88243c9d4273c0ad2025a7270980f4c Merge: 8ac101b5 1416d3eb 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 1416d3eb16d117c7cb9bd9fb9273aa1d3a93779c Author: Ben Hall Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b6b7111dbdc48f6af84ce1b50195d7a855 Author: Ben Hall Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit 28d5e7226319c65f8976b519d1162793991da576 Author: CL16gtgh Date: Wed May 22 16:17:14 2024 -0400 Revert "Squashed commit of the following:" This reverts commit 25136c7f3b5790c11e6ad2022c372ac16ea1a189. commit 25136c7f3b5790c11e6ad2022c372ac16ea1a189 Author: CL16gtgh Date: Wed May 22 16:15:59 2024 -0400 Squashed commit of the following: commit bb038370a0f0c13cd0e1e4bcb25e634b6ee7feed Author: CL16gtgh Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b5642365faf8db118b26bba7cd2324b8d8ffa7f Author: CL16gtgh Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 23be5cd4e88243c9d4273c0ad2025a7270980f4c Merge: 8ac101b5 1416d3eb 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 1416d3eb16d117c7cb9bd9fb9273aa1d3a93779c Author: Ben Hall Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b6b7111dbdc48f6af84ce1b50195d7a855 Author: Ben Hall Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit ccd205a147946ce2619be9fbd60d3a7be67dfcd3 Author: CL16gtgh Date: Wed May 22 15:50:25 2024 -0400 finale commit d0c523e211f2316917893533fb185f0d1f8f3ee2 Author: CL16gtgh Date: Wed May 22 14:57:58 2024 -0400 read more values commit b528dfc3c72147cf90c0177a1c478504a2fd2db8 Merge: f4e69efc 207b2991 Author: CL16gtgh Date: Mon May 20 21:26:55 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit f4e69efce5c21a20d632c5b64b0400da11872775 Author: CL16gtgh Date: Mon May 20 01:16:52 2024 -0400 Added comments to steering test for readability commit c1710c297db5fda3bb99a6213de3eb25367f658d Author: CL16gtgh Date: Sat May 18 22:01:15 2024 -0400 Recalibrated bottom sensor and wheel spun commit 04524b778b34709dba2cb7e79f0b80f0fcfb5360 Author: CL16gtgh Date: Sat May 18 19:02:42 2024 -0400 Minor adjustment commit 455aab32aeba2e99e75fc6087c9c6bb9375be00e Author: CL16gtgh Date: Sat May 18 18:45:53 2024 -0400 Adjusted steering system test for iir filter commit 0c6639ff352665cde326ecfea17d33c34b1de844 Author: CL16gtgh Date: Sat May 18 14:38:51 2024 -0400 Added non zero filer alpha commit 7ab723e7da6eb52058e14655fb37757a9f8a879f Merge: e7dceb74 04ad455f Author: CL16gtgh Date: Fri May 17 23:49:31 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit e7dceb740d831226a64781a118fe671c93b078b7 Author: CL16gtgh Date: Sun May 12 22:06:24 2024 -0400 configuration for testing commit c76391e9a03d42b181fa2c0a006a281fdad7edf9 Author: CL16gtgh Date: Sun May 12 20:02:55 2024 -0400 compiles. to be tested commit 6df7c595af71eb39512a781410067e2887d03e46 Author: CL16gtgh Date: Sun May 12 00:52:58 2024 -0400 Integrating filter into steering system --- include/MCU_rev15_defs.h | 9 +- lib/mock_interfaces/Filter_IIR.h | 69 ++++++++++++ lib/systems/include/SteeringSystem.h | 28 ++++- lib/systems/src/SteeringSystem.cpp | 14 ++- platformio.ini | 2 +- src/main.cpp | 2 +- test/test_systems/steering_system_test.h | 132 ++++++++++++++++------- 7 files changed, 205 insertions(+), 51 deletions(-) create mode 100644 lib/mock_interfaces/Filter_IIR.h diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 0a4991b64..1ca6f11fb 100644 --- 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__ */ \ No newline at end of file diff --git a/lib/mock_interfaces/Filter_IIR.h b/lib/mock_interfaces/Filter_IIR.h new file mode 100644 index 000000000..2c9c7e69c --- /dev/null +++ b/lib/mock_interfaces/Filter_IIR.h @@ -0,0 +1,69 @@ +/* IIR digital low pass filter */ +#ifndef __FILTER_IIR__ +#define __FILTER_IIR__ + +#include + +#define DEFAULT_ALPHA 0.0 + +template +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 +void Filter_IIR::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 +dataType Filter_IIR::filtered_result(dataType new_val) { + prev_reading = (1 - alpha) * new_val + alpha * prev_reading; + + return prev_reading; +} + +template +class FilterIIRMulti +{ +protected: + Filter_IIR filter_channels_[N]; +public: + virtual void setAlphas(int channel, float alpha) + { + filter_channels_[channel].set_alpha(alpha); + } +}; + +#endif +#pragma once \ No newline at end of file diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 950cb2ce3..c2fa7f7c5 100644 --- 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 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(filterAlphaPrimary); + steeringFilters_[1] = Filter_IIR(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 index ae2db4042..32439736e 100644 --- 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 index a401d9517..5d9237b66 100644 --- 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 index 4aad6bc6b..8f2357000 100644 --- 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); diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 9b0d22fa5..7c907f8d2 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -4,6 +4,13 @@ #include #include "SteeringSystem.h" +#define PRIMARY_ALPHA (0.7) // parameter used on car +#define SECONDARY_ALPHA (0.7) +#define WARN_DIVERGENCE_OFFSET (0.6) +#define ERR_DIVERGENCE_OFFSET (0.6) +#define WARN_FILTER_LATENCY (8) +#define ERR_FILTER_LATENCY (9) + TEST(SteeringSystemTesting, test_steering_nominal) { float angles_to_test[5] = {-120.0f, -60.0f, 0.0f, 60.0f, 120.0f}; @@ -11,7 +18,9 @@ TEST(SteeringSystemTesting, test_steering_nominal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; // Sweep through a few angles where the sensors agree perfectly and check the system is nominal for (float angle: angles_to_test) @@ -30,7 +39,8 @@ TEST(SteeringSystemTesting, test_steering_nominal) } } ); - ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL); } } @@ -41,7 +51,9 @@ TEST(SteeringSystemTesting, test_steering_primary_is_marginal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 0.0f; primarySensor.mockConversion.angle = angle; @@ -58,7 +70,7 @@ TEST(SteeringSystemTesting, test_steering_primary_is_marginal) } } ); - ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); } @@ -68,7 +80,9 @@ TEST(SteeringSystemTesting, test_steering_secondary_is_marginal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 0.0f; primarySensor.mockConversion.angle = angle; @@ -85,7 +99,7 @@ TEST(SteeringSystemTesting, test_steering_secondary_is_marginal) } } ); - ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); } @@ -95,25 +109,45 @@ TEST(SteeringSystemTesting, test_steering_divergence_warning) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float mockPrimaryFilteredAngle; + float mockSecondaryFilteredAngle; + float primaryAngle = 0.0f; - float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_WARN_THRESHOLD; + // Pend divergence to sensor readings + // - IIR filter eq.: filtered_val = (1-alpha)*new_val + alpha*prev_val + // - when divergence set exactly to system threshold + // - divergence magnitude gets shaved off, will not trigger above threshold + // - pend offset to divergence to trip threshold + float secondaryAngle = primaryAngle + WARN_DIVERGENCE_OFFSET + STEERING_DIVERGENCE_WARN_THRESHOLD; // edge discrepancy - primarySensor.mockConversion.angle = primaryAngle; - primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; - steeringSystem.tick( - (SteeringSystemTick_s) - { - .tick = sys_tick, - .secondaryConversion = (AnalogConversion_s) + // Latency due to filtering + // - required steps of filtering for divergence to rise up to threshold with offset pended + // - e.g. when alpha = 0.7, pending offset = 0.6, it takes a minimal of 8 steps of filtering to exceed warn threshold + for (int i = 0; i < WARN_FILTER_LATENCY; i++) // edge filter steps to align output + { + mockPrimaryFilteredAngle = mockAngleFilters[0].filtered_result(primaryAngle); + mockSecondaryFilteredAngle = mockAngleFilters[1].filtered_result(secondaryAngle); + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) { - .raw = 0, - .conversion = secondaryAngle, - .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } } - } - ); - ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == primaryAngle); + ); + } + + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockPrimaryFilteredAngle); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); } @@ -123,24 +157,44 @@ TEST(SteeringSystemTesting, test_steering_divergence_error) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float mockPrimaryFilteredAngle; + float mockSecondaryFilteredAngle; + float primaryAngle = 0.0f; - float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_ERROR_THRESHOLD; + // Pend divergence to sensor readings + // - IIR filter eq.: filtered_val = (1-alpha)*new_val + alpha*prev_val + // - when divergence set exactly to system threshold + // - divergence magnitude gets shaved off, will not trigger above threshold + // - pend offset to divergence to trip threshold + float secondaryAngle = primaryAngle + ERR_DIVERGENCE_OFFSET + STEERING_DIVERGENCE_ERROR_THRESHOLD; // edge discrepancy - primarySensor.mockConversion.angle = primaryAngle; - primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; - steeringSystem.tick( - (SteeringSystemTick_s) - { - .tick = sys_tick, - .secondaryConversion = (AnalogConversion_s) + // Latency due to filtering + // - required steps of filtering for divergence to rise up to threshold with offset pended + // - e.g. when alpha = 0.7, pending offset = 0.6, it takes a minimal of 9 steps of filtering to exceed error threshold + for (int i = 0; i < ERR_FILTER_LATENCY; i++) // edge filter steps to align output + { + mockPrimaryFilteredAngle = mockAngleFilters[0].filtered_result(primaryAngle); + mockSecondaryFilteredAngle = mockAngleFilters[1].filtered_result(secondaryAngle); + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) { - .raw = 0, - .conversion = secondaryAngle, - .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } } - } - ); + ); + } + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == 0.0f); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR); } @@ -151,7 +205,9 @@ TEST(SteeringSystemTesting, test_steering_primary_is_missing) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 100.0f; primarySensor.mockConversion.angle = 0.0f; @@ -168,7 +224,7 @@ TEST(SteeringSystemTesting, test_steering_primary_is_missing) } } ); - ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[1].filtered_result(angle)); ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED); } @@ -178,7 +234,7 @@ TEST(SteeringSystemTesting, test_steering_primary_is_missing_and_secondary_is_ma SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor); + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); float angle = 100.0f; primarySensor.mockConversion.angle = 0.0f;