Skip to content

Commit 27fbcd4

Browse files
AlexBailey11AlexBailey11
committed
fixed checks
1 parent 873ba4b commit 27fbcd4

4 files changed

Lines changed: 53 additions & 36 deletions

File tree

‎lib/systems/include/SteeringSystem.h‎

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -13,15 +13,15 @@ struct SteeringParams_s {
1313
uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle
1414
uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle
1515

16-
int32_t analog_min_with_margins;
17-
int32_t analog_max_with_margins;
18-
int32_t digital_min_with_margins;
19-
int32_t digital_max_with_margins;
16+
int32_t analog_min_with_margins; // Added margins to min raw value
17+
int32_t analog_max_with_margins; // Added margins to max raw value
18+
int32_t digital_min_with_margins; // Added margins to min raw value
19+
int32_t digital_max_with_margins; // Added margins to max raw value
2020

21-
uint32_t span_signal_analog;
22-
uint32_t span_signal_digital;
23-
int32_t digital_midpoint;
24-
int32_t analog_midpoint;
21+
uint32_t span_signal_analog; // range of the analog sensor in counts
22+
uint32_t span_signal_digital; // range of the digital sensor in counts
23+
uint32_t digital_midpoint; // midpoint of raw values
24+
uint32_t analog_midpoint; // midpoint of raw values
2525

2626
// conversion rates
2727
// float deg_per_count_analog = 0.0439f; //hard coded for analog (180)
@@ -58,6 +58,11 @@ class SteeringSystem {
5858
const SteeringSystemData_s &get_steering_system_data() const {
5959
return _steeringSystemData;
6060
}
61+
62+
float get_unfiltered_analog_steering_deg() const {
63+
return _analog_angle_unfiltered;
64+
}
65+
6166
// Setters
6267
void set_steering_params(const SteeringParams_s &steeringParams) {
6368
_steeringParams = steeringParams;
@@ -92,6 +97,7 @@ class SteeringSystem {
9297
SteeringSystemData_s _steeringSystemData {};
9398
SteeringParams_s _steeringParams;
9499
//track the state of our system from the previous tick to compare against current state for implausibility checks
100+
float _analog_angle_unfiltered = 0.0f;
95101
float _prev_analog_angle = 0.0f;
96102
float _prev_digital_angle = 0.0f;
97103
float _prev_digital_vel_angle = 0.0f;

‎lib/systems/src/SteeringSystem.cpp‎

Lines changed: 17 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -23,19 +23,19 @@ void SteeringSystem::recalibrate_steering_digital() {
2323
_steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital;
2424
_steeringParams.analog_tol_deg = static_cast<float>(_steeringParams.span_signal_analog) * _steeringParams.analog_tolerance * _steeringParams.deg_per_count_analog;
2525
_steeringParams.digital_tol_deg = static_cast<float>(_steeringParams.span_signal_digital) *_steeringParams.digital_tolerance * _steeringParams.deg_per_count_digital;
26-
_steeringParams.digital_midpoint = static_cast<int32_t>((_steeringParams.max_steering_signal_digital + _steeringParams.min_steering_signal_digital) / 2); //NOLINT
27-
_steeringParams.analog_midpoint = static_cast<int32_t>((_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2); //NOLINT
28-
_steeringParams.analog_min_with_margins = static_cast<int32_t>(_steeringParams.min_steering_signal_analog) - _steeringParams.analog_tol_deg;
29-
_steeringParams.analog_max_with_margins = static_cast<int32_t>(_steeringParams.max_steering_signal_analog) + _steeringParams.analog_tol_deg;
30-
_steeringParams.digital_min_with_margins = static_cast<int32_t>(_steeringParams.min_steering_signal_digital) - _steeringParams.digital_tol_deg;
31-
_steeringParams.digital_max_with_margins = static_cast<int32_t>(_steeringParams.max_steering_signal_digital) + _steeringParams.digital_tol_deg;
32-
33-
if (max_observed_analog > min_observed_analog && _steeringParams.span_signal_analog > 2000) // prevents wrap around
26+
_steeringParams.digital_midpoint = (_steeringParams.max_steering_signal_digital + _steeringParams.min_steering_signal_digital) / 2;
27+
_steeringParams.analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2;
28+
_steeringParams.analog_min_with_margins = static_cast<unsigned long>(_steeringParams.min_steering_signal_analog - _steeringParams.analog_tol_deg); // NOLINT
29+
_steeringParams.analog_max_with_margins = static_cast<unsigned long>(_steeringParams.max_steering_signal_analog + _steeringParams.analog_tol_deg); // NOLINT
30+
_steeringParams.digital_min_with_margins = static_cast<unsigned long>(_steeringParams.min_steering_signal_digital - _steeringParams.digital_tol_deg); // NOLINT
31+
_steeringParams.digital_max_with_margins = static_cast<unsigned long>(_steeringParams.max_steering_signal_digital + _steeringParams.digital_tol_deg); // NOLINT
32+
33+
if (max_observed_analog > min_observed_analog && _steeringParams.span_signal_analog > 2500) // NOLINT with 360 deg analog sensor, typical span is about 2000
3434
{
3535
min_observed_analog = UINT32_MAX; // after calculating params, if the range is marginally greater than half the steering wheel adc, likely the min and max are clinging to a prior run that is not applicable, meaning we will need to reset the boundaries.
3636
max_observed_analog = 0;
3737
}
38-
if (max_observed_digital > min_observed_digital && _steeringParams.span_signal_digital > 9000)
38+
if (max_observed_digital > min_observed_digital && _steeringParams.span_signal_digital > 9000) // NOLINT with digital sensor, typical span is about 9000
3939
{
4040
min_observed_digital = UINT32_MAX;
4141
max_observed_digital = 0;
@@ -60,6 +60,7 @@ void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const Steering
6060
_steeringSystemData.digital_raw = digital_raw;
6161

6262
_steeringSystemData.analog_raw = analog_raw;
63+
_analog_angle_unfiltered = _convert_analog_sensor(analog_raw);
6364

6465
//Conversion from raw ADC to degrees
6566
_steeringSystemData.digital_steering_angle = _convert_digital_sensor(digital_raw);
@@ -73,14 +74,14 @@ void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const Steering
7374

7475

7576
if (dt >= 2) {
76-
float filtered_analog_angle = _filter_analog_angle(_convert_analog_sensor(analog_raw));
77+
float filtered_analog_angle = _filter_analog_angle(_analog_angle_unfiltered);
7778
_steeringSystemData.analog_steering_angle = filtered_analog_angle; // update the angle to the filtered value for downstream use and velocity calculation
7879
float dtheta_analog = filtered_analog_angle - _prev_analog_vel_angle;
7980
float dtheta_digital = _steeringSystemData.digital_steering_angle - _prev_digital_vel_angle;
8081

81-
_steeringSystemData.analog_steering_velocity_deg_s = (dtheta_analog / static_cast<float>(dt)) * 1000.0f;
82+
_steeringSystemData.analog_steering_velocity_deg_s = (dtheta_analog / static_cast<float>(dt)) * 1000.0f; // NOLINT 1000.0f is result of converting dt in millis to seconds
8283

83-
_steeringSystemData.digital_steering_velocity_deg_s = (dtheta_digital / static_cast<float>(dt)) * 1000.0f;
84+
_steeringSystemData.digital_steering_velocity_deg_s = (dtheta_digital / static_cast<float>(dt)) * 1000.0f; // NOLINT 1000.0f is result of converting dt in millis to seconds
8485

8586
_last_filtered_analog_angle = filtered_analog_angle;
8687
} else {
@@ -140,19 +141,19 @@ void SteeringSystem::update_observed_steering_limits(const uint32_t analog_raw,
140141
max_observed_analog = std::max(max_observed_analog, static_cast<uint32_t>(analog_raw));
141142
min_observed_digital = std::min(min_observed_digital, static_cast<uint32_t>(digital_raw)); //NOLINT should both be uint32_t
142143
max_observed_digital = std::max(max_observed_digital, static_cast<uint32_t>(digital_raw)); //NOLINT ^
143-
if (min_observed_analog < 5)
144+
if (min_observed_analog < 5) // NOLINT want to prevent sticking at 0 or clipping with small value
144145
{
145146
min_observed_analog = UINT32_MAX; // clipping if it is at 0, it is likely sensor is clipping or clipped in past and reading is holding the 0 value.
146147
}
147-
if (max_observed_analog > 3685)
148+
if (max_observed_analog > 3675) // NOLINT prevents clipping, this is slightly less than calculated value of actual max output of sensor with current resistor divider on VCF's ADC
148149
{
149150
max_observed_analog = 0; // clipping
150151
}
151-
if (min_observed_digital < 5)
152+
if (min_observed_digital < 10) // NOLINT want to prevent sticking at 0 or clipping
152153
{
153154
min_observed_digital = UINT32_MAX; // clipping on prior run.
154155
}
155-
if (max_observed_digital > 16384)
156+
if (max_observed_digital > 16374) // NOLINT 16374 = 2^14 - 10 to prevent clipping with 14 bit resolution on sensor
156157
{
157158
max_observed_digital = 0; // clipping
158159
}

‎src/VCF_Tasks.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -94,8 +94,8 @@ HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMic
9494
}
9595

9696
HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) {
97-
const uint32_t analog_raw = SteeringSystemInstance::instance().get_steering_system_data().analog_raw;
98-
const uint32_t digital_raw = SteeringSystemInstance::instance().get_steering_system_data().digital_raw;
97+
const uint32_t analog_raw = SteeringSystemInstance::instance().get_steering_system_data().analog_raw; // NOLINT thinks this is not initialized
98+
const uint32_t digital_raw = SteeringSystemInstance::instance().get_steering_system_data().digital_raw; // NOLINT thinks this is not initialized
9999

100100
SteeringSystemInstance::instance().update_observed_steering_limits(analog_raw, digital_raw);
101101

@@ -756,10 +756,10 @@ void setup_all_interfaces() {
756756
.max_steering_signal_analog = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG_ADDR),
757757
.min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::MIN_STEERING_SIGNAL_DIGITAL_ADDR),
758758
.max_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_DIGITAL_ADDR),
759-
.analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR),
760-
.analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR),
761-
.digital_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR),
762-
.digital_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR),
759+
.analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR), // NOLINT this is prev saved value so it is ok
760+
.analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR), // NOLINT this is prev saved value so it is ok
761+
.digital_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR), // NOLINT this is prev saved value so it is ok
762+
.digital_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR), // NOLINT this is prev saved value so it is ok
763763
.deg_per_count_analog = VCFSystemConstants::DEG_PER_COUNT_ANALOG,
764764
.deg_per_count_digital = VCFSystemConstants::DEG_PER_COUNT_DIGITAL,
765765
.analog_tolerance = VCFSystemConstants::ANALOG_TOLERANCE,
@@ -773,7 +773,7 @@ void setup_all_interfaces() {
773773
steering_params.span_signal_digital = steering_params.max_steering_signal_digital - steering_params.min_steering_signal_digital;
774774
steering_params.digital_midpoint = (steering_params.min_steering_signal_digital + steering_params.max_steering_signal_digital) / 2;
775775

776-
SteeringSystemInstance::create(steering_params);
776+
SteeringSystemInstance::create(steering_params); // NOLINT thinks steering params is not initialized
777777

778778
// Create Digital Steering Sensor singleton
779779
OrbisBRInstance::create(&Serial2);

‎test/test_systems/steering_system_test.h‎

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ SteeringParams_s gen_default_params(){
1616
params.min_steering_signal_digital = 25;
1717
params.max_steering_signal_digital = 8000; //testing values
1818

19-
params.span_signal_analog = 4095;
19+
params.span_signal_analog = 3071;
2020
params.span_signal_digital = 8000;
2121

2222

@@ -72,7 +72,7 @@ static SteeringEncoderReading_s hardcode_digital_data(int rawValue, SteeringEnco
7272
TEST(SteeringSystemTesting, test_adc_to_degree_conversion)
7373
{
7474
auto params = gen_default_params();
75-
SteeringSystem steering(params);
75+
SteeringSystem steering = SteeringSystem(params);
7676

7777
uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2;
7878
uint32_t digital_mid = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2;
@@ -83,6 +83,9 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion)
8383

8484
steering.evaluate_steering(analog_raw, digital_data, 1000);
8585
auto data = steering.get_steering_system_data();
86+
data.analog_steering_angle = steering.get_unfiltered_analog_steering_deg();
87+
88+
// debug_print_steering(data);
8689
EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f);
8790
EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f);
8891

@@ -92,25 +95,32 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion)
9295

