Skip to content

Commit

Permalink
Adjusted steering system test for iir filter
Browse files Browse the repository at this point in the history
  • Loading branch information
@CL16gtgh
CL16gtgh committed May 18, 2024
1 parent 0c6639f commit 455aab3
Show file tree
Hide file tree
Showing 2 changed files with 94 additions and 38 deletions.
21 changes: 21 additions & 0 deletions lib/mock_interfaces/Filter_IIR.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,27 @@ class Filter_IIR
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
{
Expand Down
111 changes: 73 additions & 38 deletions test/test_systems/steering_system_test.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,22 @@
#include <gmock/gmock.h>
#include "SteeringSystem.h"

#define PRIMARY_ALPHA (0.6)
#define SECONDARY_ALPHA (0.6)
#define WARN_DISCREPANCY_OFFSET (0.5)
#define ERR_DISCREPANCY_OFFSET (0.5)
#define WARN_FILTER_LATENCY (6)
#define ERR_FILTER_LATENCY (7)

TEST(SteeringSystemTesting, test_steering_nominal)
{
float angles_to_test[5] = {-120.0f, -60.0f, 0.0f, 60.0f, 120.0f};
// Boilerplate definitions for the testing aparatus
SysClock sys_clock;
SysTick_s sys_tick = sys_clock.tick(0);
SteeringEncoderInterface primarySensor;
SteeringSystem steeringSystem(&primarySensor);
SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA);
Filter_IIR<float> 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)
Expand All @@ -30,7 +38,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);
}
}
Expand All @@ -41,7 +50,8 @@ 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_IIR<float> mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA};
float angle = 0.0f;

primarySensor.mockConversion.angle = angle;
Expand All @@ -58,7 +68,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);
}

Expand All @@ -68,7 +78,8 @@ 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_IIR<float> mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA};
float angle = 0.0f;

primarySensor.mockConversion.angle = angle;
Expand All @@ -85,7 +96,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);
}

Expand All @@ -95,25 +106,37 @@ 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_IIR<float> mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA};
float mockPrimaryFilteredAngle;
float mockSecondaryFilteredAngle;

float primaryAngle = 0.0f;
float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_WARN_THRESHOLD;
float secondaryAngle = primaryAngle + WARN_DISCREPANCY_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)
// Edge filter steps to align output
for (int i = 0; i < WARN_FILTER_LATENCY; i++)
{
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);
}

Expand All @@ -123,24 +146,35 @@ 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_IIR<float> mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA};
float mockPrimaryFilteredAngle;
float mockSecondaryFilteredAngle;

float primaryAngle = 0.0f;
float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_ERROR_THRESHOLD;
float secondaryAngle = primaryAngle + ERR_DISCREPANCY_OFFSET + STEERING_DIVERGENCE_ERROR_THRESHOLD;

primarySensor.mockConversion.angle = primaryAngle;
primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL;
steeringSystem.tick(
(SteeringSystemTick_s)
{
.tick = sys_tick,
.secondaryConversion = (AnalogConversion_s)
for (int i = 0; i < ERR_FILTER_LATENCY; i++)
{
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);
}
Expand All @@ -151,7 +185,8 @@ 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_IIR<float> mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA};
float angle = 100.0f;

primarySensor.mockConversion.angle = 0.0f;
Expand All @@ -168,7 +203,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);
}

Expand All @@ -178,7 +213,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;
Expand Down

0 comments on commit 455aab3

Please sign in to comment.