9396
steering.evaluate_steering(analog_raw, digital_data, 1010);
9497
data = steering.get_steering_system_data();
98+
data.analog_steering_angle = steering.get_unfiltered_analog_steering_deg();
9599

96100
float expected_analog_min = (static_cast<int32_t>(params.min_steering_signal_analog) - static_cast<int32_t>(analog_mid)) * params.deg_per_count_analog;
97101
float expected_digital_min = -1 * (static_cast<int32_t>(params.min_steering_signal_digital) - static_cast<int32_t>(digital_mid)) * params.deg_per_count_digital;
98-
102+
103+
// debug_print_steering(data);
104+
// std::printf("Exp A Min: %.1f\n Exp D Min: %.1f\nMin A: %d\n Min D: %d\n\n", expected_analog_min, expected_digital_min, params.min_steering_signal_analog, params.min_steering_signal_digital);
99105
EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f);
100106
EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f);
101107

102108
//max values
103109
analog_raw = params.max_steering_signal_analog;
110+
std::printf("Analog Raw: %d\n", analog_raw);
104111
digital_data = hardcode_digital_data(params.max_steering_signal_digital);
105112

106113
steering.evaluate_steering(analog_raw, digital_data, 1020);
107114
data = steering.get_steering_system_data();
115+
data.analog_steering_angle = steering.get_unfiltered_analog_steering_deg();
116+
108117
float expected_analog_max = (static_cast<int32_t>(params.max_steering_signal_analog) - static_cast<int32_t>(analog_mid)) * params.deg_per_count_analog;
109118
float expected_digital_max = -1 * (static_cast<int32_t>(params.max_steering_signal_digital) - static_cast<int32_t>(digital_mid)) * params.deg_per_count_digital;
110119

120+
// debug_print_steering(data);
121+
// std::printf("Exp A Max: %.1f\nExp D Max: %.1f\nMax A: %d\nMax D: %d\n\n", expected_analog_max, expected_digital_max, params.max_steering_signal_analog, params.max_steering_signal_digital);
111122
EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f);
112-
EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f);
113-
123+
EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f);
114124
}
115125

116126
//FIXED

0 commit comments

Comments
 (0)