From 2d2ebdf55085b17dd95e8535cf491b5f4718bb47 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 8 Feb 2026 17:50:03 -0500 Subject: [PATCH 01/39] created PedalsSystem files --- lib/systems/include/SteeringSystem.h | 0 lib/systems/src/SteeringSystem.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 lib/systems/include/SteeringSystem.h create mode 100644 lib/systems/src/SteeringSystem.cpp diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h new file mode 100644 index 0000000..e69de29 diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp new file mode 100644 index 0000000..e69de29 From d95efeda4b8e161478bf10ece307bae167a8cc7f Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 9 Feb 2026 20:53:07 -0500 Subject: [PATCH 02/39] setup most of SteeringSystem class --- lib/systems/include/SteeringSystem.h | 135 +++++++++++++++++++++++++++ lib/systems/src/SteeringSystem.cpp | 38 ++++++++ 2 files changed, 173 insertions(+) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index e69de29..6d2195f 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -0,0 +1,135 @@ +#ifndef STEERING_SYSTEM_H +#define STEERING_SYSTEM_H +#include + +#include "SharedFirmwareTypes.h" + + + +struct SteeringParams_s { + uint32_t min_steering_signal_analog; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) + uint32_t max_steering_signal_analog; //Raw ADC value from analog sensor at maximum (right) steering angle + uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle + uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle + + + uint32_t min_steering_analog; //minimum sensor output *analog + uint32_t max_steering_analog; //maximimum sensor output *analog + uint32_t min_steering_digital; // maximum sensor output *digital + uint32_t max_steering_digital; //maximum sensor output *digital + + float deg_per_count; //degrees of steering angle per count of the sensor + + + float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid + float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed + +}; + + +struct SteeringSystemData_s +{ + float analog_steering_angle; //in degrees + float digital_steering_angle; //in degrees + + float steering_velocity_deg_s; //in degrees per second + + bool digital_sensor_error; + bool sensor_difference_implausibility; + bool digital_oor_implausibility; + bool analog_oor_implausibility; +} + + +class SteeringSystem +{ +public: + SteeringSystem(const SteeringParams &steeringParams) : _steeringParams(steeringParams) + {} + + // Setters + void set_steering_params(const SteeringParams &steeringParams); + + void set_steering_system_data(const SteeringSystemData_s &steeringSystemData); + + void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData); + + + // Getters + const SteeringParams_s &get_steering_params() const; + + const SteeringSystemData_s &get_steering_system_data() const; + + const SteeringSensorData_s &get_steering_sensor_data() const; + + // Other Functions + void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data); + + + + + + +private: + + float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); + + float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); + + + + //returns true if steering_analog is outside of the range defined by min and max sensor values + bool _evaluate_steering_oor_analog(int steering_analog, + int min_sensor_value_analog, + int max_sensor_value_analog); + + //returns true if steering_digital is outside the range defined by min and max sensor values + bool _evaluate_steering_oor_digital(int steering_digital, + int min_sensor_value_digital, + int max_sensor_value_digital); + + //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) + bool _evaluate_steering_dtheta_exceeded(int steering_analog, + int steering_digital, + float dt); + + + +/* + + + + int32_t _prev_raw_adc = -1; // Initialize to -1 so we know it's the first run + int32_t _rotation_count = 0; // Tracks full 360-degree turns (e.g., -1, 0, 1, 2) + + - + const int32_t MAX_ADC_LIMIT = 4095; // The rollover point + const int32_t HALF_ADC_LIMIT = 2048; // Threshold to detect a wrap + + In order to check current total number of rotations (analog goes from max value back down to 0), we need to see if we exceeded a limit between readings + leaving this commented for now because this conflicts with one of the implausibility checks (max change in angle per reading) and we can revisit this if we find a solution +*/ + SteeringSensorData_s _steeringSensorData; + SteeringSystemData_s _steeringSystemData; + SteeringParams_s _steeringParams; +}; + + + + + + + + + + + + + + + + + +using SteeringSystemInstance = etl::singleton; + +#endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index e69de29..bac01e3 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -0,0 +1,38 @@ +#include +#include "SteeringSystem.h" + + +bool SteeringSystem::_evaluate_steering_oor_analog(int steering_analog, + int min_sensor_value_analog, + int max_sensor_value_analog,){ + if(steering_analog < min_sensor_value_analog || steering_analog > max_sensor_value_analog){ + SteeringParams.analog_oor_implausibility = true; + return true; + } + return false; + } + +bool SteeringSystem::_evaluate_steering_oor_digital(int steering_digital, + int min_sensor_value_digital, + int max_sensor_value_digital){ + if(steering_digital < min_sensor_value_digital || steering_digital > max_sensor_value_analog){ + SteeringParams.digital_oor_implausibility = true; + return true; + } + return false; + } + +bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dt){ + if(dt > SteeringParams.max_dtheta_threshold){ + return true; + } + return false; + } + + +float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) + + + + +//_steeringParams.deg_per_count From 66577d4db6c9c26e9ca1a5e2ea420dfb528b42e7 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 9 Feb 2026 21:39:15 -0500 Subject: [PATCH 03/39] added a few last function declarations, might be done with header for now --- lib/systems/include/SteeringSystem.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 6d2195f..f194fcf 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -65,7 +65,7 @@ class SteeringSystem // Other Functions void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data); - + void evaluate_steering(const SteeringSensorData_s ¤t_steering_data); @@ -96,9 +96,6 @@ class SteeringSystem /* - - - int32_t _prev_raw_adc = -1; // Initialize to -1 so we know it's the first run int32_t _rotation_count = 0; // Tracks full 360-degree turns (e.g., -1, 0, 1, 2) From 1b177cdf00edb662d77f00247031acbfe518f817 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 9 Feb 2026 21:42:49 -0500 Subject: [PATCH 04/39] added to system data struct --- lib/systems/include/SteeringSystem.h | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index f194fcf..2b14c07 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -31,6 +31,7 @@ struct SteeringSystemData_s { float analog_steering_angle; //in degrees float digital_steering_angle; //in degrees + float output_steering_angle; // represents the better output of the two sensors or some combination of the values float steering_velocity_deg_s; //in degrees per second From c1abbff00486e9d1f85f3723f75d6b5baa6cdbeb Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 11 Feb 2026 20:14:23 -0500 Subject: [PATCH 05/39] added conversion to degree functions, worked on recalibrating values --- lib/systems/include/SteeringSystem.h | 21 +++++++++++++++++--- lib/systems/src/SteeringSystem.cpp | 29 ++++++++++++++++++++++++++-- 2 files changed, 45 insertions(+), 5 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 2b14c07..0c24072 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -45,15 +45,24 @@ struct SteeringSystemData_s class SteeringSystem { public: - SteeringSystem(const SteeringParams &steeringParams) : _steeringParams(steeringParams) + SteeringSystem(const SteeringParams_s &steeringParams) : _steeringParams(steeringParams) {} // Setters - void set_steering_params(const SteeringParams &steeringParams); + void set_steering_params(const SteeringParams_s &steeringParams) + { +//parameters + } + void set_steering_system_data(const SteeringSystemData_s &steeringSystemData); + { + + //output that comes once we've calibrated everything, we output this data + } void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData); + //get the current value from the sensor // Getters @@ -65,6 +74,12 @@ class SteeringSystem // Other Functions void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data); + { + uint32_t analog_raw_value = static_cast(current_steering_data.analog_raw); + uint32_t digital_raw_value = static_cast(current_steering_data.digital_raw); + bool steering_flipped = + + } void evaluate_steering(const SteeringSensorData_s ¤t_steering_data); @@ -74,7 +89,7 @@ class SteeringSystem private: float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); - + float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index bac01e3..fdd00fc 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -22,17 +22,42 @@ bool SteeringSystem::_evaluate_steering_oor_digital(int steering_digital, return false; } -bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dt){ - if(dt > SteeringParams.max_dtheta_threshold){ +bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ + if(dtheta > SteeringParams.max_dtheta_threshold){ return true; } return false; } + + +float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) +{ + // Get the raw value + uint32_t raw_val = current_steering_data.steering_sensor_analog; // Check your struct for exact name + + // Calculate how far we are from the minimum raw count delta + int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; + + // Convert that delta into degrees + float degrees_delta = (float)count_delta * _steeringParams.deg_per_count; + + // Add to the starting minimum angle to get absolute position + return (float)_steeringParams.min_steering_analog + degrees_delta; +} + + float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) +{ + // Same logic for digital + uint32_t raw_val = current_steering_data.steering_sensor_digital; + int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_digital; + float degrees_delta = (float)count_delta * _steeringParams.deg_per_count; + return (float)_steeringParams.min_steering_digital + degrees_delta; +} //_steeringParams.deg_per_count From d083f5a72d5704004366ffc43d67bd4ee106e370 Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Sun, 15 Feb 2026 17:02:15 -0500 Subject: [PATCH 06/39] comments --- lib/systems/include/SteeringSystem.h | 15 ++++++++------- lib/systems/src/SteeringSystem.cpp | 12 +++++------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 2b14c07..33ca85e 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -13,10 +13,10 @@ struct SteeringParams_s { uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - uint32_t min_steering_analog; //minimum sensor output *analog - uint32_t max_steering_analog; //maximimum sensor output *analog - uint32_t min_steering_digital; // maximum sensor output *digital - uint32_t max_steering_digital; //maximum sensor output *digital + uint32_t min_steering_analog; //minimum sensor output *analog 0 + uint32_t max_steering_analog; //maximimum sensor output *analog 4095 + uint32_t min_steering_digital; // maximum sensor output *digital 4095 + uint32_t max_steering_digital; //maximum sensor output *digital0 float deg_per_count; //degrees of steering angle per count of the sensor @@ -107,9 +107,10 @@ class SteeringSystem In order to check current total number of rotations (analog goes from max value back down to 0), we need to see if we exceeded a limit between readings leaving this commented for now because this conflicts with one of the implausibility checks (max change in angle per reading) and we can revisit this if we find a solution */ - SteeringSensorData_s _steeringSensorData; - SteeringSystemData_s _steeringSystemData; - SteeringParams_s _steeringParams; + SteeringSensorData_s _steeringSensorData{}; + SteeringSystemData_s _steeringSystemData{}; + SteeringParams_s _steeringParams{}; + bool _implaus }; diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index bac01e3..0b48dbc 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -4,18 +4,16 @@ bool SteeringSystem::_evaluate_steering_oor_analog(int steering_analog, int min_sensor_value_analog, - int max_sensor_value_analog,){ - if(steering_analog < min_sensor_value_analog || steering_analog > max_sensor_value_analog){ - SteeringParams.analog_oor_implausibility = true; - return true; - } - return false; + int max_sensor_value_analog){ + + return (steering_analog < min_sensor_value_analog || steering_analog > max_sensor_value_analog); + } bool SteeringSystem::_evaluate_steering_oor_digital(int steering_digital, int min_sensor_value_digital, int max_sensor_value_digital){ - if(steering_digital < min_sensor_value_digital || steering_digital > max_sensor_value_analog){ + if(steering_digital < min_sensor_value_digital || steering_digital > max_sensor_value_digital){ SteeringParams.digital_oor_implausibility = true; return true; } From 804cbfde40f77f1018670a3754482566308e8e14 Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Mon, 16 Feb 2026 20:19:56 -0500 Subject: [PATCH 07/39] implausability error, dtheta check, sensor priority --- lib/systems/src/SteeringSystem.cpp | 184 +++++++++++++++++++++++++---- 1 file changed, 164 insertions(+), 20 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index c0d1e8c..7aa5312 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -2,41 +2,86 @@ #include "SteeringSystem.h" -bool SteeringSystem::_evaluate_steering_oor_analog(int steering_analog, - int min_sensor_value_analog, - int max_sensor_value_analog){ - - return (steering_analog < min_sensor_value_analog || steering_analog > max_sensor_value_analog); - +void SteeringSystem::recalibrate_steering(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on){ + //get current raw angles + const uint32_t analog_raw = static_cast(current_steering_data.steering_sensor_analog); + const uint32_t digital_raw = static_cast(current_steering_data.steering_sensor_digital); + // _update_observed_limits(analog_raw, digital_raw); + + //button just pressed ->recalibration window + if (calibration_is_on && !_calibrating){ + _calibrating = true; + _min_observed_analog = 4096; + _max_observed_analog = 0; + _min_observed_digital = 4096; + _max_observed_digital = 0; + } + + //update relative extremeties + if (calibration_is_on){ + _min_observed_analog = std::min(_min_observed_analog, analog_raw); + _max_observed_analog = std::max(_max_observed_analog, analog_raw); + _min_observed_digital = std::min(_min_observed_digital, digital_raw); + _max_observed_digital = std::max(_max_observed_digital, digital_raw); + return; + } + //button released ->commit the values + if (!calibration_is_on && _calibrating) { + _calibrating = false; + _steeringParams.min_steering_signal_analog = _min_observed_analog; + _steeringParams.max_steering_signal_analog = _max_observed_analog; + _steeringParams.min_steering_signal_digital = _min_observed_digital; + _steeringParams.max_steering_signal_digital = _max_observed_digital; + } + // swaps min & max in the params if sensor is negative + if (_steeringParams.min_steering_signal_analog > _steeringParams.max_steering_signal_analog) { + std::swap(_steeringParams.min_steering_signal_analog, _steeringParams.max_steering_signal_analog); + } + if (_steeringParams.min_steering_signal_digital > _steeringParams.max_steering_signal_digital) + { + std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); + } + + +} +bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog) + { + //check if the analog value is within bounds +- error + const uint32_t span = _steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog; + const uint32_t analog_margin_counts = static_cast(_steeringParams.analog_tol_deg * span); + const uint32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; + const uint32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; + return (steering_analog < min_ok || steering_analog > max_ok); } -bool SteeringSystem::_evaluate_steering_oor_digital(int steering_digital, +bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital, int min_sensor_value_digital, - int max_sensor_value_digital){ - if(steering_digital < min_sensor_value_digital || steering_digital > max_sensor_value_digital){ - SteeringParams.digital_oor_implausibility = true; - return true; - } - return false; + int max_sensor_value_digital) + { + //check if the digital value is within bounds +- error + + const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg / _steeringParams.deg_per_count); + const uint32_t min_ok = _steeringParams.min_steering_signal_digital - digital_margin_counts; + const uint32_t max_ok = _steeringParams.max_steering_signal_digital + digital_margin_counts; + return (steering_digital < min_ok || steering_digital > max_ok); } bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ - if(dtheta > SteeringParams.max_dtheta_threshold){ - return true; - } - return false; + return (dtheta > _steeringParams.max_dtheta_threshold); } + + float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - uint32_t raw_val = current_steering_data.steering_sensor_analog; // Check your struct for exact name + uint32_t raw_val = current_steering_data.steering_sensor_analog; // Calculate how far we are from the minimum raw count delta - int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; + uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; // Convert that delta into degrees float degrees_delta = (float)count_delta * _steeringParams.deg_per_count; @@ -58,4 +103,103 @@ float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤ return (float)_steeringParams.min_steering_digital + degrees_delta; } -//_steeringParams.deg_per_count +SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis){ + SteeringSystemData_s out = {};// initialize struct to be blank + + + //Conversion from raw ADC to degrees + out.analog_steering_angle = _convert_analog_sensor(current_steering_data); + out.digital_steering_angle = _convert_digital_sensor(current_steering_data); + + uint32_t dt = current_millis - _prev_timestamp; + + if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data + float dtheta_analog = fabsf(out.analog_steering_angle - _prev_analog_angle); + float dtheta_digital = fabsf(out.digital_steering_angle - _prev_digital_angle); + + out.steering_velocity_deg_ms = dtheta_analog / dt; //need to add separate velocity for digital and analog, for now just have analog + out.digital_steering_velocity_deg_ms = dtheta_digital / dt; + + + //Check if either sensor moved too fast + if (_evaluate_steering_dtheta_exceeded(dtheta_analog)){ + out.dtheta_exceeded_analog = true; + } + if (_evaluate_steering_dtheta_exceeded(dtheta_digital)) { + out.dtheta_exceeded_digital = true; + } + + + //Check if either sensor is out of range: + + out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.steering_sensor_analog, _steeringParams.min_steering_signal_analog, _steeringParams.max_steering_signal_analog); + out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.steering_sensor_digital, _steeringParams.min_steering_signal_digital, _steeringParams.max_steering_signal_digital); + + + //Check if there is too much of a difference between sensor values + float difference = fabsf(out.analog_steering_angle - out.digital_steering_angle); + + const float full_scale_deg = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); + const float analog_tol_deg = _steeringParams.analog_tol_deg *full_scale_deg; + const float digital_tol_deg = _steeringParams.digital_tol_deg; + const float allowed_difference = analog_tol_deg+digital_tol_deg; //the allowed difference equals digital error+ analog error + //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% + bool sensors_agree = difference <= allowed_difference; //steeringParams.error + + + + //Still to do: create an algorithm/ checklist to determine which sensor we trust more, + //or, if we should have an algorithm to have a weighted calculation based on both values + bool analog_valid = !out.analog_oor_implausibility && !out.dtheta_exceeded_analog; + bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; + if (analog_valid && digital_valid) + { + //if sensors have acceptable difference, use average as steering angle, otherwise default to analog + if (sensors_agree) + { + out.output_steering_angle = 0.5f * (out.analog_steering_angle + out.digital_steering_angle); + } + else + { + out.output_steering_angle = out.analog_steering_angle; + } + } + else if (analog_valid) + { + out.output_steering_angle = out.analog_steering_angle; + } + else if (digital_valid) { + out.output_steering_angle = out.digital_steering_angle; + } + else // if both sensors fail + { + // alert or something + } + + + + } + //Update states + + _prev_analog_angle = out.analog_steering_angle; + _prev_digital_angle = out.digital_steering_angle; + _prev_timestamp = current_millis; + _first_run = false; + + return out; +} + +void SteeringSystem::update_steering_system() { + _steeringSystemData = evaluate_steering(/* */); +} + + + + + + + + + + + From 2ec44f142cd7b439434d017df3506c053202dc69 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 16 Feb 2026 20:40:56 -0500 Subject: [PATCH 08/39] fixed a few naming bugs --- lib/systems/include/SteeringSystem.h | 20 +++++--------------- lib/systems/src/SteeringSystem.cpp | 4 +--- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 5f2bfbd..a2142c1 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -39,7 +39,7 @@ struct SteeringSystemData_s bool sensor_difference_implausibility; bool digital_oor_implausibility; bool analog_oor_implausibility; -} +}; class SteeringSystem @@ -73,15 +73,9 @@ class SteeringSystem const SteeringSensorData_s &get_steering_sensor_data() const; // Other Functions - void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data); - { - uint32_t analog_raw_value = static_cast(current_steering_data.analog_raw); - uint32_t digital_raw_value = static_cast(current_steering_data.digital_raw); - bool steering_flipped = - - } + void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - void evaluate_steering(const SteeringSensorData_s ¤t_steering_data); + SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); @@ -95,14 +89,10 @@ class SteeringSystem //returns true if steering_analog is outside of the range defined by min and max sensor values - bool _evaluate_steering_oor_analog(int steering_analog, - int min_sensor_value_analog, - int max_sensor_value_analog); + bool _evaluate_steering_oor_analog(uint32_t steering_analog); //returns true if steering_digital is outside the range defined by min and max sensor values - bool _evaluate_steering_oor_digital(int steering_digital, - int min_sensor_value_digital, - int max_sensor_value_digital); + bool _evaluate_steering_oor_digital(uint32_t steering_digital); //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) bool _evaluate_steering_dtheta_exceeded(int steering_analog, diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 7aa5312..6dc64e4 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -54,9 +54,7 @@ bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog) return (steering_analog < min_ok || steering_analog > max_ok); } -bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital, - int min_sensor_value_digital, - int max_sensor_value_digital) +bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital) { //check if the digital value is within bounds +- error From 5b655582e075690229b05dde4369014d6b7da1d9 Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Wed, 18 Feb 2026 19:59:44 -0500 Subject: [PATCH 09/39] files not updated --- lib/systems/include/SteeringSystem.h | 121 +++++++++++++-------------- lib/systems/src/SteeringSystem.cpp | 3 +- 2 files changed, 58 insertions(+), 66 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 5f2bfbd..a8833a0 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -2,6 +2,10 @@ #define STEERING_SYSTEM_H #include +#include +#include +#include + #include "SharedFirmwareTypes.h" @@ -13,12 +17,14 @@ struct SteeringParams_s { uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - uint32_t min_steering_analog; //minimum sensor output *analog 0 - uint32_t max_steering_analog; //maximimum sensor output *analog 4095 - uint32_t min_steering_digital; // maximum sensor output *digital 4095 - uint32_t max_steering_digital; //maximum sensor output *digital0 + uint32_t min_steering_analog = 0; //minimum sensor output *analog 0 + uint32_t max_steering_analog = 4096; //maximimum sensor output *analog 4095 + uint32_t min_steering_digital = 0; // maximum sensor output *digital 4095 + uint32_t max_steering_digital = 4096; //maximum sensor output *digital0 float deg_per_count; //degrees of steering angle per count of the sensor + float analog_tol_deg = 0.005f; //+- 0.5% + float digital_tol_deg = 0.2f; // +- 0.2 degrees float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid @@ -33,13 +39,18 @@ struct SteeringSystemData_s float digital_steering_angle; //in degrees float output_steering_angle; // represents the better output of the two sensors or some combination of the values - float steering_velocity_deg_s; //in degrees per second + float analog_steering_velocity_deg_s; //in degrees per second + float digital_steering_velocity_deg_s; bool digital_sensor_error; bool sensor_difference_implausibility; bool digital_oor_implausibility; bool analog_oor_implausibility; -} + + bool dtheta_exceeded_analog; + bool dtheta_exceeded_digital; + bool analog_digital_disagreement; +}; class SteeringSystem @@ -49,101 +60,81 @@ class SteeringSystem {} // Setters - void set_steering_params(const SteeringParams_s &steeringParams) - { -//parameters + void set_steering_params(const SteeringParams_s &steeringParams) { + _steeringParams = steeringParams; } - - void set_steering_system_data(const SteeringSystemData_s &steeringSystemData); - { - - //output that comes once we've calibrated everything, we output this data + void set_steering_system_data(const SteeringSystemData_s &steeringSystemData) { + _steeringSystemData = steeringSystemData; } - void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData); - //get the current value from the sensor - + void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData) { + _steeringSensorData = steeringSensorData; + } // Getters const SteeringParams_s &get_steering_params() const; - const SteeringSystemData_s &get_steering_system_data() const; + const SteeringSystemData_s &get_steering_system_data() const { + return _steeringSystemData; + } - const SteeringSensorData_s &get_steering_sensor_data() const; + const SteeringSensorData_s &get_steering_sensor_data() const { + return _steeringSensorData; + } // Other Functions - void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data); - { - uint32_t analog_raw_value = static_cast(current_steering_data.analog_raw); - uint32_t digital_raw_value = static_cast(current_steering_data.digital_raw); - bool steering_flipped = + void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - } - - void evaluate_steering(const SteeringSensorData_s ¤t_steering_data); + //must have an input that says recalibration is on, then collect the abs min and max value + //from both sensors to establish a universal min & max + SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); + void update_steering_system(); private: + + //track the state of our system from the previous tick to compare against current state for implausibility checks + float _prev_analog_angle = 0.0f; + float _prev_digital_angle = 0.0f; + uint32_t _prev_timestamp = 0; + bool _calibrating = false; + bool _first_run = true; // skip dTheta check on the very first tick + uint32_t _min_observed_analog; //These are class members so they persist during calibration + uint32_t _max_observed_analog; + uint32_t _min_observed_digital; + uint32_t _max_observed_digital; + + + float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); - + float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); //returns true if steering_analog is outside of the range defined by min and max sensor values - bool _evaluate_steering_oor_analog(int steering_analog, - int min_sensor_value_analog, - int max_sensor_value_analog); - + bool _evaluate_steering_oor_analog(int steering_analog); + //returns true if steering_digital is outside the range defined by min and max sensor values - bool _evaluate_steering_oor_digital(int steering_digital, - int min_sensor_value_digital, - int max_sensor_value_digital); + bool _evaluate_steering_oor_digital(int steering_digital); + //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) - bool _evaluate_steering_dtheta_exceeded(int steering_analog, - int steering_digital, - float dt); + bool _evaluate_steering_dtheta_exceeded(float dtheta); -/* - int32_t _prev_raw_adc = -1; // Initialize to -1 so we know it's the first run - int32_t _rotation_count = 0; // Tracks full 360-degree turns (e.g., -1, 0, 1, 2) - - - const int32_t MAX_ADC_LIMIT = 4095; // The rollover point - const int32_t HALF_ADC_LIMIT = 2048; // Threshold to detect a wrap - - In order to check current total number of rotations (analog goes from max value back down to 0), we need to see if we exceeded a limit between readings - leaving this commented for now because this conflicts with one of the implausibility checks (max change in angle per reading) and we can revisit this if we find a solution -*/ SteeringSensorData_s _steeringSensorData{}; SteeringSystemData_s _steeringSystemData{}; SteeringParams_s _steeringParams{}; - bool _implaus + }; - - - - - - - - - - - - - - - - using SteeringSystemInstance = etl::singleton; #endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 7aa5312..1942fa5 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -190,7 +190,8 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ } void SteeringSystem::update_steering_system() { - _steeringSystemData = evaluate_steering(/* */); + + _steeringSystemData = evaluate_steering(/* real current_steering_data location*/); } From 7bb6485154e47dcdf4b350972bd3e11df0f5589f Mon Sep 17 00:00:00 2001 From: William Schotte Date: Wed, 18 Feb 2026 21:15:49 -0500 Subject: [PATCH 10/39] fixed up some functions --- lib/systems/include/SteeringSystem.h | 29 ++++++------- lib/systems/src/SteeringSystem.cpp | 52 ++++++++++-------------- test/test_systems/steering_system_test.h | 9 ++++ 3 files changed, 46 insertions(+), 44 deletions(-) create mode 100644 test/test_systems/steering_system_test.h diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index a8833a0..936e940 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -16,15 +16,20 @@ struct SteeringParams_s { uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle + uint32_t _min_observed_analog; //These are class members so they persist during calibration + uint32_t _max_observed_analog; + uint32_t _min_observed_digital; + uint32_t _max_observed_digital; - uint32_t min_steering_analog = 0; //minimum sensor output *analog 0 + uint32_t min_steering_analog = 0; //minimum sensor output *analog 0 clarify on the steering turn, associating 0 uint32_t max_steering_analog = 4096; //maximimum sensor output *analog 4095 - uint32_t min_steering_digital = 0; // maximum sensor output *digital 4095 - uint32_t max_steering_digital = 4096; //maximum sensor output *digital0 + uint32_t min_steering_digital; // maximum sensor output *digital 4095 + uint32_t max_steering_digital; //maximum sensor output *digital0 - float deg_per_count; //degrees of steering angle per count of the sensor - float analog_tol_deg = 0.005f; //+- 0.5% - float digital_tol_deg = 0.2f; // +- 0.2 degrees + float deg_per_count_analog = 0.0439f; //hard coded for analog + float deg_per_count_digital; //based on digital readings + float analog_tol_deg = 0.005f; //+- 0.5% error + float digital_tol_deg = 0.2f; // +- 0.2 degrees error float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid @@ -42,7 +47,6 @@ struct SteeringSystemData_s float analog_steering_velocity_deg_s; //in degrees per second float digital_steering_velocity_deg_s; - bool digital_sensor_error; bool sensor_difference_implausibility; bool digital_oor_implausibility; bool analog_oor_implausibility; @@ -84,7 +88,7 @@ class SteeringSystem } // Other Functions - void recalibrate_steering(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); + void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); //must have an input that says recalibration is on, then collect the abs min and max value //from both sensors to establish a universal min & max @@ -103,10 +107,7 @@ class SteeringSystem bool _calibrating = false; bool _first_run = true; // skip dTheta check on the very first tick - uint32_t _min_observed_analog; //These are class members so they persist during calibration - uint32_t _max_observed_analog; - uint32_t _min_observed_digital; - uint32_t _max_observed_digital; + @@ -117,10 +118,10 @@ class SteeringSystem //returns true if steering_analog is outside of the range defined by min and max sensor values - bool _evaluate_steering_oor_analog(int steering_analog); + bool _evaluate_steering_oor_analog(uint32_t steering_analog); //returns true if steering_digital is outside the range defined by min and max sensor values - bool _evaluate_steering_oor_digital(int steering_digital); + bool _evaluate_steering_oor_digital(uint32_t steering_digital); //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index dd35657..da4cb37 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -2,36 +2,28 @@ #include "SteeringSystem.h" -void SteeringSystem::recalibrate_steering(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on){ +void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on){ //get current raw angles - const uint32_t analog_raw = static_cast(current_steering_data.steering_sensor_analog); - const uint32_t digital_raw = static_cast(current_steering_data.steering_sensor_digital); - // _update_observed_limits(analog_raw, digital_raw); + const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; - _min_observed_analog = 4096; - _max_observed_analog = 0; - _min_observed_digital = 4096; - _max_observed_digital = 0; + _steeringParams._min_observed_digital = 4096; + _steeringParams._max_observed_digital = 0; } //update relative extremeties if (calibration_is_on){ - _min_observed_analog = std::min(_min_observed_analog, analog_raw); - _max_observed_analog = std::max(_max_observed_analog, analog_raw); - _min_observed_digital = std::min(_min_observed_digital, digital_raw); - _max_observed_digital = std::max(_max_observed_digital, digital_raw); + _steeringParams.min_observed_digital = std::min(_min_observed_digital, curr_digital_raw); + _steeringParams._max_observed_digital = std::max(_max_observed_digital, curr_digital_raw); return; } //button released ->commit the values if (!calibration_is_on && _calibrating) { _calibrating = false; - _steeringParams.min_steering_signal_analog = _min_observed_analog; - _steeringParams.max_steering_signal_analog = _max_observed_analog; - _steeringParams.min_steering_signal_digital = _min_observed_digital; - _steeringParams.max_steering_signal_digital = _max_observed_digital; + _steeringParams.min_steering_signal_digital = _steeringParams._min_observed_digital; + _steeringParams.max_steering_signal_digital = _steeringParams._max_observed_digital; } // swaps min & max in the params if sensor is negative if (_steeringParams.min_steering_signal_analog > _steeringParams.max_steering_signal_analog) { @@ -76,7 +68,7 @@ bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - uint32_t raw_val = current_steering_data.steering_sensor_analog; + uint32_t raw_val = current_steering_data.analog_steering_degrees; // Calculate how far we are from the minimum raw count delta uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; @@ -92,7 +84,7 @@ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { // Same logic for digital - uint32_t raw_val = current_steering_data.steering_sensor_digital; + uint32_t raw_val = current_steering_data.digital_steering_analog; int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_digital; @@ -115,11 +107,11 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ float dtheta_analog = fabsf(out.analog_steering_angle - _prev_analog_angle); float dtheta_digital = fabsf(out.digital_steering_angle - _prev_digital_angle); - out.steering_velocity_deg_ms = dtheta_analog / dt; //need to add separate velocity for digital and analog, for now just have analog - out.digital_steering_velocity_deg_ms = dtheta_digital / dt; + out.analog_steering_velocity_deg_s = dtheta_analog / dt; + out.digital_steering_velocity_deg_s = dtheta_digital / dt; - //Check if either sensor moved too fast + //Check if either sensor moved too much in one tick if (_evaluate_steering_dtheta_exceeded(dtheta_analog)){ out.dtheta_exceeded_analog = true; } @@ -128,10 +120,10 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ } - //Check if either sensor is out of range: + //Check if either sensor is out of range - out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.steering_sensor_analog, _steeringParams.min_steering_signal_analog, _steeringParams.max_steering_signal_analog); - out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.steering_sensor_digital, _steeringParams.min_steering_signal_digital, _steeringParams.max_steering_signal_digital); + out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.analog_steering_degrees); + out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.digital_steering_analog); //Check if there is too much of a difference between sensor values @@ -152,14 +144,14 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; if (analog_valid && digital_valid) { - //if sensors have acceptable difference, use average as steering angle, otherwise default to analog + //if sensors have acceptable difference, use average as steering angle, otherwise default to digital if (sensors_agree) { out.output_steering_angle = 0.5f * (out.analog_steering_angle + out.digital_steering_angle); } else { - out.output_steering_angle = out.analog_steering_angle; + out.output_steering_angle = out.digital_steering_angle;//default to original, but we need to consider what we really want to put here } } else if (analog_valid) @@ -171,7 +163,7 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ } else // if both sensors fail { - // alert or something + out.analog_digital_disagreement = true; } @@ -187,10 +179,10 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ return out; } -void SteeringSystem::update_steering_system() { +// void SteeringSystem::update_steering_system() { - _steeringSystemData = evaluate_steering(/* real current_steering_data location*/); -} +// _steeringSystemData = evaluate_steering(/* real current_steering_data location*/); +// } diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h new file mode 100644 index 0000000..70abe62 --- /dev/null +++ b/test/test_systems/steering_system_test.h @@ -0,0 +1,9 @@ +#define STEERING_SYSTEM_TEST +#include +#include +#include "SteeringSystem.h" +#include "SharedFirmwareTypes.h" +#include + +#include + From 4afe0f3c5dd93a91b8528c7dd3a2ddcb08db1435 Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Sun, 22 Feb 2026 18:04:31 -0500 Subject: [PATCH 11/39] minor bug fixes before meet --- lib/systems/include/SteeringSystem.h | 14 ++++++--- lib/systems/src/SteeringSystem.cpp | 43 +++++++++++++++++----------- 2 files changed, 37 insertions(+), 20 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 936e940..ade5522 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -11,28 +11,34 @@ struct SteeringParams_s { + //raw ADC input signals uint32_t min_steering_signal_analog; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) uint32_t max_steering_signal_analog; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - uint32_t _min_observed_analog; //These are class members so they persist during calibration - uint32_t _max_observed_analog; + //calibration limits: only digital is recalibrated uint32_t _min_observed_digital; uint32_t _max_observed_digital; - + + //driving limits: post calibration uint32_t min_steering_analog = 0; //minimum sensor output *analog 0 clarify on the steering turn, associating 0 uint32_t max_steering_analog = 4096; //maximimum sensor output *analog 4095 uint32_t min_steering_digital; // maximum sensor output *digital 4095 uint32_t max_steering_digital; //maximum sensor output *digital0 + //conversion rates float deg_per_count_analog = 0.0439f; //hard coded for analog float deg_per_count_digital; //based on digital readings + + //implausability values float analog_tol_deg = 0.005f; //+- 0.5% error float digital_tol_deg = 0.2f; // +- 0.2 degrees error - + //rate of angle change float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid + + //difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed }; diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index da4cb37..738aced 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -9,14 +9,14 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; - _steeringParams._min_observed_digital = 4096; + _steeringParams._min_observed_digital = 10000000; //establishes a big number that will be greater than the readings _steeringParams._max_observed_digital = 0; } //update relative extremeties if (calibration_is_on){ - _steeringParams.min_observed_digital = std::min(_min_observed_digital, curr_digital_raw); - _steeringParams._max_observed_digital = std::max(_max_observed_digital, curr_digital_raw); + _steeringParams._min_observed_digital = std::min(_steeringParams._min_observed_digital, curr_digital_raw); + _steeringParams._max_observed_digital = std::max(_steeringParams._max_observed_digital, curr_digital_raw); return; } //button released ->commit the values @@ -24,14 +24,15 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu _calibrating = false; _steeringParams.min_steering_signal_digital = _steeringParams._min_observed_digital; _steeringParams.max_steering_signal_digital = _steeringParams._max_observed_digital; + return; } - // swaps min & max in the params if sensor is negative - if (_steeringParams.min_steering_signal_analog > _steeringParams.max_steering_signal_analog) { - std::swap(_steeringParams.min_steering_signal_analog, _steeringParams.max_steering_signal_analog); - } + // swaps min & max in the params if sensor is flipped + + if (_steeringParams.min_steering_signal_digital > _steeringParams.max_steering_signal_digital) { std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); + return; } @@ -50,7 +51,7 @@ bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital) { //check if the digital value is within bounds +- error - const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg / _steeringParams.deg_per_count); + const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg); const uint32_t min_ok = _steeringParams.min_steering_signal_digital - digital_margin_counts; const uint32_t max_ok = _steeringParams.max_steering_signal_digital + digital_margin_counts; return (steering_digital < min_ok || steering_digital > max_ok); @@ -68,29 +69,39 @@ bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - uint32_t raw_val = current_steering_data.analog_steering_degrees; + const uint32_t raw_val = current_steering_data.analog_steering_degrees; //is this adc input value? + + constexpr float kMindeg = -90.0f; // Calculate how far we are from the minimum raw count delta - uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; + const uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; // Convert that delta into degrees - float degrees_delta = (float)count_delta * _steeringParams.deg_per_count; + const float degrees_delta = count_delta * _steeringParams.deg_per_count_analog; // Add to the starting minimum angle to get absolute position - return (float)_steeringParams.min_steering_analog + degrees_delta; + return kMindeg + degrees_delta; } float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { // Same logic for digital - uint32_t raw_val = current_steering_data.digital_steering_analog; + const uint32_t raw_val = current_steering_data.digital_steering_analog; //adc value + //create a span for the digital sensor + const uint32_t span_digital = static_cast(_steeringParams.max_steering_digital) - static_cast(_steeringParams.min_steering_digital); + + //degree span we are assuming: + constexpr float kMindeg = -90.0f; + constexpr float kSpan = 180.0f; - int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_digital; + _steeringParams.deg_per_count_digital = kSpan/(float)span_digital; - float degrees_delta = (float)count_delta * _steeringParams.deg_per_count; + const int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_digital; - return (float)_steeringParams.min_steering_digital + degrees_delta; + float degrees_delta = (float)count_delta * _steeringParams.deg_per_count_digital; + //output in degreees_delta + return kMindeg + degrees_delta; } SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis){ From 954961a805e8256617ea41929cf997a2efc9aca7 Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Sun, 22 Feb 2026 20:02:32 -0500 Subject: [PATCH 12/39] basic cleanup and revising functions --- lib/systems/include/SteeringSystem.h | 17 ++---- lib/systems/src/SteeringSystem.cpp | 83 +++++++++++++--------------- 2 files changed, 43 insertions(+), 57 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index ade5522..9715dc9 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -12,24 +12,18 @@ struct SteeringParams_s { //raw ADC input signals - uint32_t min_steering_signal_analog; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) - uint32_t max_steering_signal_analog; //Raw ADC value from analog sensor at maximum (right) steering angle + uint32_t min_steering_signal_analog = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) + uint32_t max_steering_signal_analog = 4096; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - + //calibration limits: only digital is recalibrated uint32_t _min_observed_digital; uint32_t _max_observed_digital; - //driving limits: post calibration - uint32_t min_steering_analog = 0; //minimum sensor output *analog 0 clarify on the steering turn, associating 0 - uint32_t max_steering_analog = 4096; //maximimum sensor output *analog 4095 - uint32_t min_steering_digital; // maximum sensor output *digital 4095 - uint32_t max_steering_digital; //maximum sensor output *digital0 - //conversion rates float deg_per_count_analog = 0.0439f; //hard coded for analog - float deg_per_count_digital; //based on digital readings + float deg_per_count_digital = 0.021973997f; //based on digital readings //implausability values float analog_tol_deg = 0.005f; //+- 0.5% error @@ -100,9 +94,6 @@ class SteeringSystem //from both sensors to establish a universal min & max SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); - - void update_steering_system(); - private: diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 738aced..70894cb 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -34,27 +34,27 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); return; } - - + _steeringParams.deg_per_count_digital = (180.0f)/(_steeringParams.max_steering_signal_digital - _steeringParams.min_steering_signal_digital); //assigns value for deg_per_ct } -bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog) + +bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_deg) // CONVERTED { //check if the analog value is within bounds +- error const uint32_t span = _steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog; const uint32_t analog_margin_counts = static_cast(_steeringParams.analog_tol_deg * span); const uint32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; const uint32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; - return (steering_analog < min_ok || steering_analog > max_ok); + return (steering_analog_deg < min_ok || steering_analog_deg > max_ok); } -bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital) +bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_deg) // CONVERTED { //check if the digital value is within bounds +- error - const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg); - const uint32_t min_ok = _steeringParams.min_steering_signal_digital - digital_margin_counts; - const uint32_t max_ok = _steeringParams.max_steering_signal_digital + digital_margin_counts; - return (steering_digital < min_ok || steering_digital > max_ok); + // const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); + const uint32_t min_ok = _steeringParams.min_steering_signal_digital - _steeringParams.digital_tol_deg; + const uint32_t max_ok = _steeringParams.max_steering_signal_digital + _steeringParams.digital_tol_deg; + return (steering_digital_deg < min_ok || steering_digital_deg > max_ok); } bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ @@ -62,25 +62,25 @@ bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ } - - - - float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - const uint32_t raw_val = current_steering_data.analog_steering_degrees; //is this adc input value? - - constexpr float kMindeg = -90.0f; + const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //is this adc input value? + const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; + + const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); + + const float curr_analog_degrees = curr_analog_offset * _steeringParams.deg_per_count_analog; + return curr_analog_degrees; // Calculate how far we are from the minimum raw count delta - const uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; + // const uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; - // Convert that delta into degrees - const float degrees_delta = count_delta * _steeringParams.deg_per_count_analog; + // // Convert that delta into degrees + // const float degrees_delta = count_delta * _steeringParams.deg_per_count_analog; - // Add to the starting minimum angle to get absolute position - return kMindeg + degrees_delta; + // // Add to the starting minimum angle to get absolute position + // return kMindeg + degrees_delta; } @@ -89,33 +89,34 @@ float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤ // Same logic for digital const uint32_t raw_val = current_steering_data.digital_steering_analog; //adc value //create a span for the digital sensor - const uint32_t span_digital = static_cast(_steeringParams.max_steering_digital) - static_cast(_steeringParams.min_steering_digital); + const uint32_t span_digital = static_cast(_steeringParams.max_steering_signal_digital) - static_cast(_steeringParams.min_steering_signal_digital); //degree span we are assuming: - constexpr float kMindeg = -90.0f; constexpr float kSpan = 180.0f; - _steeringParams.deg_per_count_digital = kSpan/(float)span_digital; - const int32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_digital; + const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2.0f)*_steeringParams.deg_per_count_digital; + + const int32_t count_delta = (int32_t)raw_val - (int32_t)digital_midpoint; float degrees_delta = (float)count_delta * _steeringParams.deg_per_count_digital; //output in degreees_delta - return kMindeg + degrees_delta; + return digital_midpoint + degrees_delta; } SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis){ SteeringSystemData_s out = {};// initialize struct to be blank - + const uint32_t curr_analog_deg = _convert_analog_sensor(current_steering_data); + const uint32_t curr_digital_deg = _convert_digital_sensor(current_steering_data); //Conversion from raw ADC to degrees - out.analog_steering_angle = _convert_analog_sensor(current_steering_data); - out.digital_steering_angle = _convert_digital_sensor(current_steering_data); + out.analog_steering_angle = curr_analog_deg; + out.digital_steering_angle = curr_digital_deg; - uint32_t dt = current_millis - _prev_timestamp; + uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data - float dtheta_analog = fabsf(out.analog_steering_angle - _prev_analog_angle); + float dtheta_analog = fabsf(out.analog_steering_angle - _prev_analog_angle); //prev_angle established in last run float dtheta_digital = fabsf(out.digital_steering_angle - _prev_digital_angle); out.analog_steering_velocity_deg_s = dtheta_analog / dt; @@ -133,17 +134,16 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ //Check if either sensor is out of range - out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.analog_steering_degrees); - out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.digital_steering_analog); + out.analog_oor_implausibility = _evaluate_steering_oor_analog(curr_analog_deg); + out.digital_oor_implausibility = _evaluate_steering_oor_digital(curr_digital_deg); //Check if there is too much of a difference between sensor values float difference = fabsf(out.analog_steering_angle - out.digital_steering_angle); - const float full_scale_deg = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); - const float analog_tol_deg = _steeringParams.analog_tol_deg *full_scale_deg; - const float digital_tol_deg = _steeringParams.digital_tol_deg; - const float allowed_difference = analog_tol_deg+digital_tol_deg; //the allowed difference equals digital error+ analog error + const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc + const float analog_tol_deg = _steeringParams.analog_tol_deg*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count + const float allowed_difference = analog_tol_deg+ _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% bool sensors_agree = difference <= allowed_difference; //steeringParams.error @@ -158,11 +158,11 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ //if sensors have acceptable difference, use average as steering angle, otherwise default to digital if (sensors_agree) { - out.output_steering_angle = 0.5f * (out.analog_steering_angle + out.digital_steering_angle); + out.output_steering_angle = out.digital_steering_angle; } else { - out.output_steering_angle = out.digital_steering_angle;//default to original, but we need to consider what we really want to put here + out.output_steering_angle = out.analog_steering_angle;//default to original, but we need to consider what we really want to put here } } else if (analog_valid) @@ -190,11 +190,6 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ return out; } -// void SteeringSystem::update_steering_system() { - -// _steeringSystemData = evaluate_steering(/* real current_steering_data location*/); -// } - From 330e03aeed1ea76c8bf8ddfba3f4399749eeb61d Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 23 Feb 2026 19:17:18 -0500 Subject: [PATCH 13/39] current version, bug fixes --- lib/systems/include/SteeringSystem.h | 78 +++++++++++++----- lib/systems/src/SteeringSystem.cpp | 116 ++++++++++++++------------- 2 files changed, 121 insertions(+), 73 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 9715dc9..ce527ea 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -2,129 +2,167 @@ #define STEERING_SYSTEM_H #include + #include #include #include + #include "SharedFirmwareTypes.h" + + + struct SteeringParams_s { //raw ADC input signals uint32_t min_steering_signal_analog = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) uint32_t max_steering_signal_analog = 4096; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - + uint32_t span_signal_analog = 4096; + uint32_t span_signal_digital; + + //calibration limits: only digital is recalibrated uint32_t _min_observed_digital; uint32_t _max_observed_digital; + //conversion rates - float deg_per_count_analog = 0.0439f; //hard coded for analog - float deg_per_count_digital = 0.021973997f; //based on digital readings + // float deg_per_count_analog = 0.0439f; //hard coded for analog (180) + float deg_per_count_analog = 0.087890625f; + float deg_per_count_digital = 0.02197265625f; //based on digital readings + //implausability values - float analog_tol_deg = 0.005f; //+- 0.5% error + float analog_tol = 0.005f; //+- 0.5% error + float analog_tol_deg; float digital_tol_deg = 0.2f; // +- 0.2 degrees error - + //rate of angle change float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid + //difference rating - float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed - + float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed + }; -struct SteeringSystemData_s + + +struct SteeringSystemData_s { float analog_steering_angle; //in degrees float digital_steering_angle; //in degrees float output_steering_angle; // represents the better output of the two sensors or some combination of the values + float analog_steering_velocity_deg_s; //in degrees per second float digital_steering_velocity_deg_s; - bool sensor_difference_implausibility; + bool digital_oor_implausibility; bool analog_oor_implausibility; + bool dtheta_exceeded_analog; bool dtheta_exceeded_digital; bool analog_digital_disagreement; }; -class SteeringSystem + + +class SteeringSystem { public: SteeringSystem(const SteeringParams_s &steeringParams) : _steeringParams(steeringParams) {} + // Setters void set_steering_params(const SteeringParams_s &steeringParams) { _steeringParams = steeringParams; } + void set_steering_system_data(const SteeringSystemData_s &steeringSystemData) { _steeringSystemData = steeringSystemData; } + void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData) { _steeringSensorData = steeringSensorData; } + // Getters const SteeringParams_s &get_steering_params() const; - + const SteeringSystemData_s &get_steering_system_data() const { return _steeringSystemData; } + const SteeringSensorData_s &get_steering_sensor_data() const { return _steeringSensorData; } + // Other Functions void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - //must have an input that says recalibration is on, then collect the abs min and max value + + //must have an input that says recalibration is on, then collect the abs min and max value //from both sensors to establish a universal min & max - + SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); + private: + //track the state of our system from the previous tick to compare against current state for implausibility checks float _prev_analog_angle = 0.0f; float _prev_digital_angle = 0.0f; - uint32_t _prev_timestamp = 0; + uint32_t _prev_timestamp = 0; bool _calibrating = false; bool _first_run = true; // skip dTheta check on the very first tick - + + + + float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); - + float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); - + + + //returns true if steering_analog is outside of the range defined by min and max sensor values bool _evaluate_steering_oor_analog(uint32_t steering_analog); - + //returns true if steering_digital is outside the range defined by min and max sensor values bool _evaluate_steering_oor_digital(uint32_t steering_digital); + + //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) bool _evaluate_steering_dtheta_exceeded(float dtheta); - + + + + SteeringSensorData_s _steeringSensorData{}; @@ -133,6 +171,8 @@ class SteeringSystem }; + using SteeringSystemInstance = etl::singleton; + #endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 70894cb..e380683 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,18 +1,20 @@ -#include +#include #include "SteeringSystem.h" + + void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on){ //get current raw angles const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); - + //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; _steeringParams._min_observed_digital = 10000000; //establishes a big number that will be greater than the readings _steeringParams._max_observed_digital = 0; } - + //update relative extremeties if (calibration_is_on){ _steeringParams._min_observed_digital = std::min(_steeringParams._min_observed_digital, curr_digital_raw); @@ -28,112 +30,120 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } // swaps min & max in the params if sensor is flipped - + + if (_steeringParams.min_steering_signal_digital > _steeringParams.max_steering_signal_digital) { std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); return; } - _steeringParams.deg_per_count_digital = (180.0f)/(_steeringParams.max_steering_signal_digital - _steeringParams.min_steering_signal_digital); //assigns value for deg_per_ct + _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital; + } -bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_deg) // CONVERTED + +bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) // RAW { //check if the analog value is within bounds +- error - const uint32_t span = _steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog; - const uint32_t analog_margin_counts = static_cast(_steeringParams.analog_tol_deg * span); + const uint32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * _steeringParams.span_signal_analog); const uint32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; const uint32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; - return (steering_analog_deg < min_ok || steering_analog_deg > max_ok); + return (steering_analog_raw < min_ok || steering_analog_raw > max_ok); } + bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_deg) // CONVERTED { //check if the digital value is within bounds +- error - + // const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); const uint32_t min_ok = _steeringParams.min_steering_signal_digital - _steeringParams.digital_tol_deg; const uint32_t max_ok = _steeringParams.max_steering_signal_digital + _steeringParams.digital_tol_deg; return (steering_digital_deg < min_ok || steering_digital_deg > max_ok); } + bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ - return (dtheta > _steeringParams.max_dtheta_threshold); + return (abs(dtheta) > _steeringParams.max_dtheta_threshold); } + + float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //is this adc input value? const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; - + const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); - + const float curr_analog_degrees = curr_analog_offset * _steeringParams.deg_per_count_analog; + return curr_analog_degrees; // Calculate how far we are from the minimum raw count delta // const uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; + // // Convert that delta into degrees // const float degrees_delta = count_delta * _steeringParams.deg_per_count_analog; + // // Add to the starting minimum angle to get absolute position // return kMindeg + degrees_delta; } + + float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { // Same logic for digital - const uint32_t raw_val = current_steering_data.digital_steering_analog; //adc value + const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //adc value //create a span for the digital sensor - const uint32_t span_digital = static_cast(_steeringParams.max_steering_signal_digital) - static_cast(_steeringParams.min_steering_signal_digital); + const uint32_t span_digital = _steeringParams.max_steering_signal_digital - _steeringParams.min_steering_signal_digital; - //degree span we are assuming: - constexpr float kSpan = 180.0f; - _steeringParams.deg_per_count_digital = kSpan/(float)span_digital; - const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2.0f)*_steeringParams.deg_per_count_digital; + //degree span we are assuming: + //constexpr float kSpan = 180.0f; + //_steeringParams.deg_per_count_digital = kSpan/(float)span_digital; -> we are assuming deg_per_count hardcoded from datasheet + const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2); - const int32_t count_delta = (int32_t)raw_val - (int32_t)digital_midpoint; - float degrees_delta = (float)count_delta * _steeringParams.deg_per_count_digital; - //output in degreees_delta - return digital_midpoint + degrees_delta; + const int32_t curr_digital_offset = static_cast(curr_raw_digital) - static_cast(digital_midpoint); + float curr_digital_degrees = (float)curr_digital_offset * _steeringParams.deg_per_count_digital; + return ; } -SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis){ + +SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ SteeringSystemData_s out = {};// initialize struct to be blank + const uint32_t curr_analog_deg = _convert_analog_sensor(current_steering_data); const uint32_t curr_digital_deg = _convert_digital_sensor(current_steering_data); //Conversion from raw ADC to degrees out.analog_steering_angle = curr_analog_deg; out.digital_steering_angle = curr_digital_deg; - + + uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input - if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data - float dtheta_analog = fabsf(out.analog_steering_angle - _prev_analog_angle); //prev_angle established in last run - float dtheta_digital = fabsf(out.digital_steering_angle - _prev_digital_angle); - out.analog_steering_velocity_deg_s = dtheta_analog / dt; + if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data + float dtheta_analog = curr_analog_deg - _prev_analog_angle; //prev_angle established in last run + float dtheta_digital = curr_digital_deg - _prev_digital_angle; + out.analog_steering_velocity_deg_s = dtheta_analog / dt; out.digital_steering_velocity_deg_s = dtheta_digital / dt; //Check if either sensor moved too much in one tick - if (_evaluate_steering_dtheta_exceeded(dtheta_analog)){ - out.dtheta_exceeded_analog = true; - } - if (_evaluate_steering_dtheta_exceeded(dtheta_digital)) { - out.dtheta_exceeded_digital = true; - } + out.dtheta_exceeded_analog = _evaluate_steering_dtheta_exceeded(dtheta_analog); + out.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); //Check if either sensor is out of range - out.analog_oor_implausibility = _evaluate_steering_oor_analog(curr_analog_deg); out.digital_oor_implausibility = _evaluate_steering_oor_digital(curr_digital_deg); @@ -141,32 +151,34 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ //Check if there is too much of a difference between sensor values float difference = fabsf(out.analog_steering_angle - out.digital_steering_angle); + const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc - const float analog_tol_deg = _steeringParams.analog_tol_deg*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count - const float allowed_difference = analog_tol_deg+ _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error + const float analog_tol = _steeringParams.analog_tol*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count + const float allowed_difference = analog_tol + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% bool sensors_agree = difference <= allowed_difference; //steeringParams.error - - + + + //Still to do: create an algorithm/ checklist to determine which sensor we trust more, //or, if we should have an algorithm to have a weighted calculation based on both values bool analog_valid = !out.analog_oor_implausibility && !out.dtheta_exceeded_analog; bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; - if (analog_valid && digital_valid) + if (analog_valid && digital_valid) { //if sensors have acceptable difference, use average as steering angle, otherwise default to digital - if (sensors_agree) + if (sensors_agree) { out.output_steering_angle = out.digital_steering_angle; } - else + else { out.output_steering_angle = out.analog_steering_angle;//default to original, but we need to consider what we really want to put here } } - else if (analog_valid) - { + else if (analog_valid) + { out.output_steering_angle = out.analog_steering_angle; } else if (digital_valid) { @@ -177,26 +189,22 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ out.analog_digital_disagreement = true; } - - + + + } //Update states + _prev_analog_angle = out.analog_steering_angle; _prev_digital_angle = out.digital_steering_angle; _prev_timestamp = current_millis; _first_run = false; + return out; } - - - - - - - From 64ae5a5071b864f803fa220f15949fe843285147 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 23 Feb 2026 21:19:48 -0500 Subject: [PATCH 14/39] more bug fixing and edited calibration slightly --- lib/systems/include/SteeringSystem.h | 12 +- lib/systems/src/SteeringSystem.cpp | 158 ++++++++++++----------- test/test_systems/steering_system_test.h | 45 +++++++ 3 files changed, 135 insertions(+), 80 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index ce527ea..af6808b 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -18,7 +18,7 @@ struct SteeringParams_s { //raw ADC input signals uint32_t min_steering_signal_analog = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) - uint32_t max_steering_signal_analog = 4096; //Raw ADC value from analog sensor at maximum (right) steering angle + uint32_t max_steering_signal_analog = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle uint32_t span_signal_analog = 4096; @@ -26,8 +26,8 @@ struct SteeringParams_s { //calibration limits: only digital is recalibrated - uint32_t _min_observed_digital; - uint32_t _max_observed_digital; + uint32_t min_observed_digital; + uint32_t max_observed_digital; //conversion rates @@ -66,11 +66,11 @@ struct SteeringSystemData_s bool digital_oor_implausibility; bool analog_oor_implausibility; - + bool sensor_disagreement_implausibility; bool dtheta_exceeded_analog; bool dtheta_exceeded_digital; - bool analog_digital_disagreement; + bool both_sensors_fail; }; @@ -137,7 +137,7 @@ class SteeringSystem - + void update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data); float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index e380683..18f4251 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -4,67 +4,70 @@ -void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on){ +void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on) { //get current raw angles const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; - _steeringParams._min_observed_digital = 10000000; //establishes a big number that will be greater than the readings - _steeringParams._max_observed_digital = 0; + _steeringParams.min_observed_digital = 10000000; //establishes a big number that will be greater than the readings + _steeringParams.max_observed_digital = 0; } - - //update relative extremeties - if (calibration_is_on){ - _steeringParams._min_observed_digital = std::min(_steeringParams._min_observed_digital, curr_digital_raw); - _steeringParams._max_observed_digital = std::max(_steeringParams._max_observed_digital, curr_digital_raw); - return; + + if (calibration_is_on && _calibrating) { + update_observed_steering_limits(current_steering_data); } + //update relative extremeties + // assigning min and max observed during calibration only vs. constantly? + // if (calibration_is_on){ + // _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, curr_digital_raw); + // _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, curr_digital_raw); + // return; + // } + //button released ->commit the values if (!calibration_is_on && _calibrating) { + _calibrating = false; - _steeringParams.min_steering_signal_digital = _steeringParams._min_observed_digital; - _steeringParams.max_steering_signal_digital = _steeringParams._max_observed_digital; - return; - } + _steeringParams.min_steering_signal_digital = _steeringParams.min_observed_digital; + _steeringParams.max_steering_signal_digital = _steeringParams.max_observed_digital; // swaps min & max in the params if sensor is flipped - - - - if (_steeringParams.min_steering_signal_digital > _steeringParams.max_steering_signal_digital) - { - std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); - return; - } - _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital; - + if (_steeringParams.min_steering_signal_digital > _steeringParams.max_steering_signal_digital) + { + std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); + } + _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital; + _steeringParams.analog_tol_deg = _steeringParams.span_signal_analog * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog; + } } bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) // RAW { + //check if the analog value is within bounds +- error - const uint32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * _steeringParams.span_signal_analog); - const uint32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; - const uint32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; - return (steering_analog_raw < min_ok || steering_analog_raw > max_ok); + const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * _steeringParams.span_signal_analog); + const int32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; + const int32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; + return (static_cast(steering_analog_raw) < min_ok || static_cast(steering_analog_raw) > max_ok); + } -bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_deg) // CONVERTED +bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_raw) // RAW { //check if the digital value is within bounds +- error - - // const uint32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); - const uint32_t min_ok = _steeringParams.min_steering_signal_digital - _steeringParams.digital_tol_deg; - const uint32_t max_ok = _steeringParams.max_steering_signal_digital + _steeringParams.digital_tol_deg; - return (steering_digital_deg < min_ok || steering_digital_deg > max_ok); + + const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); + const int32_t min_ok = _steeringParams.min_steering_signal_digital - digital_margin_counts; + const int32_t max_ok = _steeringParams.max_steering_signal_digital + digital_margin_counts; + return (static_cast(steering_digital_raw) < min_ok || static_cast(steering_digital_raw) > max_ok); } bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ - return (abs(dtheta) > _steeringParams.max_dtheta_threshold); + return (fabs(dtheta) > _steeringParams.max_dtheta_threshold); } @@ -73,7 +76,7 @@ bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //is this adc input value? + const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); @@ -102,40 +105,50 @@ float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤ // Same logic for digital const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //adc value //create a span for the digital sensor - const uint32_t span_digital = _steeringParams.max_steering_signal_digital - _steeringParams.min_steering_signal_digital; //degree span we are assuming: - //constexpr float kSpan = 180.0f; //_steeringParams.deg_per_count_digital = kSpan/(float)span_digital; -> we are assuming deg_per_count hardcoded from datasheet const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2); const int32_t curr_digital_offset = static_cast(curr_raw_digital) - static_cast(digital_midpoint); float curr_digital_degrees = (float)curr_digital_offset * _steeringParams.deg_per_count_digital; - return ; + return curr_digital_degrees; } + +void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { + _steeringParams.min_observed_digital = std::fmin(_steeringParams.min_observed_digital, current_steering_data.digital_steering_analog); + _steeringParams.max_observed_digital = std::fmax(_steeringParams.max_observed_digital, current_steering_data.digital_steering_analog); +} + SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ + SteeringSystemData_s out = {};// initialize struct to be blank + // Reset flags + out.digital_oor_implausibility = false; + out.analog_oor_implausibility = false; + out.sensor_disagreement_implausibility = false; + out.dtheta_exceeded_analog = false; + out.dtheta_exceeded_digital = false; + out.both_sensors_fail = false; - const uint32_t curr_analog_deg = _convert_analog_sensor(current_steering_data); - const uint32_t curr_digital_deg = _convert_digital_sensor(current_steering_data); //Conversion from raw ADC to degrees - out.analog_steering_angle = curr_analog_deg; - out.digital_steering_angle = curr_digital_deg; - + out.analog_steering_angle = _convert_analog_sensor(current_steering_data); + out.digital_steering_angle = _convert_digital_sensor(current_steering_data); + uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data - float dtheta_analog = curr_analog_deg - _prev_analog_angle; //prev_angle established in last run - float dtheta_digital = curr_digital_deg - _prev_digital_angle; - out.analog_steering_velocity_deg_s = dtheta_analog / dt; - out.digital_steering_velocity_deg_s = dtheta_digital / dt; + float dtheta_analog = out.analog_steering_angle - _prev_analog_angle; //prev_angle established in last run + float dtheta_digital = out.digital_steering_angle - _prev_digital_angle; + out.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; + out.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; //Check if either sensor moved too much in one tick @@ -143,38 +156,37 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ out.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); - //Check if either sensor is out of range - out.analog_oor_implausibility = _evaluate_steering_oor_analog(curr_analog_deg); - out.digital_oor_implausibility = _evaluate_steering_oor_digital(curr_digital_deg); - + //Check if either sensor is out of range (pass in raw) + out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.analog_steering_degrees); + out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.digital_steering_analog); //Check if there is too much of a difference between sensor values - float difference = fabsf(out.analog_steering_angle - out.digital_steering_angle); + float sensor_difference = fabs(out.analog_steering_angle - out.digital_steering_angle); - - const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc - const float analog_tol = _steeringParams.analog_tol*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count - const float allowed_difference = analog_tol + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error + // const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc + // const float analog_tol = _steeringParams.analog_tol*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count + const float allowed_difference = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% - bool sensors_agree = difference <= allowed_difference; //steeringParams.error - + bool sensors_agree = (sensor_difference <= allowed_difference); //steeringParams.error + out.sensor_disagreement_implausibility = !sensors_agree; - - - //Still to do: create an algorithm/ checklist to determine which sensor we trust more, + + + //create an algorithm/ checklist to determine which sensor we trust more, //or, if we should have an algorithm to have a weighted calculation based on both values bool analog_valid = !out.analog_oor_implausibility && !out.dtheta_exceeded_analog; bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; + if (analog_valid && digital_valid) { - //if sensors have acceptable difference, use average as steering angle, otherwise default to digital + //if sensors have acceptable difference, use digital as steering angle if (sensors_agree) { out.output_steering_angle = out.digital_steering_angle; } else { - out.output_steering_angle = out.analog_steering_angle;//default to original, but we need to consider what we really want to put here + out.output_steering_angle = out.digital_steering_angle; //default to original, but we need to consider what we really want to put here } } else if (analog_valid) @@ -186,23 +198,21 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ } else // if both sensors fail { - out.analog_digital_disagreement = true; + out.output_steering_angle = _prev_digital_angle; + out.both_sensors_fail = true; } - - - - } //Update states - _prev_analog_angle = out.analog_steering_angle; - _prev_digital_angle = out.digital_steering_angle; - _prev_timestamp = current_millis; - _first_run = false; - + _prev_analog_angle = out.analog_steering_angle; + _prev_digital_angle = out.digital_steering_angle; + _prev_timestamp = current_millis; + _first_run = false; + - return out; + return out; + } diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 70abe62..f210fdc 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -7,3 +7,48 @@ #include +SteeringParams_s gen_analog_and_digital_params(){ + SteeringParams_s params; + //hard code the parmas for sensors + params.min_steering_signal_analog = 0; + params.max_steering_signal_digital = 4096;//actual hard coded + params.min_steering_signal_digital = 0; + params.max_steering_signal_digital = 8000; //testing values + + params.analog_tol_deg = 0.11377778f; + params.digital_tol_deg = 0.2f; + params.max_dtheta_threshold = 5;//change + params.error_between_sensors_tolerance = 0.31377778f; + return params; +} + + + + +void debug_print_steering(const SteeringSystemData_s& data){ + std::cout<<"analog_steering_angle: "< Date: Wed, 25 Feb 2026 20:06:56 -0500 Subject: [PATCH 15/39] work on unit test and fix unsafe conversions --- lib/systems/src/SteeringSystem.cpp | 11 +- platformio.ini | 2 +- test/test_systems/steering_system_test.h | 156 +++++++++++++++++++++-- 3 files changed, 150 insertions(+), 19 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 18f4251..9b43770 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,17 +1,18 @@ #include #include "SteeringSystem.h" +#include void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on) { //get current raw angles - const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); + const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); //NOLINT will eventually be uint32 //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; - _steeringParams.min_observed_digital = 10000000; //establishes a big number that will be greater than the readings + _steeringParams.min_observed_digital = UINT32_MAX; //establishes a big number that will be greater than the readings _steeringParams.max_observed_digital = 0; } @@ -38,7 +39,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu std::swap(_steeringParams.min_steering_signal_digital,_steeringParams.max_steering_signal_digital); } _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital; - _steeringParams.analog_tol_deg = _steeringParams.span_signal_analog * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog; + _steeringParams.analog_tol_deg = static_cast(_steeringParams.span_signal_analog) * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog; } } @@ -147,8 +148,8 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data float dtheta_analog = out.analog_steering_angle - _prev_analog_angle; //prev_angle established in last run float dtheta_digital = out.digital_steering_angle - _prev_digital_angle; - out.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; - out.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; + out.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; //NOLINT ms to s + out.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; //NOLINT ms to s //Check if either sensor moved too much in one tick diff --git a/platformio.ini b/platformio.ini index cb7b0bd..6b2607f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -136,7 +136,7 @@ check_src_filters = + + - -platform = teensy +platform = teensyit board = teensy41 framework = arduino monitor_speed = 115200 diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index f210fdc..6df1af2 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -7,17 +7,22 @@ #include -SteeringParams_s gen_analog_and_digital_params(){ +SteeringParams_s gen_default_params(){ SteeringParams_s params; //hard code the parmas for sensors params.min_steering_signal_analog = 0; - params.max_steering_signal_digital = 4096;//actual hard coded + params.max_steering_signal_analog = 4096;//actual hard coded + params.min_steering_signal_digital = 0; params.max_steering_signal_digital = 8000; //testing values + params.deg_per_count_analog = 0.087890625f; + params.deg_per_count_digital = 0.02197265625f; + params.analog_tol_deg = 0.11377778f; params.digital_tol_deg = 0.2f; - params.max_dtheta_threshold = 5;//change + + params.max_dtheta_threshold = 5.0f;//change params.error_between_sensors_tolerance = 0.31377778f; return params; } @@ -38,17 +43,142 @@ void debug_print_steering(const SteeringSystemData_s& data){ } +static SteeringSensorData_s make_raw(uint32_t analog_adc, uint32_t digital_adc){ + SteeringSensorData_s raw{}; + raw.analog_steering_degrees = analog_adc; + raw.digital_steering_analog = digital_adc; +} + +static SteeringSystemData_s tick(SteeringSystem& sys,uint32_t analog_raw, uint32_t digital_raw, uint32_t t_ms){ + return sys.evaluate_steering(make_raw(analog_raw,digital_raw),t_ms); +} + +static SteeringSystemData_s tick2(SteeringSystem & sys, uint32_t analog_raw, uint32_t digital_raw, uint32_t t0, uint32_t t1){ + (void)tick(sys,analog_raw,digital_raw,t0); + return(tick(sys,analog_raw,digital_raw,t1)); //get the value of timestamp between evalute steerings +} + +static float expected_angle_from_midpoint(uint32_t raw, uint32_t min, uint32_t max, float deg_per_count) +{ + uint32_t midpoint = (min + max) /2 + int32_t offset = static_cast(raw) - static_cast(midpoint); + return static_cast(offset) * deg_per_count +} + + +/* commented out initial draft because too general TEST(SteeringSystemTesting, test_good_sensors) { - SteeringParams_s params; - params.min_steering_signal_analog = 0; - params.max_steering_signal_digital = 4096;//actual hard coded - params.min_steering_signal_digital = 0; - params.max_steering_signal_digital = 8000; //testing values + auto params = gen_default_params(); + SteeringSystem steering(params); + SteeringSensorData_s sensor{}; + sensor.analog_steering_Degrees = 2048; + sensor.digital_steering_analog = 4000; - params.analog_tol_deg = 0.11377778f; - params.digital_tol_deg = 0.2f; - params.max_dtheta_threshold = 5;//change - params.error_between_sensors_tolerance = 0.31377778f; - return params; + auto data = steering.evaluate_steering(sensor, 1000); //simulated timestamp for tests + + + EXPECT_FALSE(data.sensor_disagreement_implausibility); + +} +*/ +TEST(SteeringSystemTesting, test_adc_to_degree_conversion) +{ + auto prams = gen_default_params(); + SteeringSystem steering(params); + + uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; +} + +TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ + + auto params = gen_default_params(); + SteeringSystem steering(params); + + + //Check for a good value first + SteeringSensorData_s good_val = {}; + good_val.analog_steering_degrees = 2100; + good_val.digital_steering_analog = 4100; + auto data = steering.evaluate_steering(good_val, 1000); + EXPECT_FALSE(data.analog_oor_implausability); + EXPECT_FALSE(data.digital_oor_implausability); + + //OOR High + SteeringSensorData_s high_val = {5000,9000}; + data = steering.evaluate_steering(high_val, 1010); + EXPECT_TRUE(data.analog_oor_implausability); + EXPECT_TRUE(data.digital_oor_implausability); + + //OOR Low + SteeringSensorData_s low_val = {0, 0}; + data = steering.evaluate_steering(low_val, 1020); + EXPECT_TRUE(data.analog_oor_implausability); + EXPECT_TRUE(data.digital_oor_implausability); + +} + +TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ + auto params = gen_default_params(); + SteeringSystem steering(params); + + SteeringSensorData_s baseline = {2100,4100}; + + //First run (just to verify that we don't get a dtheta exceeded on the first run since we have no previous data to compare to) + auto data = steering.evaluate_steering(baseline, 1000); + EXPECT_FALSE(data.dtheta_exceeded_analog); + EXPECT_FALSE(data.dtheta_exceeded_digital); + + //Now verify again when dt is zero we don't get a dtheta exceeded since we can't divide by zero/ + SteeringSensorData_s massive_jump = {4100,8100}; + data = steering.evaluate_steering(massive_jump, 1000); + EXPECT_FALSE(data.dtheta_exceeded_analog); + EXPECT_FALSE(data.dtheta_exceeded_digital); + + //Small motion valid + SteeringSensorData_s small_motion = {2110,4110}; + data = steering.evaluate_steering(small_motion, 1010); //advance time by 10 ms + EXPECT_FALSE(data.dtheta_exceeded_analog); + EXPECT_FALSE(data.dtheta_exceeded_digital); + + //Big motion NOT valid + data = steering.evaluate_steering(massive_jump, 1020); //advance time by another 10 ms + EXPECT_TRUE(data.dtheta_exceeded_analog); + EXPECT_TRUE(data.dtheta_exceeded_digital); } + + + +TEST(SteeringSystemTesting, test_sensor_disagreemnet) +{ + auto params = gen_default_params(); + SteeringSystem steering(params); + + SteeringSensorData_s base{}; + base.analog_steering_degrees = 2000; + base.digital_steering_analog = 4000; + + steering.evaluate_steering(base, 1000); + + //create disagreement between sensors to test + SteeringSensorData_s disagree{}; + disagree.analog_steering_degrees = 2000; //stays same + disagree.digital_steering_analog = 7000; //large offset from analog + + auto data = steering.evaluate_steering(disagree, 1100); + EXPECT_TRUE(data.sensor_diagreement_implausibility); +} + + +TEST(SteeringSystemTesting,test_sensor_output_logic){ + + auto params = gen_standard_params(); + SteeringSystem steering(params); + + //When both valid and agreeing, we default to digital + SteeringSensorData_s both_valid = {2100,4100}; + auto data = steering.evaluate_steering(both_valid, 1000); + EXPECT_NEAR(data.output_steering) + + +} \ No newline at end of file From 34d1ae716a89a138a66e050f82f202e747ab8637 Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 25 Feb 2026 20:28:42 -0500 Subject: [PATCH 16/39] continued working on conversion errors --- lib/systems/src/SteeringSystem.cpp | 22 +++---- test/test_systems/steering_system_test.h | 83 +++++++++++++++--------- 2 files changed, 62 insertions(+), 43 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 9b43770..1be6b85 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -48,9 +48,9 @@ bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) { //check if the analog value is within bounds +- error - const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * _steeringParams.span_signal_analog); - const int32_t min_ok = _steeringParams.min_steering_signal_analog - analog_margin_counts; - const int32_t max_ok = _steeringParams.max_steering_signal_analog + analog_margin_counts; + const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * static_cast(_steeringParams.span_signal_analog)); + const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_analog) - analog_margin_counts; + const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; return (static_cast(steering_analog_raw) < min_ok || static_cast(steering_analog_raw) > max_ok); } @@ -61,8 +61,8 @@ bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_ra //check if the digital value is within bounds +- error const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); - const int32_t min_ok = _steeringParams.min_steering_signal_digital - digital_margin_counts; - const int32_t max_ok = _steeringParams.max_steering_signal_digital + digital_margin_counts; + const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; + const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; return (static_cast(steering_digital_raw) < min_ok || static_cast(steering_digital_raw) > max_ok); } @@ -77,12 +77,12 @@ bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; + const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //NOLINT should be uint32_t later const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); - const float curr_analog_degrees = curr_analog_offset * _steeringParams.deg_per_count_analog; + const float curr_analog_degrees = static_cast(curr_analog_offset) * _steeringParams.deg_per_count_analog; return curr_analog_degrees; @@ -104,7 +104,7 @@ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { // Same logic for digital - const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //adc value + const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //NOLINT should be uint32_t later //create a span for the digital sensor @@ -121,8 +121,8 @@ float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤ void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { - _steeringParams.min_observed_digital = std::fmin(_steeringParams.min_observed_digital, current_steering_data.digital_steering_analog); - _steeringParams.max_observed_digital = std::fmax(_steeringParams.max_observed_digital, current_steering_data.digital_steering_analog); + _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, current_steering_data.digital_steering_analog); + _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, current_steering_data.digital_steering_analog); } SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ @@ -162,7 +162,7 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.digital_steering_analog); //Check if there is too much of a difference between sensor values - float sensor_difference = fabs(out.analog_steering_angle - out.digital_steering_angle); + float sensor_difference = std::fabs(out.analog_steering_angle - out.digital_steering_angle); // const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc // const float analog_tol = _steeringParams.analog_tol*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 6df1af2..c56ab3f 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -42,7 +42,7 @@ void debug_print_steering(const SteeringSystemData_s& data){ } - +/* helper functions, however we are asserting the values in each function as variables static SteeringSensorData_s make_raw(uint32_t analog_adc, uint32_t digital_adc){ SteeringSensorData_s raw{}; raw.analog_steering_degrees = analog_adc; @@ -65,29 +65,50 @@ static float expected_angle_from_midpoint(uint32_t raw, uint32_t min, uint32_t m return static_cast(offset) * deg_per_count } +*/ -/* commented out initial draft because too general -TEST(SteeringSystemTesting, test_good_sensors) +TEST(SteeringSystemTesting, test_adc_to_degree_conversion) { auto params = gen_default_params(); SteeringSystem steering(params); - SteeringSensorData_s sensor{}; - sensor.analog_steering_Degrees = 2048; - sensor.digital_steering_analog = 4000; - auto data = steering.evaluate_steering(sensor, 1000); //simulated timestamp for tests + uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; + uint32_t digital_mid = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; + + //midpoints + SteeringSensorData_s midpoint{}; + midpoint.analog_steering_degrees = analog_mid; + midpoint.digital_steering_analog = digital_mid; + auto data = steering.evaluate_steering(midpoint, 1000); + + EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); + + //min values + SteeringSensorData_s min_val {}; + min_val.analog_steering_degrees = params.min_steering_signal_analog; + min_val.digital_steering_analog = params.min_steering_signal_digital; + + data = steering.evaluate_steering(min_val, 1010); + float expected_analog_min = (static_cast(params.min_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; + float expected_digital_min = (static_cast(params.min_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; - EXPECT_FALSE(data.sensor_disagreement_implausibility); + EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); -} -*/ -TEST(SteeringSystemTesting, test_adc_to_degree_conversion) -{ - auto prams = gen_default_params(); - SteeringSystem steering(params); + //max values + SteeringSensorData_s max_val{}; + max_val.analog_steering_degrees = params.max_steering_signal_analog; + max_val.digital_steering_analog = params.max_steering_signal_digital; - uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; + data = steering.evaluate_steering(max_val, 1020); + float expected_analog_max = (static_cast(params.max_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; + float expected_digital_max = (static_cast(params.max_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; + + EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); + } TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ @@ -95,26 +116,25 @@ TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ auto params = gen_default_params(); SteeringSystem steering(params); - //Check for a good value first SteeringSensorData_s good_val = {}; - good_val.analog_steering_degrees = 2100; - good_val.digital_steering_analog = 4100; + good_val.analog_steering_degrees = 2048; + good_val.digital_steering_analog = 4000; auto data = steering.evaluate_steering(good_val, 1000); - EXPECT_FALSE(data.analog_oor_implausability); - EXPECT_FALSE(data.digital_oor_implausability); + EXPECT_FALSE(data.analog_oor_implausibility); + EXPECT_FALSE(data.digital_oor_implausibility); //OOR High - SteeringSensorData_s high_val = {5000,9000}; + SteeringSensorData_s high_val = {5000, 9000}; data = steering.evaluate_steering(high_val, 1010); - EXPECT_TRUE(data.analog_oor_implausability); - EXPECT_TRUE(data.digital_oor_implausability); + EXPECT_TRUE(data.analog_oor_implausibility); + EXPECT_TRUE(data.digital_oor_implausibility); //OOR Low - SteeringSensorData_s low_val = {0, 0}; + SteeringSensorData_s low_val = {static_cast(-1000), static_cast(-1000)}; data = steering.evaluate_steering(low_val, 1020); - EXPECT_TRUE(data.analog_oor_implausability); - EXPECT_TRUE(data.digital_oor_implausability); + EXPECT_TRUE(data.analog_oor_implausibility); + EXPECT_TRUE(data.digital_oor_implausibility); } @@ -122,7 +142,7 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ auto params = gen_default_params(); SteeringSystem steering(params); - SteeringSensorData_s baseline = {2100,4100}; + SteeringSensorData_s baseline = {2048, 4000}; //First run (just to verify that we don't get a dtheta exceeded on the first run since we have no previous data to compare to) auto data = steering.evaluate_steering(baseline, 1000); @@ -130,13 +150,13 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ EXPECT_FALSE(data.dtheta_exceeded_digital); //Now verify again when dt is zero we don't get a dtheta exceeded since we can't divide by zero/ - SteeringSensorData_s massive_jump = {4100,8100}; + SteeringSensorData_s massive_jump = {4096, 8000}; data = steering.evaluate_steering(massive_jump, 1000); EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); //Small motion valid - SteeringSensorData_s small_motion = {2110,4110}; + SteeringSensorData_s small_motion = {2060, 4050}; data = steering.evaluate_steering(small_motion, 1010); //advance time by 10 ms EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); @@ -148,7 +168,6 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ } - TEST(SteeringSystemTesting, test_sensor_disagreemnet) { auto params = gen_default_params(); @@ -176,9 +195,9 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ SteeringSystem steering(params); //When both valid and agreeing, we default to digital - SteeringSensorData_s both_valid = {2100,4100}; + SteeringSensorData_s both_valid = {2000,4000}; auto data = steering.evaluate_steering(both_valid, 1000); - EXPECT_NEAR(data.output_steering) + EXPECT_NEAR(data.output_steering_angle,data); } \ No newline at end of file From d3b219cc3b7cbfcd5ebbbd3c69be8599d9cb2a9a Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 25 Feb 2026 20:33:01 -0500 Subject: [PATCH 17/39] more work on conversion errors --- lib/systems/src/SteeringSystem.cpp | 4 ++-- test/test_systems/steering_system_test.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 1be6b85..0fa8687 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -121,8 +121,8 @@ float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤ void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { - _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, current_steering_data.digital_steering_analog); - _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, current_steering_data.digital_steering_analog); + _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT should both be uint32_t + _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT ^ } SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index c56ab3f..4e73118 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -168,7 +168,7 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ } -TEST(SteeringSystemTesting, test_sensor_disagreemnet) +TEST(SteeringSystemTesting, test_sensor_disagreement) { auto params = gen_default_params(); SteeringSystem steering(params); From b5ef37727e6caf4df074a052b64820209196d7a7 Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 25 Feb 2026 20:36:49 -0500 Subject: [PATCH 18/39] AGAIN --- lib/systems/src/SteeringSystem.cpp | 4 ++-- test/test_systems/steering_system_test.h | 14 +++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 0fa8687..e875104 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -158,8 +158,8 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ //Check if either sensor is out of range (pass in raw) - out.analog_oor_implausibility = _evaluate_steering_oor_analog(current_steering_data.analog_steering_degrees); - out.digital_oor_implausibility = _evaluate_steering_oor_digital(current_steering_data.digital_steering_analog); + out.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(current_steering_data.analog_steering_degrees)); + out.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(current_steering_data.digital_steering_analog)); //Check if there is too much of a difference between sensor values float sensor_difference = std::fabs(out.analog_steering_angle - out.digital_steering_angle); diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 4e73118..2420a8a 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -191,13 +191,21 @@ TEST(SteeringSystemTesting, test_sensor_disagreement) TEST(SteeringSystemTesting,test_sensor_output_logic){ - auto params = gen_standard_params(); + auto params = gen_default_params(); SteeringSystem steering(params); //When both valid and agreeing, we default to digital - SteeringSensorData_s both_valid = {2000,4000}; + SteeringSensorData_s both_valid = {2048,4000}; auto data = steering.evaluate_steering(both_valid, 1000); - EXPECT_NEAR(data.output_steering_angle,data); + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + EXPECT_FALSE(data.both_sensors_fail); + EXPECT_FALSE(data.sensor_disagreement_implausibility); + + //When both valid but disagreeing, we default to digital + SteeringSensorData_s both_valid_disagree = {2048, 7000}; + auto data = steering.evaluate_steering(both_valid_disagree, 1010); + + } \ No newline at end of file From 1f49f9ec3879ac712f52b4fdaf191aa0db524fcd Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 25 Feb 2026 20:41:51 -0500 Subject: [PATCH 19/39] finish unit test --- test/test_systems/steering_system_test.h | 27 +++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 2420a8a..8c5ca7e 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -85,7 +85,7 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); //min values - SteeringSensorData_s min_val {}; + SteeringSensorData_s min_val{}; min_val.analog_steering_degrees = params.min_steering_signal_analog; min_val.digital_steering_analog = params.min_steering_signal_digital; @@ -205,6 +205,31 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ SteeringSensorData_s both_valid_disagree = {2048, 7000}; auto data = steering.evaluate_steering(both_valid_disagree, 1010); + EXPECT_TRUE(data.sensor_disagreement_implausibility); + EXPECT_FALSE(data.analog_oor_implausibility); + EXPECT_FALSE(data.digital_oor_implausibility); + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + + //When analog is good but digital is bad, we put analog + SteeringSensorData_s digital_bad = {2048, 9000}; + data = steering.evaluate_steering(digital_bad, 1020); + EXPECT_TRUE(data.digital_oor_implausibility); + EXPECT_FALSE(data.analog_oor_implausibility); + EXPECT_NEAR(data.output_steering_angle, data.analog_steering_angle, 0.001f); + + //When digital is good but analog is bad, we put digital + SteeringSensorData_s analog_bad = {5000, 4000}; + data = steering.evaluate_steering(analog_bad, 1030); + EXPECT_TRUE(data.analog_oor_implausibility); + EXPECT_FALSE(data.digital_oor_implausibility); + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + + //When both bad, we flagging that error + SteeringSensorData_s both_bad = {5000, 9000}; + auto data = steering.evaluate_steering(both_bad, 1040); + EXPECT_TRUE(data.analog_oor_implausibility); + EXPECT_TRUE(data.digital_oor_implausibility); + EXPECT_TRUE(data.both_sensors_fail); From 940f5568f8cb46badc536dfaba3e0418b329d652 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Wed, 25 Feb 2026 21:36:44 -0500 Subject: [PATCH 20/39] fixed formatting --- lib/systems/include/SteeringSystem.h | 127 +++++------------ lib/systems/src/SteeringSystem.cpp | 170 +++++++---------------- platformio.ini | 2 +- test/test_systems/main.cpp | 1 + test/test_systems/steering_system_test.h | 22 +-- 5 files changed, 106 insertions(+), 216 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index af6808b..c8d6d1f 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -1,151 +1,104 @@ #ifndef STEERING_SYSTEM_H #define STEERING_SYSTEM_H -#include - +#include #include #include #include - #include "SharedFirmwareTypes.h" - - - - - struct SteeringParams_s { - //raw ADC input signals + // raw ADC input signals uint32_t min_steering_signal_analog = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) uint32_t max_steering_signal_analog = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle - uint32_t span_signal_analog = 4096; + uint32_t span_signal_analog = 4095; uint32_t span_signal_digital; - - //calibration limits: only digital is recalibrated + // calibration limits uint32_t min_observed_digital; uint32_t max_observed_digital; + uint32_t min_observed_analog; // do we need to do min/max calibration for analog? + uint32_t max_observed_analog; - - //conversion rates + // conversion rates // float deg_per_count_analog = 0.0439f; //hard coded for analog (180) float deg_per_count_analog = 0.087890625f; float deg_per_count_digital = 0.02197265625f; //based on digital readings - - //implausability values + // implausibility values float analog_tol = 0.005f; //+- 0.5% error float analog_tol_deg; float digital_tol_deg = 0.2f; // +- 0.2 degrees error - //rate of angle change + // rate of angle change float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid - - //difference rating + // difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed }; - - - struct SteeringSystemData_s { float analog_steering_angle; //in degrees float digital_steering_angle; //in degrees float output_steering_angle; // represents the better output of the two sensors or some combination of the values - float analog_steering_velocity_deg_s; //in degrees per second float digital_steering_velocity_deg_s; - bool digital_oor_implausibility; bool analog_oor_implausibility; bool sensor_disagreement_implausibility; - bool dtheta_exceeded_analog; bool dtheta_exceeded_digital; bool both_sensors_fail; }; +class SteeringSystem { +public: + SteeringSystem(const SteeringParams_s &steeringParams) : _steeringParams(steeringParams) {} + // Functions + void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); + + SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); + // Getters + const SteeringParams_s &get_steering_params() const { + return _steeringParams; + } -class SteeringSystem -{ -public: - SteeringSystem(const SteeringParams_s &steeringParams) : _steeringParams(steeringParams) - {} + const SteeringSystemData_s &get_steering_system_data() const { + return _steeringSystemData; + } + const SteeringSensorData_s &get_steering_sensor_data() const { + return _steeringSensorData; + } // Setters void set_steering_params(const SteeringParams_s &steeringParams) { _steeringParams = steeringParams; } - void set_steering_system_data(const SteeringSystemData_s &steeringSystemData) { _steeringSystemData = steeringSystemData; } - void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData) { _steeringSensorData = steeringSensorData; } - - - // Getters - const SteeringParams_s &get_steering_params() const; - - const SteeringSystemData_s &get_steering_system_data() const { - return _steeringSystemData; - } - - - const SteeringSensorData_s &get_steering_sensor_data() const { - return _steeringSensorData; - } - - - // Other Functions - void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - - - //must have an input that says recalibration is on, then collect the abs min and max value - //from both sensors to establish a universal min & max - - SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); - - + private: - - - //track the state of our system from the previous tick to compare against current state for implausibility checks - float _prev_analog_angle = 0.0f; - float _prev_digital_angle = 0.0f; - uint32_t _prev_timestamp = 0; - bool _calibrating = false; - bool _first_run = true; // skip dTheta check on the very first tick - - - - - - void update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data); float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); - - - - //returns true if steering_analog is outside of the range defined by min and max sensor values bool _evaluate_steering_oor_analog(uint32_t steering_analog); @@ -153,26 +106,20 @@ class SteeringSystem //returns true if steering_digital is outside the range defined by min and max sensor values bool _evaluate_steering_oor_digital(uint32_t steering_digital); - - - //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) bool _evaluate_steering_dtheta_exceeded(float dtheta); - - - - - - - SteeringSensorData_s _steeringSensorData{}; - SteeringSystemData_s _steeringSystemData{}; - SteeringParams_s _steeringParams{}; - + SteeringSensorData_s _steeringSensorData {}; + SteeringSystemData_s _steeringSystemData {}; + SteeringParams_s _steeringParams; + //track the state of our system from the previous tick to compare against current state for implausibility checks + float _prev_analog_angle = 0.0f; + float _prev_digital_angle = 0.0f; + uint32_t _prev_timestamp = 0; + bool _calibrating = false; + bool _first_run = true; // skip dTheta check on the very first tick }; - using SteeringSystemInstance = etl::singleton; - #endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index e875104..a6f2b80 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,9 +1,6 @@ #include -#include "SteeringSystem.h" #include - - - +#include "SteeringSystem.h" void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on) { //get current raw angles @@ -19,6 +16,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu if (calibration_is_on && _calibrating) { update_observed_steering_limits(current_steering_data); } + //update relative extremeties // assigning min and max observed during calibration only vs. constantly? // if (calibration_is_on){ @@ -27,7 +25,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu // return; // } - //button released ->commit the values + //button released -> commit the values if (!calibration_is_on && _calibrating) { _calibrating = false; @@ -43,90 +41,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } } - -bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) // RAW - { - - //check if the analog value is within bounds +- error - const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * static_cast(_steeringParams.span_signal_analog)); - const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_analog) - analog_margin_counts; - const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; - return (static_cast(steering_analog_raw) < min_ok || static_cast(steering_analog_raw) > max_ok); - - } - - -bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_raw) // RAW - { - //check if the digital value is within bounds +- error - - const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); - const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; - const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; - return (static_cast(steering_digital_raw) < min_ok || static_cast(steering_digital_raw) > max_ok); - } - - -bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ - return (fabs(dtheta) > _steeringParams.max_dtheta_threshold); - } - - - - -float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) -{ - // Get the raw value - const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //NOLINT should be uint32_t later - const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; - - const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); - - const float curr_analog_degrees = static_cast(curr_analog_offset) * _steeringParams.deg_per_count_analog; - - - return curr_analog_degrees; - // Calculate how far we are from the minimum raw count delta - // const uint32_t count_delta = (int32_t)raw_val - (int32_t)_steeringParams.min_steering_signal_analog; - - - // // Convert that delta into degrees - // const float degrees_delta = count_delta * _steeringParams.deg_per_count_analog; - - - // // Add to the starting minimum angle to get absolute position - // return kMindeg + degrees_delta; -} - - - - -float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) -{ - // Same logic for digital - const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //NOLINT should be uint32_t later - //create a span for the digital sensor - - - //degree span we are assuming: - //_steeringParams.deg_per_count_digital = kSpan/(float)span_digital; -> we are assuming deg_per_count hardcoded from datasheet - const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2); - - - const int32_t curr_digital_offset = static_cast(curr_raw_digital) - static_cast(digital_midpoint); - float curr_digital_degrees = (float)curr_digital_offset * _steeringParams.deg_per_count_digital; - return curr_digital_degrees; -} - - - -void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { - _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT should both be uint32_t - _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT ^ -} - SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ - SteeringSystemData_s out = {};// initialize struct to be blank // Reset flags @@ -141,81 +56,102 @@ SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_ out.analog_steering_angle = _convert_analog_sensor(current_steering_data); out.digital_steering_angle = _convert_digital_sensor(current_steering_data); - uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input - - if(!_first_run && dt > 0){ //check that we not on the first run which would mean no previous data + if (!_first_run && dt > 0) { //check that we not on the first run which would mean no previous data float dtheta_analog = out.analog_steering_angle - _prev_analog_angle; //prev_angle established in last run float dtheta_digital = out.digital_steering_angle - _prev_digital_angle; out.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; //NOLINT ms to s out.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; //NOLINT ms to s - //Check if either sensor moved too much in one tick out.dtheta_exceeded_analog = _evaluate_steering_dtheta_exceeded(dtheta_analog); out.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); - //Check if either sensor is out of range (pass in raw) out.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(current_steering_data.analog_steering_degrees)); out.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(current_steering_data.digital_steering_analog)); //Check if there is too much of a difference between sensor values float sensor_difference = std::fabs(out.analog_steering_angle - out.digital_steering_angle); - - // const float full_scale_deg_analog = fabsf(_steeringParams.max_steering_signal_analog - _steeringParams.min_steering_signal_analog); //in adc - // const float analog_tol = _steeringParams.analog_tol*full_scale_deg_analog*_steeringParams.deg_per_count_analog; // analog percentage*adc span *deg/adc count const float allowed_difference = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% bool sensors_agree = (sensor_difference <= allowed_difference); //steeringParams.error out.sensor_disagreement_implausibility = !sensors_agree; - - //create an algorithm/ checklist to determine which sensor we trust more, //or, if we should have an algorithm to have a weighted calculation based on both values bool analog_valid = !out.analog_oor_implausibility && !out.dtheta_exceeded_analog; bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; - if (analog_valid && digital_valid) - { + if (analog_valid && digital_valid) { //if sensors have acceptable difference, use digital as steering angle - if (sensors_agree) - { + if (sensors_agree) { out.output_steering_angle = out.digital_steering_angle; - } - else - { + } else { out.output_steering_angle = out.digital_steering_angle; //default to original, but we need to consider what we really want to put here } - } - else if (analog_valid) - { + } else if (analog_valid) { out.output_steering_angle = out.analog_steering_angle; - } - else if (digital_valid) { + } else if (digital_valid) { out.output_steering_angle = out.digital_steering_angle; - } - else // if both sensors fail - { + } else { // if both sensors fail out.output_steering_angle = _prev_digital_angle; out.both_sensors_fail = true; } } //Update states - - _prev_analog_angle = out.analog_steering_angle; _prev_digital_angle = out.digital_steering_angle; _prev_timestamp = current_millis; _first_run = false; - return out; - } +void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { + _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT should both be uint32_t + _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT ^ +} + +float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { + // Same logic for digital + const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //NOLINT should be uint32_t later + const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2); + + const int32_t curr_digital_offset = static_cast(curr_raw_digital) - static_cast(digital_midpoint); + float curr_digital_degrees = (float)curr_digital_offset * _steeringParams.deg_per_count_digital; + return curr_digital_degrees; +} + +float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { + // Get the raw value + const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //NOLINT should be uint32_t later + const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; + + const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); + + const float curr_analog_degrees = static_cast(curr_analog_offset) * _steeringParams.deg_per_count_analog; + return curr_analog_degrees; +} + +bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) { // RAW + //check if the analog value is within bounds +- error + const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * static_cast(_steeringParams.span_signal_analog)); + const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_analog) - analog_margin_counts; + const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; + return (static_cast(steering_analog_raw) < min_ok || static_cast(steering_analog_raw) > max_ok); +} +bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_raw) {// RAW + //check if the digital value is within bounds +- error + const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); + const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; + const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; + return (static_cast(steering_digital_raw) < min_ok || static_cast(steering_digital_raw) > max_ok); +} +bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ + return (fabs(dtheta) > _steeringParams.max_dtheta_threshold); +} diff --git a/platformio.ini b/platformio.ini index 6b2607f..656d261 100644 --- a/platformio.ini +++ b/platformio.ini @@ -151,4 +151,4 @@ lib_deps = blemasle/MCP23017@^2.0.0 https://github.com/hytech-racing/HT_CAN/releases/download/192/can_lib.tar.gz https://github.com/hytech-racing/shared_firmware_interfaces.git#29a9cb09f91b684382e897de780d8d52a14659a7 - https://github.com/KSU-MS/pio-git-hash-gen#7998b5b3f8a2464209b0e73338717998bcf511ee \ No newline at end of file + https://github.com/KSU-MS/pio-git-hash-gen#7998b5b3f8a2464209b0e73338717998bcf511ee diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 812379c..0be3f13 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -2,6 +2,7 @@ #include #include "pedals_system_test.h" #include "test_buzzer.h" +#include "steering_system_test.h" int main(int argc, char **argv) { diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 8c5ca7e..b5ed819 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -11,11 +11,17 @@ SteeringParams_s gen_default_params(){ SteeringParams_s params; //hard code the parmas for sensors params.min_steering_signal_analog = 0; - params.max_steering_signal_analog = 4096;//actual hard coded + params.max_steering_signal_analog = 4095;//actual hard coded params.min_steering_signal_digital = 0; params.max_steering_signal_digital = 8000; //testing values + params.span_signal_analog = 4095; + params.span_signal_digital 8000; + + params.min_observed_digital = 2000; + params.max_observed_digital = 6000; + params.deg_per_count_analog = 0.087890625f; params.deg_per_count_digital = 0.02197265625f; @@ -34,9 +40,9 @@ void debug_print_steering(const SteeringSystemData_s& data){ std::cout<<"analog_steering_angle: "< Date: Wed, 25 Feb 2026 21:37:50 -0500 Subject: [PATCH 21/39] forgot one line --- lib/systems/include/SteeringSystem.h | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index c8d6d1f..ebbd757 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -38,7 +38,6 @@ struct SteeringParams_s { // difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed - }; struct SteeringSystemData_s From 4a292e0c1bd24c53be70c35cf97b4a0c72921245 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Wed, 25 Feb 2026 21:57:13 -0500 Subject: [PATCH 22/39] fix build errors maybe --- test/test_systems/steering_system_test.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index b5ed819..f77380a 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -17,11 +17,11 @@ SteeringParams_s gen_default_params(){ params.max_steering_signal_digital = 8000; //testing values params.span_signal_analog = 4095; - params.span_signal_digital 8000; + params.span_signal_digital = 8000; params.min_observed_digital = 2000; params.max_observed_digital = 6000; - + params.deg_per_count_analog = 0.087890625f; params.deg_per_count_digital = 0.02197265625f; @@ -33,9 +33,6 @@ SteeringParams_s gen_default_params(){ return params; } - - - void debug_print_steering(const SteeringSystemData_s& data){ std::cout<<"analog_steering_angle: "<(-1000), static_cast(-1000)}; + SteeringSensorData_s low_val = {static_cast(0), static_cast(0)}; data = steering.evaluate_steering(low_val, 1020); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); From 56e1f397fb2a3f97a654342234f0020ddaf954ca Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 1 Mar 2026 18:26:01 -0500 Subject: [PATCH 23/39] fixed some unit tests --- lib/systems/src/SteeringSystem.cpp | 1 + test/test_systems/steering_system_test.h | 26 +++++++++++++++++------- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index a6f2b80..4e2fe16 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -155,3 +155,4 @@ bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_ra bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ return (fabs(dtheta) > _steeringParams.max_dtheta_threshold); } + diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index f77380a..f2466af 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -70,6 +70,7 @@ static float expected_angle_from_midpoint(uint32_t raw, uint32_t min, uint32_t m */ +// fix TEST(SteeringSystemTesting, test_adc_to_degree_conversion) { auto params = gen_default_params(); @@ -114,9 +115,12 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) } +//fix TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ auto params = gen_default_params(); + params.min_steering_signal_analog = 1000; + params.min_steering_signal_digital = 1000; SteeringSystem steering(params); //Check for a good value first @@ -134,13 +138,16 @@ TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ EXPECT_TRUE(data.digital_oor_implausibility); //OOR Low + SteeringSensorData_s low_val = {static_cast(0), static_cast(0)}; data = steering.evaluate_steering(low_val, 1020); + // data = steering.evaluate_steering(low_val, 1030); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); } +//fix TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ auto params = gen_default_params(); SteeringSystem steering(params); @@ -152,20 +159,22 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); - //Now verify again when dt is zero we don't get a dtheta exceeded since we can't divide by zero/ SteeringSensorData_s massive_jump = {4096, 8000}; - data = steering.evaluate_steering(massive_jump, 1000); - EXPECT_FALSE(data.dtheta_exceeded_analog); - EXPECT_FALSE(data.dtheta_exceeded_digital); + data = steering.evaluate_steering(massive_jump, 1005); + EXPECT_TRUE(data.dtheta_exceeded_analog); + EXPECT_TRUE(data.dtheta_exceeded_digital); + + // Reset the last value of evaluate steering to baseline + steering.evaluate_steering(baseline, 1010); //Small motion valid SteeringSensorData_s small_motion = {2060, 4050}; - data = steering.evaluate_steering(small_motion, 1010); //advance time by 10 ms + data = steering.evaluate_steering(small_motion, 1020); //advance time by 10 ms EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); //Big motion NOT valid - data = steering.evaluate_steering(massive_jump, 1020); //advance time by another 10 ms + data = steering.evaluate_steering(massive_jump, 1030); //advance time by another 10 ms EXPECT_TRUE(data.dtheta_exceeded_analog); EXPECT_TRUE(data.dtheta_exceeded_digital); } @@ -191,7 +200,7 @@ TEST(SteeringSystemTesting, test_sensor_disagreement) EXPECT_TRUE(data.sensor_disagreement_implausibility); } - +// fix TEST(SteeringSystemTesting,test_sensor_output_logic){ auto params = gen_default_params(); @@ -204,6 +213,9 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ EXPECT_FALSE(data.both_sensors_fail); EXPECT_FALSE(data.sensor_disagreement_implausibility); + // Prevent dtheta exceeded for the next test + steering.evaluate_steering({2048, 7000}, 1000); + //When both valid but disagreeing, we default to digital SteeringSensorData_s both_valid_disagree = {2048, 7000}; data = steering.evaluate_steering(both_valid_disagree, 1010); From 3fba1ef1cf0338745e4597305f84769641c6152d Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 1 Mar 2026 18:55:28 -0500 Subject: [PATCH 24/39] made conversion functions simpler --- lib/systems/include/SteeringSystem.h | 3 +++ lib/systems/src/SteeringSystem.cpp | 20 ++++++-------------- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index ebbd757..d4499ce 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -14,8 +14,11 @@ struct SteeringParams_s { uint32_t max_steering_signal_analog = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle + uint32_t span_signal_analog = 4095; uint32_t span_signal_digital; + int32_t digital_midpoint; + int32_t analog_midpoint; // calibration limits uint32_t min_observed_digital; diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 4e2fe16..1d17119 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -38,6 +38,8 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital; _steeringParams.analog_tol_deg = static_cast(_steeringParams.span_signal_analog) * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog; + _steeringParams.digital_midpoint = static_cast((_steeringParams.max_steering_signal_digital + _steeringParams.min_steering_signal_digital) / 2); //NOLINT + _steeringParams.analog_midpoint = static_cast((_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2); //NOLINT } } @@ -116,24 +118,14 @@ void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { // Same logic for digital - const uint32_t curr_raw_digital = current_steering_data.digital_steering_analog; //NOLINT should be uint32_t later - const uint32_t digital_midpoint = ((_steeringParams.max_steering_signal_digital+_steeringParams.min_steering_signal_digital)/2); - - const int32_t curr_digital_offset = static_cast(curr_raw_digital) - static_cast(digital_midpoint); - float curr_digital_degrees = (float)curr_digital_offset * _steeringParams.deg_per_count_digital; - return curr_digital_degrees; + const int32_t offset = static_cast(current_steering_data.digital_steering_analog) - _steeringParams.digital_midpoint; //NOLINT + return static_cast(offset) * _steeringParams.deg_per_count_digital; } float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { // Get the raw value - const uint32_t curr_raw_analog = current_steering_data.analog_steering_degrees; //NOLINT should be uint32_t later - const uint32_t analog_midpoint = (_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2; - - const int32_t curr_analog_offset = static_cast(curr_raw_analog) - static_cast(analog_midpoint); - - const float curr_analog_degrees = static_cast(curr_analog_offset) * _steeringParams.deg_per_count_analog; - - return curr_analog_degrees; + const int32_t offset = static_cast(current_steering_data.analog_steering_degrees) - _steeringParams.analog_midpoint; //NOLINT + return static_cast(offset) * _steeringParams.deg_per_count_analog; } bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) { // RAW From f1897d2f5657386cd594e8e479c6390656c7fb63 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 1 Mar 2026 19:04:57 -0500 Subject: [PATCH 25/39] simplified oor functions --- lib/systems/include/SteeringSystem.h | 5 +++++ lib/systems/src/SteeringSystem.cpp | 18 ++++++++---------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index d4499ce..1899ec6 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -15,6 +15,11 @@ struct SteeringParams_s { uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle + int32_t analog_min_with_margins; + int32_t analog_max_with_margins; + int32_t digital_min_with_margins; + int32_t digital_max_with_margins; + uint32_t span_signal_analog = 4095; uint32_t span_signal_digital; int32_t digital_midpoint; diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 1d17119..96b2963 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -40,6 +40,12 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu _steeringParams.analog_tol_deg = static_cast(_steeringParams.span_signal_analog) * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog; _steeringParams.digital_midpoint = static_cast((_steeringParams.max_steering_signal_digital + _steeringParams.min_steering_signal_digital) / 2); //NOLINT _steeringParams.analog_midpoint = static_cast((_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2); //NOLINT + const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * static_cast(_steeringParams.span_signal_analog)); //NOLINT + const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); //NOLINT + _steeringParams.analog_min_with_margins = static_cast(_steeringParams.min_steering_signal_analog) - analog_margin_counts; + _steeringParams.analog_max_with_margins = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; + _steeringParams.digital_min_with_margins = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; + _steeringParams.digital_max_with_margins = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; } } @@ -129,19 +135,11 @@ float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t } bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) { // RAW - //check if the analog value is within bounds +- error - const int32_t analog_margin_counts = static_cast(_steeringParams.analog_tol * static_cast(_steeringParams.span_signal_analog)); - const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_analog) - analog_margin_counts; - const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; - return (static_cast(steering_analog_raw) < min_ok || static_cast(steering_analog_raw) > max_ok); + return (static_cast(steering_analog_raw) < _steeringParams.analog_min_with_margins || static_cast(steering_analog_raw) > _steeringParams.analog_max_with_margins); } bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_raw) {// RAW - //check if the digital value is within bounds +- error - const int32_t digital_margin_counts = static_cast(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); - const int32_t min_ok = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; - const int32_t max_ok = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; - return (static_cast(steering_digital_raw) < min_ok || static_cast(steering_digital_raw) > max_ok); + return (static_cast(steering_digital_raw) < _steeringParams.digital_min_with_margins || static_cast(steering_digital_raw) > _steeringParams.digital_max_with_margins); } bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ From c464983a4a7721bcdd9ef81346d2c8db01519a79 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 1 Mar 2026 19:16:45 -0500 Subject: [PATCH 26/39] addressed all comments besides EEPROM stuff --- lib/systems/include/SteeringSystem.h | 2 +- lib/systems/src/SteeringSystem.cpp | 62 +++++++++++++--------------- 2 files changed, 30 insertions(+), 34 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 1899ec6..b2647ef 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -72,7 +72,7 @@ class SteeringSystem { // Functions void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - SteeringSystemData_s evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); + void evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); // Getters const SteeringParams_s &get_steering_params() const { diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 96b2963..eb31e49 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -49,72 +49,68 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } } -SteeringSystemData_s SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ - SteeringSystemData_s out = {};// initialize struct to be blank - +void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ // Reset flags - out.digital_oor_implausibility = false; - out.analog_oor_implausibility = false; - out.sensor_disagreement_implausibility = false; - out.dtheta_exceeded_analog = false; - out.dtheta_exceeded_digital = false; - out.both_sensors_fail = false; + _steeringSystemData.digital_oor_implausibility = false; + _steeringSystemData.analog_oor_implausibility = false; + _steeringSystemData.sensor_disagreement_implausibility = false; + _steeringSystemData.dtheta_exceeded_analog = false; + _steeringSystemData.dtheta_exceeded_digital = false; + _steeringSystemData.both_sensors_fail = false; //Conversion from raw ADC to degrees - out.analog_steering_angle = _convert_analog_sensor(current_steering_data); - out.digital_steering_angle = _convert_digital_sensor(current_steering_data); + _steeringSystemData.analog_steering_angle = _convert_analog_sensor(current_steering_data); + _steeringSystemData.digital_steering_angle = _convert_digital_sensor(current_steering_data); uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input if (!_first_run && dt > 0) { //check that we not on the first run which would mean no previous data - float dtheta_analog = out.analog_steering_angle - _prev_analog_angle; //prev_angle established in last run - float dtheta_digital = out.digital_steering_angle - _prev_digital_angle; - out.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; //NOLINT ms to s - out.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; //NOLINT ms to s + float dtheta_analog = _steeringSystemData.analog_steering_angle - _prev_analog_angle; //prev_angle established in last run + float dtheta_digital = _steeringSystemData.digital_steering_angle - _prev_digital_angle; + _steeringSystemData.analog_steering_velocity_deg_s = (dtheta_analog / dt) * 1000.0f; //NOLINT ms to s + _steeringSystemData.digital_steering_velocity_deg_s = (dtheta_digital / dt) * 1000.0f; //NOLINT ms to s //Check if either sensor moved too much in one tick - out.dtheta_exceeded_analog = _evaluate_steering_dtheta_exceeded(dtheta_analog); - out.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); + _steeringSystemData.dtheta_exceeded_analog = _evaluate_steering_dtheta_exceeded(dtheta_analog); + _steeringSystemData.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); //Check if either sensor is out of range (pass in raw) - out.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(current_steering_data.analog_steering_degrees)); - out.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(current_steering_data.digital_steering_analog)); + _steeringSystemData.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(current_steering_data.analog_steering_degrees)); + _steeringSystemData.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(current_steering_data.digital_steering_analog)); //Check if there is too much of a difference between sensor values - float sensor_difference = std::fabs(out.analog_steering_angle - out.digital_steering_angle); + float sensor_difference = std::fabs(_steeringSystemData.analog_steering_angle - _steeringSystemData.digital_steering_angle); const float allowed_difference = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% bool sensors_agree = (sensor_difference <= allowed_difference); //steeringParams.error - out.sensor_disagreement_implausibility = !sensors_agree; + _steeringSystemData.sensor_disagreement_implausibility = !sensors_agree; //create an algorithm/ checklist to determine which sensor we trust more, //or, if we should have an algorithm to have a weighted calculation based on both values - bool analog_valid = !out.analog_oor_implausibility && !out.dtheta_exceeded_analog; - bool digital_valid = !out.digital_oor_implausibility && !out.dtheta_exceeded_digital; + bool analog_valid = !_steeringSystemData.analog_oor_implausibility && !_steeringSystemData.dtheta_exceeded_analog; + bool digital_valid = !_steeringSystemData.digital_oor_implausibility && !_steeringSystemData.dtheta_exceeded_digital; if (analog_valid && digital_valid) { //if sensors have acceptable difference, use digital as steering angle if (sensors_agree) { - out.output_steering_angle = out.digital_steering_angle; + _steeringSystemData.output_steering_angle = _steeringSystemData.digital_steering_angle; } else { - out.output_steering_angle = out.digital_steering_angle; //default to original, but we need to consider what we really want to put here + _steeringSystemData.output_steering_angle = _steeringSystemData.digital_steering_angle; //default to original, but we need to consider what we really want to put here } } else if (analog_valid) { - out.output_steering_angle = out.analog_steering_angle; + _steeringSystemData.output_steering_angle = _steeringSystemData.analog_steering_angle; } else if (digital_valid) { - out.output_steering_angle = out.digital_steering_angle; + _steeringSystemData.output_steering_angle = _steeringSystemData.digital_steering_angle; } else { // if both sensors fail - out.output_steering_angle = _prev_digital_angle; - out.both_sensors_fail = true; + _steeringSystemData.output_steering_angle = _prev_digital_angle; + _steeringSystemData.both_sensors_fail = true; } } //Update states - _prev_analog_angle = out.analog_steering_angle; - _prev_digital_angle = out.digital_steering_angle; + _prev_analog_angle = _steeringSystemData.analog_steering_angle; + _prev_digital_angle = _steeringSystemData.digital_steering_angle; _prev_timestamp = current_millis; _first_run = false; - - return out; } void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { From 15e5d83d25ce15804d1a65eb00f98805e2ef138e Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 1 Mar 2026 20:37:55 -0500 Subject: [PATCH 27/39] added VCF tasks for writing to EEPROM, running system, and initializing --- include/VCF_Constants.h | 29 +++++++++++++++++++- include/VCF_Tasks.h | 2 ++ lib/systems/include/SteeringSystem.h | 2 +- lib/systems/src/SteeringSystem.cpp | 7 ++--- src/VCF_Tasks.cpp | 41 ++++++++++++++++++++++++++++ src/main.cpp | 2 +- 6 files changed, 76 insertions(+), 7 deletions(-) diff --git a/include/VCF_Constants.h b/include/VCF_Constants.h index 388baa6..cdab4ae 100644 --- a/include/VCF_Constants.h +++ b/include/VCF_Constants.h @@ -110,6 +110,33 @@ namespace VCFInterfaceConstants { // calibration and processing constants namespace VCFSystemConstants { constexpr float LBS_TO_NEWTONS = 4.4482216153; + + // Steering System Constants + // TODO: determine addresses + constexpr uint32_t MIN_STEERING_SIGNAL_ANALOG = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) + constexpr uint32_t MAX_STEERING_SIGNAL_ANALOG = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle + constexpr uint32_t MIN_STEERING_SIGNAL_DIGITAL_ADDR = 57; //Raw ADC value from digital sensor at minimum (left) steering angle + constexpr uint32_t MAX_STEERING_SIGNAL_DIGITAL_ADDR = 57; //Raw ADC value from digital sensor at maximum (right) steering angle + + constexpr int32_t ANALOG_MIN_WITH_MARGINS_ADDR = 57; + constexpr int32_t ANALOG_MAX_WITH_MARGINS_ADDR = 57; + constexpr int32_t DIGITAL_MIN_WITH_MARGINS_ADDR = 57; + constexpr int32_t DIGITAL_MAX_WITH_MARGINS_ADDR = 57; + + constexpr uint32_t SPAN_SIGNAL_ANALOG = MAX_STEERING_SIGNAL_ANALOG - MIN_STEERING_SIGNAL_ANALOG; + constexpr int32_t ANALOG_MIDPOINT = (MIN_STEERING_SIGNAL_ANALOG + MAX_STEERING_SIGNAL_ANALOG) / 2; + + // conversion rates + // float deg_per_count_analog = 0.0439f; //hard coded for analog (180) + constexpr float DEG_PER_COUNT_ANALOG = 360.0f / 4095.0f; + constexpr float DEG_PER_COUNT_DIGITAL = 360.0f / 16383.0f; // Assuming 14 bit resolution + + // implausibility values + constexpr float ANALOG_TOL = 0.005f; //+- 0.5% error + constexpr float DIGITAL_TOL_DEG = 0.2f; // +- 0.2 degrees error + + // rate of angle change + constexpr float MAX_DTHETA_THRESHOLD = 5.0f; //maximum change in angle since last reading to consider the reading valid } // software configuration constants @@ -166,4 +193,4 @@ namespace VCFTaskConstants { } -#endif /* VCF_CONSTANTS */ \ No newline at end of file +#endif /* VCF_CONSTANTS */ diff --git a/include/VCF_Tasks.h b/include/VCF_Tasks.h index beac165..ed18f62 100644 --- a/include/VCF_Tasks.h +++ b/include/VCF_Tasks.h @@ -34,6 +34,7 @@ #include "VCFCANInterfaceImpl.h" #include "VCFEthernetInterface.h" #include "PedalsSystem.h" +#include "SteeringSystem.h" #include "ht_sched.hpp" #include "ht_task.hpp" #include "BuzzerController.h" @@ -54,6 +55,7 @@ HT_TASK::TaskResponse run_kick_watchdog(const unsigned long& sysMicros, const HT HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); +HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); /** * NOTE: These channels are UNUSED BY DEFAULT and exist ONLY FOR TESTING. You * may edit this manually to add sensors. diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index b2647ef..6331e55 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -42,7 +42,7 @@ struct SteeringParams_s { float digital_tol_deg = 0.2f; // +- 0.2 degrees error // rate of angle change - float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid + float max_dtheta_threshold = 5.0f; //maximum change in angle since last reading to consider the reading valid // difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index eb31e49..e5eca88 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -5,7 +5,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on) { //get current raw angles const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); //NOLINT will eventually be uint32 - + //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ _calibrating = true; @@ -46,6 +46,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu _steeringParams.analog_max_with_margins = static_cast(_steeringParams.max_steering_signal_analog) + analog_margin_counts; _steeringParams.digital_min_with_margins = static_cast(_steeringParams.min_steering_signal_digital) - digital_margin_counts; _steeringParams.digital_max_with_margins = static_cast(_steeringParams.max_steering_signal_digital) + digital_margin_counts; + _steeringParams.error_between_sensors_tolerance = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg; } } @@ -80,9 +81,7 @@ void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steer //Check if there is too much of a difference between sensor values float sensor_difference = std::fabs(_steeringSystemData.analog_steering_angle - _steeringSystemData.digital_steering_angle); - const float allowed_difference = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg; //the allowed difference equals digital error+ analog error - //digital error is +- 0.2 degrees, constant regardless of angle, while analog depends on steering angle at 0.5% - bool sensors_agree = (sensor_difference <= allowed_difference); //steeringParams.error + bool sensors_agree = (sensor_difference <= _steeringParams.error_between_sensors_tolerance); //steeringParams.error _steeringSystemData.sensor_disagreement_implausibility = !sensors_agree; //create an algorithm/ checklist to determine which sensor we trust more, diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 5753e43..bce842d 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -13,6 +13,7 @@ #include "VCRInterface.h" #include "SystemTimeInterface.h" #include "PedalsSystem.h" +#include "SteeringSystem.h" #include "WatchdogSystem.h" #include "DashboardInterface.h" #include "VCFEthernetInterface.h" @@ -82,6 +83,20 @@ HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMic return HT_TASK::TaskResponse::YIELD; } +HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { + // If we have a button hold or something, maybe add something to only update_observed_steering limits when button held + SteeringSystemInstance::instance().update_observed_steering_limits(SteeringSystemInstance::instance().get_steering_sensor_data()); + + if (false /* TODO: IMPORTANT ADD SOMETHING FOR TRIGGERING CALIBRATION*/) { + SteeringSystemInstance::instance().recalibrate_steering_digital(SteeringSystemInstance::instance().get_steering_sensor_data(), false /* TODO: calibration trigger or something*/); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MIN_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params.min_steering_signal_digital); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params.max_steering_signal_digital); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.analog_min_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.analog_max_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.digital_min_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.digital_max_with_margins); + } +} // bool init_read_gpio_task() // { // // Setting digital/analog buttons D10-D6, A8 as inputs @@ -185,6 +200,7 @@ HT_TASK::TaskResponse enqueue_front_suspension_data(const unsigned long& sysMicr return HT_TASK::TaskResponse::YIELD; } +// TODO: update this and add any other sending data stuff HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { STEERING_DATA_t msg_out; @@ -393,6 +409,7 @@ namespace async_tasks PedalsSystemInstance::instance().get_pedals_sensor_data(), sys_time::hal_millis() )); + SteeringSystemInstance::instance().evaluate_steering(SteeringSystemInstance::instance().get_steering_sensor_data(), sys_time::hal_millis()) // Serial.println(VCFData_sInstance::instance().system_data.pedals_system_data.accel_percent); return HT_TASK::TaskResponse::YIELD; } @@ -553,6 +570,30 @@ void setup_all_interfaces() { PedalsSystemInstance::create(accel_params, brake_params); //pass in the two different params + SteeringParams_s steering_params = { + .min_steering_signal_analog = VCFSystemConstants::MIN_STEERING_SIGNAL_ANALOG, + .max_steering_signal_analog = VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG, + .min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), + .max_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), + .analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(), + .analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(), + .digital_min_with_margins = EEPROMUtilities::read_eeprom_32bit(), + .digital_max_with_margins = EEPROMUtilities::read_eeprom_32bit(), + .span_signal_analog = VCFSystemConstants::SPAN_SIGNAL_ANALOG, + .analog_midpoint = VCFSystemConstants::ANALOG_MIDPOINT, + .deg_per_count_analog = VCFSystemConstants::DEG_PER_COUNT_ANALOG, + .deg_per_count_digital = VCFSystemConstants::DEG_PER_COUNT_DIGITAL, + .analog_tol = VCFSystemConstants::ANALOG_TOL, + .digital_tol_deg = VCFSystemConstants::DIGITAL_TOL_DEG, + .max_dtheta_threshold = VCFSystemConstants::MAX_DTHETA_THRESHOLD, + }; + steering_params.span_signal_digital = steering_params.max_steering_signal_digital - steering_params.min_steering_signal_digital; + steering_params.digital_midpoint = (steering_params.min_steering_signal_digital + steering_params.max_steering_signal_digital) / 2; + steering_params.analog_tol_deg = static_cast(steering_params.span_signal_analog) * steering_params.analog_tol * steering_params.deg_per_count_analog; + steering_params.error_between_sensors_tolerance = steering_params.analog_tol_deg + steering_params.digital_tol_deg; + + SteeringSystemInstance::create(steering_params); + // Create dashboard singleton DashboardGPIOs_s dashboard_gpios = { .DIM_BUTTON = VCFInterfaceConstants::BTN_DIM_READ, diff --git a/src/main.cpp b/src/main.cpp index 74a4f97..e347f5f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -86,4 +86,4 @@ void setup() { void loop() { HT_SCHED::Scheduler::getInstance().run(); -} \ No newline at end of file +} From a96f276dd46fa8c0a68cd7635cd65525eb233324 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 2 Mar 2026 20:03:54 -0500 Subject: [PATCH 28/39] assigned analog values and added tasks in main --- include/VCF_Constants.h | 3 +++ src/VCF_Tasks.cpp | 18 +++++++++++------- src/main.cpp | 3 +++ 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/include/VCF_Constants.h b/include/VCF_Constants.h index cdab4ae..962f5b0 100644 --- a/include/VCF_Constants.h +++ b/include/VCF_Constants.h @@ -185,6 +185,9 @@ namespace VCFTaskConstants { constexpr unsigned long PEDALS_RECALIBRATION_PRIORITY = 150; constexpr unsigned long PEDALS_RECALIBRATION_PERIOD = 100000; // 100 000 us = 10 Hz + constexpr unsigned long STEERING_RECALIBRATION_PRIORITY = 150; // TODO: Determine real values for these + constexpr unsigned long STEERING_RECALIBRATION_PERIOD = 100000; + // IIR filter alphas constexpr float LOADCELL_IIR_FILTER_ALPHA = 0.01f; diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index bce842d..08c6f31 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -43,6 +43,10 @@ HT_TASK::TaskResponse run_read_adc1_task(const unsigned long& sysMicros, const H .brake_2 = static_cast(ADCInterfaceInstance::instance().brake_2().conversion), .pedal_pressure = 0 // Currently an unused field }); + SteeringSystemInstance::instance().set_steering_sensor_data(SteeringSensorData_s{ + .analog_steering_degrees = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw), + .digital_steering_analog = 0; /*TODO: Assign digital data*/ + }); return HT_TASK::TaskResponse::YIELD; } @@ -55,7 +59,7 @@ HT_TASK::TaskResponse init_kick_watchdog(const unsigned long& sysMicros, const H } HT_TASK::TaskResponse run_kick_watchdog(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - digitalWrite(VCFInterfaceConstants::SOFTWARE_OK_PIN , HIGH); + digitalWrite(VCFInterfaceConstants::SOFTWARE_OK_PIN, HIGH); digitalWrite(VCFInterfaceConstants::WATCHDOG_PIN, WatchdogInstance::instance().get_watchdog_state(sys_time::hal_millis())); return HT_TASK::TaskResponse::YIELD; } @@ -573,12 +577,12 @@ void setup_all_interfaces() { SteeringParams_s steering_params = { .min_steering_signal_analog = VCFSystemConstants::MIN_STEERING_SIGNAL_ANALOG, .max_steering_signal_analog = VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG, - .min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), - .max_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), - .analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(), - .analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(), - .digital_min_with_margins = EEPROMUtilities::read_eeprom_32bit(), - .digital_max_with_margins = EEPROMUtilities::read_eeprom_32bit(), + .min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::MIN_STEERING_SIGNAL_DIGITAL_ADDR), + .max_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_DIGITAL_ADDR), + .analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR), + .analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR), + .digital_min_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR), + .digital_max_with_margins = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR), .span_signal_analog = VCFSystemConstants::SPAN_SIGNAL_ANALOG, .analog_midpoint = VCFSystemConstants::ANALOG_MIDPOINT, .deg_per_count_analog = VCFSystemConstants::DEG_PER_COUNT_ANALOG, diff --git a/src/main.cpp b/src/main.cpp index e347f5f..7aa23a6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,6 +22,7 @@ #include "VCF_Constants.h" #include "VCF_Tasks.h" #include "PedalsSystem.h" +#include "SteeringSystem.h" #include "DashboardInterface.h" #include "VCFEthernetInterface.h" #include "WatchdogSystem.h" @@ -55,6 +56,7 @@ HT_TASK::Task front_suspension_message_enqueue(HT_TASK::DUMMY_FUNCTION, &enqueue HT_TASK::Task kick_watchdog_task(&init_kick_watchdog, &run_kick_watchdog, VCFTaskConstants::WATCHDOG_PRIORITY, VCFTaskConstants::WATCHDOG_KICK_PERIOD); HT_TASK::Task pedals_calibration_task(HT_TASK::DUMMY_FUNCTION, &update_pedals_calibration_task, VCFTaskConstants::PEDALS_RECALIBRATION_PRIORITY, VCFTaskConstants::PEDALS_RECALIBRATION_PERIOD); +HT_TASK::Task steering_calibration_task(HT_TASK::DUMMY_FUNCTION, &update_steering_calibration_task, VCFTaskConstants::STEERING_RECALIBRATION_PRIORITY, VCFTaskConstants::STEERING_RECALIBRATION_PERIOD); //HT_TASK::Task debug_state_print_task(HT_TASK::DUMMY_FUNCTION, &debug_print, VCFTaskConstants::DEBUG_PRIORITY, VCFTaskConstants::DEBUG_PERIOD); @@ -79,6 +81,7 @@ void setup() { HT_SCHED::Scheduler::getInstance().schedule(steering_message_enqueue); HT_SCHED::Scheduler::getInstance().schedule(front_suspension_message_enqueue); HT_SCHED::Scheduler::getInstance().schedule(pedals_calibration_task); + HT_SCHED::Scheduler::getInstance().schedule(steering_calibration_task); HT_SCHED::Scheduler::getInstance().schedule(ethernet_send_task); // HT_SCHED::Scheduler::getInstance().schedule(debug_state_print_task); From fe5bf4aeba9da8a3de3c071a5ab61696c7437206 Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Mon, 2 Mar 2026 20:30:53 -0500 Subject: [PATCH 29/39] Worked on unit tests, 2 fails remain --- test/test_systems/steering_system_test.h | 107 +++++++++++++---------- 1 file changed, 61 insertions(+), 46 deletions(-) diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index f2466af..5e4638d 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -10,10 +10,10 @@ SteeringParams_s gen_default_params(){ SteeringParams_s params; //hard code the parmas for sensors - params.min_steering_signal_analog = 0; + params.min_steering_signal_analog = 25; params.max_steering_signal_analog = 4095;//actual hard coded - params.min_steering_signal_digital = 0; + params.min_steering_signal_digital = 25; params.max_steering_signal_digital = 8000; //testing values params.span_signal_analog = 4095; @@ -70,7 +70,7 @@ static float expected_angle_from_midpoint(uint32_t raw, uint32_t min, uint32_t m */ -// fix +//FAIL TEST(SteeringSystemTesting, test_adc_to_degree_conversion) { auto params = gen_default_params(); @@ -83,71 +83,76 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) SteeringSensorData_s midpoint{}; midpoint.analog_steering_degrees = analog_mid; midpoint.digital_steering_analog = digital_mid; - auto data = steering.evaluate_steering(midpoint, 1000); - - EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f); - EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); + steering.evaluate_steering(midpoint, 1000); + auto data = steering.get_steering_system_data(); + EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f); //fail + EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); //fail //min values SteeringSensorData_s min_val{}; min_val.analog_steering_degrees = params.min_steering_signal_analog; min_val.digital_steering_analog = params.min_steering_signal_digital; - data = steering.evaluate_steering(min_val, 1010); + steering.evaluate_steering(min_val, 1010); + data = steering.get_steering_system_data(); float expected_analog_min = (static_cast(params.min_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; float expected_digital_min = (static_cast(params.min_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; - EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f); - EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); + EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f); //fail + EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); //fail //max values SteeringSensorData_s max_val{}; max_val.analog_steering_degrees = params.max_steering_signal_analog; max_val.digital_steering_analog = params.max_steering_signal_digital; - data = steering.evaluate_steering(max_val, 1020); + steering.evaluate_steering(max_val, 1020); + data = steering.get_steering_system_data(); float expected_analog_max = (static_cast(params.max_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; float expected_digital_max = (static_cast(params.max_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; - EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f); - EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); + EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f); //fail + EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); //fail } -//fix +//FIXED TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ - auto params = gen_default_params(); - params.min_steering_signal_analog = 1000; - params.min_steering_signal_digital = 1000; + uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; + uint32_t digital_mid = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; + + SteeringSystem steering(params); - //Check for a good value first - SteeringSensorData_s good_val = {}; - good_val.analog_steering_degrees = 2048; - good_val.digital_steering_analog = 4000; - auto data = steering.evaluate_steering(good_val, 1000); + //Check for known valid values first + SteeringSensorData_s midpoint{}; + midpoint.analog_steering_degrees = analog_mid; + midpoint.digital_steering_analog = digital_mid; + steering.evaluate_steering({midpoint}, 1000); + auto data = steering.get_steering_system_data(); EXPECT_FALSE(data.analog_oor_implausibility); EXPECT_FALSE(data.digital_oor_implausibility); //OOR High SteeringSensorData_s high_val = {5000, 9000}; - data = steering.evaluate_steering(high_val, 1010); + steering.evaluate_steering(high_val, 1010); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); //OOR Low - SteeringSensorData_s low_val = {static_cast(0), static_cast(0)}; - data = steering.evaluate_steering(low_val, 1020); + SteeringSensorData_s low_val = {static_cast(params.min_steering_signal_analog), static_cast(params.max_steering_signal_analog)}; + steering.evaluate_steering(low_val, 1020); // data = steering.evaluate_steering(low_val, 1030); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); - EXPECT_TRUE(data.digital_oor_implausibility); + EXPECT_TRUE(data.digital_oor_implausibility); //actual false, expected true } -//fix TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ auto params = gen_default_params(); SteeringSystem steering(params); @@ -155,12 +160,14 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ SteeringSensorData_s baseline = {2048, 4000}; //First run (just to verify that we don't get a dtheta exceeded on the first run since we have no previous data to compare to) - auto data = steering.evaluate_steering(baseline, 1000); + steering.evaluate_steering(baseline, 1000); + auto data = steering.get_steering_system_data(); EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); SteeringSensorData_s massive_jump = {4096, 8000}; - data = steering.evaluate_steering(massive_jump, 1005); + steering.evaluate_steering(massive_jump, 1005); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.dtheta_exceeded_analog); EXPECT_TRUE(data.dtheta_exceeded_digital); @@ -169,12 +176,14 @@ TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ steering.evaluate_steering(baseline, 1010); //Small motion valid SteeringSensorData_s small_motion = {2060, 4050}; - data = steering.evaluate_steering(small_motion, 1020); //advance time by 10 ms + steering.evaluate_steering(small_motion, 1020); //advance time by 10 ms + data = steering.get_steering_system_data(); EXPECT_FALSE(data.dtheta_exceeded_analog); EXPECT_FALSE(data.dtheta_exceeded_digital); //Big motion NOT valid - data = steering.evaluate_steering(massive_jump, 1030); //advance time by another 10 ms + steering.evaluate_steering(massive_jump, 1030); //advance time by another 10 ms + data = steering.get_steering_system_data(); EXPECT_TRUE(data.dtheta_exceeded_analog); EXPECT_TRUE(data.dtheta_exceeded_digital); } @@ -196,11 +205,12 @@ TEST(SteeringSystemTesting, test_sensor_disagreement) disagree.analog_steering_degrees = 2000; //stays same disagree.digital_steering_analog = 7000; //large offset from analog - auto data = steering.evaluate_steering(disagree, 1100); + steering.evaluate_steering(disagree, 1100); + auto data = steering.get_steering_system_data(); EXPECT_TRUE(data.sensor_disagreement_implausibility); } -// fix +// FAIL TEST(SteeringSystemTesting,test_sensor_output_logic){ auto params = gen_default_params(); @@ -208,8 +218,9 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ //When both valid and agreeing, we default to digital SteeringSensorData_s both_valid = {2048,4000}; - auto data = steering.evaluate_steering(both_valid, 1000); - EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + steering.evaluate_steering(both_valid, 1000); + auto data = steering.get_steering_system_data(); + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //probably same problem, recheck after fix EXPECT_FALSE(data.both_sensors_fail); EXPECT_FALSE(data.sensor_disagreement_implausibility); @@ -218,30 +229,34 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ //When both valid but disagreeing, we default to digital SteeringSensorData_s both_valid_disagree = {2048, 7000}; - data = steering.evaluate_steering(both_valid_disagree, 1010); + steering.evaluate_steering(both_valid_disagree, 1010); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.sensor_disagreement_implausibility); - EXPECT_FALSE(data.analog_oor_implausibility); - EXPECT_FALSE(data.digital_oor_implausibility); - EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + EXPECT_FALSE(data.analog_oor_implausibility); //actual true, expected false + EXPECT_FALSE(data.digital_oor_implausibility); //actual true, expected false + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //When analog is good but digital is bad, we put analog SteeringSensorData_s digital_bad = {2048, 9000}; - data = steering.evaluate_steering(digital_bad, 1020); + steering.evaluate_steering(digital_bad, 1020); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.digital_oor_implausibility); - EXPECT_FALSE(data.analog_oor_implausibility); - EXPECT_NEAR(data.output_steering_angle, data.analog_steering_angle, 0.001f); + EXPECT_FALSE(data.analog_oor_implausibility); //actual true, expected false + EXPECT_NEAR(data.output_steering_angle, data.analog_steering_angle, 0.001f); //propgated //When digital is good but analog is bad, we put digital SteeringSensorData_s analog_bad = {5000, 4000}; - data = steering.evaluate_steering(analog_bad, 1030); + steering.evaluate_steering(analog_bad, 1030); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); - EXPECT_FALSE(data.digital_oor_implausibility); - EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); + EXPECT_FALSE(data.digital_oor_implausibility); //actual true, expected false + EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //propagated from 253 //When both bad, we flagging that error SteeringSensorData_s both_bad = {5000, 9000}; - data = steering.evaluate_steering(both_bad, 1040); + steering.evaluate_steering(both_bad, 1040); + data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); EXPECT_TRUE(data.both_sensors_fail); From 708563921b6480a26d8b016fa42691e6c1593cac Mon Sep 17 00:00:00 2001 From: William Schotte Date: Mon, 2 Mar 2026 21:12:39 -0500 Subject: [PATCH 30/39] added addresses for EEPROM and edited CAN steering enqueue --- include/VCF_Constants.h | 12 ++++++------ lib/interfaces/src/ADCInterface.cpp | 4 ++-- lib/systems/include/SteeringSystem.h | 16 ++++++++-------- src/VCF_Tasks.cpp | 7 ++++--- 4 files changed, 20 insertions(+), 19 deletions(-) diff --git a/include/VCF_Constants.h b/include/VCF_Constants.h index 962f5b0..cf18733 100644 --- a/include/VCF_Constants.h +++ b/include/VCF_Constants.h @@ -115,13 +115,13 @@ namespace VCFSystemConstants { // TODO: determine addresses constexpr uint32_t MIN_STEERING_SIGNAL_ANALOG = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) constexpr uint32_t MAX_STEERING_SIGNAL_ANALOG = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle - constexpr uint32_t MIN_STEERING_SIGNAL_DIGITAL_ADDR = 57; //Raw ADC value from digital sensor at minimum (left) steering angle - constexpr uint32_t MAX_STEERING_SIGNAL_DIGITAL_ADDR = 57; //Raw ADC value from digital sensor at maximum (right) steering angle + constexpr uint32_t MIN_STEERING_SIGNAL_DIGITAL_ADDR = 32; //Raw ADC value from digital sensor at minimum (left) steering angle + constexpr uint32_t MAX_STEERING_SIGNAL_DIGITAL_ADDR = 36; //Raw ADC value from digital sensor at maximum (right) steering angle - constexpr int32_t ANALOG_MIN_WITH_MARGINS_ADDR = 57; - constexpr int32_t ANALOG_MAX_WITH_MARGINS_ADDR = 57; - constexpr int32_t DIGITAL_MIN_WITH_MARGINS_ADDR = 57; - constexpr int32_t DIGITAL_MAX_WITH_MARGINS_ADDR = 57; + constexpr int32_t ANALOG_MIN_WITH_MARGINS_ADDR = 40; + constexpr int32_t ANALOG_MAX_WITH_MARGINS_ADDR = 44; + constexpr int32_t DIGITAL_MIN_WITH_MARGINS_ADDR = 48; + constexpr int32_t DIGITAL_MAX_WITH_MARGINS_ADDR = 52; constexpr uint32_t SPAN_SIGNAL_ANALOG = MAX_STEERING_SIGNAL_ANALOG - MIN_STEERING_SIGNAL_ANALOG; constexpr int32_t ANALOG_MIDPOINT = (MIN_STEERING_SIGNAL_ANALOG + MAX_STEERING_SIGNAL_ANALOG) / 2; diff --git a/lib/interfaces/src/ADCInterface.cpp b/lib/interfaces/src/ADCInterface.cpp index 6250522..c3cd894 100644 --- a/lib/interfaces/src/ADCInterface.cpp +++ b/lib/interfaces/src/ADCInterface.cpp @@ -5,7 +5,7 @@ std::array ADCInterface: std::array scales = {}; scales[_adc_parameters.channels.steering_cw_channel] = _adc_parameters.scales.steering_cw_scale; - scales[_adc_parameters.channels.steering_ccw_channel] = _adc_parameters.scales.steering_ccw_scale; + scales[_adc_parameters.channels.steering_ccw_channel] = _adc_parameters.scales.steering_ccw_scale; //TODO: Why is there ccw? scales[_adc_parameters.channels.fr_loadcell_channel] = _adc_parameters.scales.fr_loadcell_scale; scales[_adc_parameters.channels.fl_loadcell_channel] = _adc_parameters.scales.fl_loadcell_scale; scales[_adc_parameters.channels.fr_suspot_channel] = _adc_parameters.scales.fr_suspot_scale; @@ -143,4 +143,4 @@ AnalogConversion_s ADCInterface::brake_1() { AnalogConversion_s ADCInterface::brake_2() { return _adc1.data.conversions[_adc_parameters.channels.brake_2_channel]; -} \ No newline at end of file +} diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 6331e55..d3d5b5e 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -10,8 +10,8 @@ struct SteeringParams_s { // raw ADC input signals - uint32_t min_steering_signal_analog = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) - uint32_t max_steering_signal_analog = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle + uint32_t min_steering_signal_analog; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) + uint32_t max_steering_signal_analog; //Raw ADC value from analog sensor at maximum (right) steering angle uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle @@ -20,7 +20,7 @@ struct SteeringParams_s { int32_t digital_min_with_margins; int32_t digital_max_with_margins; - uint32_t span_signal_analog = 4095; + uint32_t span_signal_analog; uint32_t span_signal_digital; int32_t digital_midpoint; int32_t analog_midpoint; @@ -33,16 +33,16 @@ struct SteeringParams_s { // conversion rates // float deg_per_count_analog = 0.0439f; //hard coded for analog (180) - float deg_per_count_analog = 0.087890625f; - float deg_per_count_digital = 0.02197265625f; //based on digital readings + float deg_per_count_analog; + float deg_per_count_digital; //based on digital readings // implausibility values - float analog_tol = 0.005f; //+- 0.5% error + float analog_tol; //+- 0.5% error float analog_tol_deg; - float digital_tol_deg = 0.2f; // +- 0.2 degrees error + float digital_tol_deg; // +- 0.2 degrees error // rate of angle change - float max_dtheta_threshold = 5.0f; //maximum change in angle since last reading to consider the reading valid + float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid // difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 08c6f31..f65a43b 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -208,9 +208,10 @@ HT_TASK::TaskResponse enqueue_front_suspension_data(const unsigned long& sysMicr HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { STEERING_DATA_t msg_out; - - msg_out.steering_analog_raw = ADCInterfaceInstance::instance().steering_degrees_cw().raw; - msg_out.steering_digital_raw = 0; //NOLINT VCFData_sInstance::instance().interface_data.steering_data.digital_steering_analog; + SteeringSystemData_s steering_system_data = SteeringSystemInstance::instance().get_steering_system_data(); + /* TODO: Change steering_*_raw to new values we have to add to CAN library. Also add other msg_out variables for implausibilities*/ + msg_out.steering_analog_raw = steering_system_data.analog_steering_angle; + msg_out.steering_digital_raw = steering_system_data.digital_steering_angle; //NOLINT VCFData_sInstance::instance().interface_data.steering_data.digital_steering_analog; CAN_util::enqueue_msg(&msg_out, &Pack_STEERING_DATA_hytech, VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance().main_can_tx_buffer); return HT_TASK::TaskResponse::YIELD; From 78e19eeceade7067d480a04b32c57454edf7b43a Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Wed, 4 Mar 2026 19:28:45 -0500 Subject: [PATCH 31/39] bug fixes --- src/VCF_Tasks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index bce842d..18fe227 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -573,7 +573,7 @@ void setup_all_interfaces() { SteeringParams_s steering_params = { .min_steering_signal_analog = VCFSystemConstants::MIN_STEERING_SIGNAL_ANALOG, .max_steering_signal_analog = VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG, - .min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), + .min_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(VCFSystemConstants::m), .max_steering_signal_digital = EEPROMUtilities::read_eeprom_32bit(), .analog_min_with_margins = EEPROMUtilities::read_eeprom_32bit(), .analog_max_with_margins = EEPROMUtilities::read_eeprom_32bit(), From 03d47d0318c4cf1d84d6be68babda77d8a7e874d Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Wed, 4 Mar 2026 20:26:55 -0500 Subject: [PATCH 32/39] only one unit test fails now --- include/VCF_Constants.h | 4 +- test/test_systems/steering_system_test.h | 106 +++++++++++++++-------- 2 files changed, 71 insertions(+), 39 deletions(-) diff --git a/include/VCF_Constants.h b/include/VCF_Constants.h index cf18733..61a3517 100644 --- a/include/VCF_Constants.h +++ b/include/VCF_Constants.h @@ -113,8 +113,8 @@ namespace VCFSystemConstants { // Steering System Constants // TODO: determine addresses - constexpr uint32_t MIN_STEERING_SIGNAL_ANALOG = 0; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) - constexpr uint32_t MAX_STEERING_SIGNAL_ANALOG = 4095; //Raw ADC value from analog sensor at maximum (right) steering angle + constexpr uint32_t MIN_STEERING_SIGNAL_ANALOG = 1024; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) TODO: test and find real values for min&max + constexpr uint32_t MAX_STEERING_SIGNAL_ANALOG = 3071; //Raw ADC value from analog sensor at maximum (right) steering angle constexpr uint32_t MIN_STEERING_SIGNAL_DIGITAL_ADDR = 32; //Raw ADC value from digital sensor at minimum (left) steering angle constexpr uint32_t MAX_STEERING_SIGNAL_DIGITAL_ADDR = 36; //Raw ADC value from digital sensor at maximum (right) steering angle diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 1687588..1792fa8 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -10,8 +10,8 @@ SteeringParams_s gen_default_params(){ SteeringParams_s params; //hard code the parmas for sensors - params.min_steering_signal_analog = 25; - params.max_steering_signal_analog = 4095;//actual hard coded + params.min_steering_signal_analog = 1024; + params.max_steering_signal_analog = 3071;//actual hard coded params.min_steering_signal_digital = 25; params.max_steering_signal_digital = 8000; //testing values @@ -25,11 +25,16 @@ SteeringParams_s gen_default_params(){ params.deg_per_count_analog = 0.087890625f; params.deg_per_count_digital = 0.02197265625f; + params.analog_tol = 0.005f; params.analog_tol_deg = 0.11377778f; params.digital_tol_deg = 0.2f; params.max_dtheta_threshold = 5.0f;//change params.error_between_sensors_tolerance = 0.31377778f; + + params.digital_midpoint = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; + params.analog_midpoint = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; + return params; } @@ -70,7 +75,7 @@ static float expected_angle_from_midpoint(uint32_t raw, uint32_t min, uint32_t m */ -//FAIL +//FIXED TEST(SteeringSystemTesting, test_adc_to_degree_conversion) { auto params = gen_default_params(); @@ -85,8 +90,8 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) midpoint.digital_steering_analog = digital_mid; steering.evaluate_steering(midpoint, 1000); auto data = steering.get_steering_system_data(); - EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f); //fail - EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); //fail + EXPECT_NEAR(data.analog_steering_angle, 0.0f, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, 0.0f, 0.001f); //min values SteeringSensorData_s min_val{}; @@ -96,11 +101,11 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) steering.evaluate_steering(min_val, 1010); data = steering.get_steering_system_data(); - float expected_analog_min = (static_cast(params.min_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; - float expected_digital_min = (static_cast(params.min_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; + float expected_analog_min = (static_cast(params.min_steering_signal_analog) - static_cast(analog_mid)) * params.deg_per_count_analog; + float expected_digital_min = (static_cast(params.min_steering_signal_digital) - static_cast(digital_mid)) * params.deg_per_count_digital; - EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f); //fail - EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); //fail + EXPECT_NEAR(data.analog_steering_angle, expected_analog_min, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); //max values SteeringSensorData_s max_val{}; @@ -109,11 +114,11 @@ TEST(SteeringSystemTesting, test_adc_to_degree_conversion) steering.evaluate_steering(max_val, 1020); data = steering.get_steering_system_data(); - float expected_analog_max = (static_cast(params.max_steering_signal_analog) - analog_mid) * params.deg_per_count_analog; - float expected_digital_max = (static_cast(params.max_steering_signal_digital) - digital_mid) * params.deg_per_count_digital; + float expected_analog_max = (static_cast(params.max_steering_signal_analog) - static_cast(analog_mid)) * params.deg_per_count_analog; + float expected_digital_max = (static_cast(params.max_steering_signal_digital) - static_cast(digital_mid)) * params.deg_per_count_digital; - EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f); //fail - EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); //fail + EXPECT_NEAR(data.analog_steering_angle, expected_analog_max, 0.001f); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); } @@ -130,7 +135,7 @@ TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ SteeringSensorData_s midpoint{}; midpoint.analog_steering_degrees = analog_mid; midpoint.digital_steering_analog = digital_mid; - steering.evaluate_steering({midpoint}, 1000); + steering.evaluate_steering(midpoint, 1000); auto data = steering.get_steering_system_data(); EXPECT_FALSE(data.analog_oor_implausibility); EXPECT_FALSE(data.digital_oor_implausibility); @@ -149,7 +154,7 @@ TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ // data = steering.evaluate_steering(low_val, 1030); data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); - EXPECT_TRUE(data.digital_oor_implausibility); //actual false, expected true// check again if + EXPECT_TRUE(data.digital_oor_implausibility); } @@ -212,55 +217,82 @@ TEST(SteeringSystemTesting, test_sensor_disagreement) // FAIL TEST(SteeringSystemTesting,test_sensor_output_logic){ - auto params = gen_default_params(); SteeringSystem steering(params); + uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; + uint32_t digital_mid = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; + +{ //When both valid and agreeing, we default to digital - SteeringSensorData_s both_valid = {2048,4000}; + SteeringSystem steering(params); + SteeringSensorData_s both_valid {}; + both_valid.analog_steering_degrees = analog_mid; + both_valid.digital_steering_analog = digital_mid; steering.evaluate_steering(both_valid, 1000); + steering.evaluate_steering(both_valid, 1100); + auto data = steering.get_steering_system_data(); EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //probably same problem, recheck after fix - EXPECT_FALSE(data.both_sensors_fail); + EXPECT_FALSE(data.both_sensors_fail); EXPECT_FALSE(data.sensor_disagreement_implausibility); - +} // Prevent dtheta exceeded for the next test - steering.evaluate_steering({2048, 7000}, 1000); - + +{ //When both valid but disagreeing, we default to digital - SteeringSensorData_s both_valid_disagree = {2048, 7000}; - steering.evaluate_steering(both_valid_disagree, 1010); - data = steering.get_steering_system_data(); + SteeringSystem steering(params); + SteeringSensorData_s both_valid_disagree {}; + both_valid_disagree.analog_steering_degrees = analog_mid; + both_valid_disagree.digital_steering_analog = digital_mid+3000; //large offset from analog + steering.evaluate_steering(both_valid_disagree, 1000); + steering.evaluate_steering(both_valid_disagree, 1100); + auto data = steering.get_steering_system_data(); EXPECT_TRUE(data.sensor_disagreement_implausibility); EXPECT_FALSE(data.analog_oor_implausibility); //actual true, expected false EXPECT_FALSE(data.digital_oor_implausibility); //actual true, expected false EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); - +} +{ //When analog is good but digital is bad, we put analog - SteeringSensorData_s digital_bad = {2048, 9000}; - steering.evaluate_steering(digital_bad, 1020); - data = steering.get_steering_system_data(); + SteeringSystem steering(params); + SteeringSensorData_s digital_bad {}; + digital_bad.analog_steering_degrees = analog_mid; + digital_bad.digital_steering_analog = params.max_steering_signal_digital + 1000; //bad digital + steering.evaluate_steering(digital_bad, 1000); + steering.evaluate_steering(digital_bad, 1100); + auto data = steering.get_steering_system_data(); EXPECT_TRUE(data.digital_oor_implausibility); EXPECT_FALSE(data.analog_oor_implausibility); //actual true, expected false EXPECT_NEAR(data.output_steering_angle, data.analog_steering_angle, 0.001f); //propgated - +} +{ //When digital is good but analog is bad, we put digital - SteeringSensorData_s analog_bad = {5000, 4000}; - steering.evaluate_steering(analog_bad, 1030); - data = steering.get_steering_system_data(); + SteeringSystem steering(params); + SteeringSensorData_s analog_bad {}; + analog_bad.analog_steering_degrees = params.max_steering_signal_analog + 1000; + analog_bad.digital_steering_analog = digital_mid; + steering.evaluate_steering(analog_bad, 1000); + steering.evaluate_steering(analog_bad, 1005); + auto data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_FALSE(data.digital_oor_implausibility); //actual true, expected false EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //propagated from 253 - +} +{ //When both bad, we flagging that error - SteeringSensorData_s both_bad = {5000, 9000}; - steering.evaluate_steering(both_bad, 1040); - data = steering.get_steering_system_data(); + SteeringSystem steering(params); + SteeringSensorData_s both_bad {}; + both_bad.analog_steering_degrees = params.max_steering_signal_analog + 1000; + both_bad.digital_steering_analog = params.max_steering_signal_digital + 1000; + steering.evaluate_steering(both_bad, 1000); + steering.evaluate_steering(both_bad, 1005); + auto data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); EXPECT_TRUE(data.both_sensors_fail); - +} } From a5674c29b7ed564d01c515b689ed3ff5f94bce90 Mon Sep 17 00:00:00 2001 From: bmorrissey9 Date: Sun, 8 Mar 2026 19:10:04 -0400 Subject: [PATCH 33/39] =?UTF-8?q?ALL=20THE=20UNIT=20TESTS=20PASS!=20?= =?UTF-8?q?=F0=9F=92=AF=F0=9F=92=AF=F0=9F=99=8F=F0=9F=99=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/test_systems/steering_system_test.h | 121 +++++++++++++++++++++-- 1 file changed, 114 insertions(+), 7 deletions(-) diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index 1792fa8..2ca7bb9 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -8,7 +8,7 @@ #include SteeringParams_s gen_default_params(){ - SteeringParams_s params; + SteeringParams_s params{}; //hard code the parmas for sensors params.min_steering_signal_analog = 1024; params.max_steering_signal_analog = 3071;//actual hard coded @@ -34,7 +34,15 @@ SteeringParams_s gen_default_params(){ params.digital_midpoint = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; params.analog_midpoint = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; - + + + const int32_t analog_margin_counts = static_cast(params.analog_tol * static_cast(params.span_signal_analog)); + const int32_t digital_margin_counts = static_cast(params.digital_tol_deg / params.deg_per_count_digital); + + params.analog_min_with_margins = static_cast(params.min_steering_signal_analog) - analog_margin_counts; + params.analog_max_with_margins = static_cast(params.max_steering_signal_analog) + analog_margin_counts; + params.digital_min_with_margins = static_cast(params.min_steering_signal_digital) - digital_margin_counts; + params.digital_max_with_margins = static_cast(params.max_steering_signal_digital) + digital_margin_counts; return params; } @@ -149,9 +157,10 @@ TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ //OOR Low - SteeringSensorData_s low_val = {static_cast(params.min_steering_signal_analog), static_cast(params.max_steering_signal_analog)}; + SteeringSensorData_s low_val = {static_cast(params.min_steering_signal_analog)-50, static_cast(params.min_steering_signal_digital)-10}; steering.evaluate_steering(low_val, 1020); - // data = steering.evaluate_steering(low_val, 1030); + steering.evaluate_steering(low_val, 1030); + //data = steering.evaluate_steering(low_val, 1030); data = steering.get_steering_system_data(); EXPECT_TRUE(data.analog_oor_implausibility); EXPECT_TRUE(data.digital_oor_implausibility); @@ -215,10 +224,9 @@ TEST(SteeringSystemTesting, test_sensor_disagreement) EXPECT_TRUE(data.sensor_disagreement_implausibility); } -// FAIL TEST(SteeringSystemTesting,test_sensor_output_logic){ auto params = gen_default_params(); - SteeringSystem steering(params); + uint32_t analog_mid = (params.min_steering_signal_analog + params.max_steering_signal_analog) / 2; uint32_t digital_mid = (params.min_steering_signal_digital + params.max_steering_signal_digital) / 2; @@ -234,7 +242,7 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ auto data = steering.get_steering_system_data(); EXPECT_NEAR(data.output_steering_angle, data.digital_steering_angle, 0.001f); //probably same problem, recheck after fix - EXPECT_FALSE(data.both_sensors_fail); + EXPECT_FALSE(data.both_sensors_fail); //fail EXPECT_FALSE(data.sensor_disagreement_implausibility); } // Prevent dtheta exceeded for the next test @@ -296,3 +304,102 @@ TEST(SteeringSystemTesting,test_sensor_output_logic){ } + +TEST(SteeringSystemTesting, test_recalibrate_steering_digital) +{ + auto params = gen_default_params(); + SteeringSystem steering(params); + + // Build calibration samples from the original digital range + const uint32_t original_digital_min = params.min_steering_signal_digital; + const uint32_t original_digital_max = params.max_steering_signal_digital; + const uint32_t original_digital_mid = params.digital_midpoint; + + // Pick values inside the original range so calibration observes a new min/max + const uint32_t observed_low = original_digital_min + (original_digital_mid - original_digital_min) / 2; + const uint32_t observed_high = original_digital_mid + (original_digital_max - original_digital_mid) / 2; + + SteeringSensorData_s start_sample{}; + start_sample.analog_steering_degrees = static_cast(params.analog_midpoint); + start_sample.digital_steering_analog = observed_high; + + SteeringSensorData_s low_sample{}; + low_sample.analog_steering_degrees = static_cast(params.analog_midpoint); + low_sample.digital_steering_analog = observed_low; + + SteeringSensorData_s high_sample{}; + high_sample.analog_steering_degrees = static_cast(params.analog_midpoint); + high_sample.digital_steering_analog = observed_high; + + // Start calibration: first sample should seed both observed min and max + steering.recalibrate_steering_digital(start_sample, true); + auto updated_params = steering.get_steering_params(); + + EXPECT_EQ(updated_params.min_observed_digital, observed_high); + EXPECT_EQ(updated_params.max_observed_digital, observed_high); + + // Feed lower value -> updates min only + steering.recalibrate_steering_digital(low_sample, true); + updated_params = steering.get_steering_params(); + + EXPECT_EQ(updated_params.min_observed_digital, observed_low); + EXPECT_EQ(updated_params.max_observed_digital, observed_high); + + // Feed high value again -> max remains high + steering.recalibrate_steering_digital(high_sample, true); + updated_params = steering.get_steering_params(); + + EXPECT_EQ(updated_params.min_observed_digital, observed_low); + EXPECT_EQ(updated_params.max_observed_digital, observed_high); + + // End calibration and commit recalculated values + steering.recalibrate_steering_digital(high_sample, false); + updated_params = steering.get_steering_params(); + + // Expected committed digital range + const uint32_t expected_min_digital = observed_low; + const uint32_t expected_max_digital = observed_high; + const uint32_t expected_span_digital = expected_max_digital - expected_min_digital; + const int32_t expected_digital_midpoint = static_cast((expected_max_digital + expected_min_digital) / 2); + + // Analog values should remain based on the original analog params + const uint32_t expected_span_analog = params.max_steering_signal_analog - params.min_steering_signal_analog; + const int32_t expected_analog_midpoint = static_cast((params.max_steering_signal_analog + params.min_steering_signal_analog) / 2); + + const float expected_analog_tol_deg = static_cast(params.span_signal_analog) *params.analog_tol * + params.deg_per_count_analog; + + const int32_t expected_analog_margin_counts = + static_cast(params.analog_tol * static_cast(params.span_signal_analog)); + + const int32_t expected_digital_margin_counts = static_cast(params.digital_tol_deg / params.deg_per_count_digital); + + const int32_t expected_analog_min_with_margins = static_cast(params.min_steering_signal_analog) - expected_analog_margin_counts; + const int32_t expected_analog_max_with_margins = static_cast(params.max_steering_signal_analog) + expected_analog_margin_counts; + + const int32_t expected_digital_min_with_margins = static_cast(expected_min_digital) - expected_digital_margin_counts; + const int32_t expected_digital_max_with_margins = static_cast(expected_max_digital) + expected_digital_margin_counts; + + const float expected_error_between_sensors_tolerance = expected_analog_tol_deg + params.digital_tol_deg; + + // Check committed digital calibration + EXPECT_EQ(updated_params.min_steering_signal_digital, expected_min_digital); + EXPECT_EQ(updated_params.max_steering_signal_digital, expected_max_digital); + EXPECT_EQ(updated_params.span_signal_digital, expected_span_digital); + EXPECT_EQ(updated_params.digital_midpoint, expected_digital_midpoint); + + // Check analog-derived values that get recomputed + EXPECT_EQ(updated_params.analog_midpoint, expected_analog_midpoint); + EXPECT_FLOAT_EQ(updated_params.analog_tol_deg, expected_analog_tol_deg); + + // Check margins + EXPECT_EQ(updated_params.analog_min_with_margins, expected_analog_min_with_margins); + EXPECT_EQ(updated_params.analog_max_with_margins, expected_analog_max_with_margins); + EXPECT_EQ(updated_params.digital_min_with_margins, expected_digital_min_with_margins); + EXPECT_EQ(updated_params.digital_max_with_margins, expected_digital_max_with_margins); + + // Check combined disagreement tolerance + EXPECT_FLOAT_EQ(updated_params.error_between_sensors_tolerance, + expected_error_between_sensors_tolerance); +} + From f61d0b8321b94972710d974665571e2a9f38114c Mon Sep 17 00:00:00 2001 From: Carter Bliss Date: Wed, 11 Mar 2026 21:13:35 -0400 Subject: [PATCH 34/39] removing the sensordata input struct, and preparing for hardware test --- lib/systems/include/SteeringSystem.h | 9 -------- lib/systems/src/SteeringSystem.cpp | 34 ++++++++++++++-------------- platformio.ini | 2 +- 3 files changed, 18 insertions(+), 27 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index d3d5b5e..88fea22 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -83,10 +83,6 @@ class SteeringSystem { return _steeringSystemData; } - const SteeringSensorData_s &get_steering_sensor_data() const { - return _steeringSensorData; - } - // Setters void set_steering_params(const SteeringParams_s &steeringParams) { _steeringParams = steeringParams; @@ -95,10 +91,6 @@ class SteeringSystem { void set_steering_system_data(const SteeringSystemData_s &steeringSystemData) { _steeringSystemData = steeringSystemData; } - - void set_steering_sensor_data(const SteeringSensorData_s &steeringSensorData) { - _steeringSensorData = steeringSensorData; - } private: void update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data); @@ -116,7 +108,6 @@ class SteeringSystem { //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) bool _evaluate_steering_dtheta_exceeded(float dtheta); - SteeringSensorData_s _steeringSensorData {}; SteeringSystemData_s _steeringSystemData {}; SteeringParams_s _steeringParams; //track the state of our system from the previous tick to compare against current state for implausibility checks diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index e5eca88..2537b4f 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -2,9 +2,9 @@ #include #include "SteeringSystem.h" -void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on) { +void SteeringSystem::recalibrate_steering_digital(const uint_32 analog_raw, const uint32_t digital_raw, bool calibration_is_on) { //get current raw angles - const uint32_t curr_digital_raw = static_cast(current_steering_data.digital_steering_analog); //NOLINT will eventually be uint32 + const uint32_t curr_digital_raw = static_cast(digital_raw); //NOLINT will eventually be uint32 //button just pressed ->recalibration window if (calibration_is_on && !_calibrating){ @@ -14,7 +14,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } if (calibration_is_on && _calibrating) { - update_observed_steering_limits(current_steering_data); + update_observed_steering_limits(analog_raw, digital_raw); } //update relative extremeties @@ -50,7 +50,7 @@ void SteeringSystem::recalibrate_steering_digital(const SteeringSensorData_s &cu } } -void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steering_data, const uint32_t current_millis){ +void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis){ // Reset flags _steeringSystemData.digital_oor_implausibility = false; _steeringSystemData.analog_oor_implausibility = false; @@ -60,8 +60,8 @@ void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steer _steeringSystemData.both_sensors_fail = false; //Conversion from raw ADC to degrees - _steeringSystemData.analog_steering_angle = _convert_analog_sensor(current_steering_data); - _steeringSystemData.digital_steering_angle = _convert_digital_sensor(current_steering_data); + _steeringSystemData.analog_steering_angle = _convert_analog_sensor(analog_raw); + _steeringSystemData.digital_steering_angle = _convert_digital_sensor(digital_raw); uint32_t dt = current_millis - _prev_timestamp; //current_millis is seperate data input @@ -76,8 +76,8 @@ void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steer _steeringSystemData.dtheta_exceeded_digital = _evaluate_steering_dtheta_exceeded(dtheta_digital); //Check if either sensor is out of range (pass in raw) - _steeringSystemData.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(current_steering_data.analog_steering_degrees)); - _steeringSystemData.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(current_steering_data.digital_steering_analog)); + _steeringSystemData.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast(analog_raw)); + _steeringSystemData.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast(digital_raw)); //Check if there is too much of a difference between sensor values float sensor_difference = std::fabs(_steeringSystemData.analog_steering_angle - _steeringSystemData.digital_steering_angle); @@ -112,28 +112,28 @@ void SteeringSystem::evaluate_steering(const SteeringSensorData_s ¤t_steer _first_run = false; } -void SteeringSystem::update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data) { - _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT should both be uint32_t - _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast(current_steering_data.digital_steering_analog)); //NOLINT ^ +void SteeringSystem::update_observed_steering_limits(const uint32_t analog_raw, const uint32_t digital_raw) { + _steeringParams.min_observed_digital = std::min(_steeringParams.min_observed_digital, static_cast(digital_raw)); //NOLINT should both be uint32_t + _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast(digital_raw)); //NOLINT ^ } -float SteeringSystem::_convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data) { +float SteeringSystem::_convert_digital_sensor(const uint32_t digital_raw) { // Same logic for digital - const int32_t offset = static_cast(current_steering_data.digital_steering_analog) - _steeringParams.digital_midpoint; //NOLINT + const int32_t offset = static_cast(digital_raw) - _steeringParams.digital_midpoint; //NOLINT return static_cast(offset) * _steeringParams.deg_per_count_digital; } -float SteeringSystem::_convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data) { +float SteeringSystem::_convert_analog_sensor(const uint32_t analog_raw) { // Get the raw value - const int32_t offset = static_cast(current_steering_data.analog_steering_degrees) - _steeringParams.analog_midpoint; //NOLINT + const int32_t offset = static_cast(analog_raw) - _steeringParams.analog_midpoint; //NOLINT return static_cast(offset) * _steeringParams.deg_per_count_analog; } -bool SteeringSystem::_evaluate_steering_oor_analog(uint32_t steering_analog_raw) { // RAW +bool SteeringSystem::_evaluate_steering_oor_analog(const uint32_t steering_analog_raw) { // RAW return (static_cast(steering_analog_raw) < _steeringParams.analog_min_with_margins || static_cast(steering_analog_raw) > _steeringParams.analog_max_with_margins); } -bool SteeringSystem::_evaluate_steering_oor_digital(uint32_t steering_digital_raw) {// RAW +bool SteeringSystem::_evaluate_steering_oor_digital(const uint32_t steering_digital_raw) {// RAW return (static_cast(steering_digital_raw) < _steeringParams.digital_min_with_margins || static_cast(steering_digital_raw) > _steeringParams.digital_max_with_margins); } diff --git a/platformio.ini b/platformio.ini index 34b3cdb..46f3ee6 100644 --- a/platformio.ini +++ b/platformio.ini @@ -7,7 +7,7 @@ lib_deps_shared = nanopb/Nanopb@0.4.91 https://github.com/hytech-racing/shared_firmware_systems.git - https://github.com/hytech-racing/shared_firmware_types.git + https://github.com/hytech-racing/shared_firmware_types.git#ab002af9b243979d49bcf7e6a8756c5afcbb9080 https://github.com/hytech-racing/HT_SCHED#c13cff762c59dd82a8c273e3e98fd1a80622656d https://github.com/hytech-racing/HT_proto/releases/download/2025-05-09T15_45_17/hytech_msgs_pb_lib.tar.gz etlcpp/Embedded Template Library From ffed5329a63457049df1b96caaf18755cb7691d3 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Thu, 12 Mar 2026 00:29:47 -0400 Subject: [PATCH 35/39] updated steering system tasks --- lib/systems/include/SteeringSystem.h | 45 +++++++--- lib/systems/src/SteeringSystem.cpp | 4 +- src/VCF_Tasks.cpp | 128 +++++++++++++++++++-------- 3 files changed, 128 insertions(+), 49 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 88fea22..e67c6b2 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -1,13 +1,16 @@ #ifndef STEERING_SYSTEM_H #define STEERING_SYSTEM_H + #include #include #include #include + #include "SharedFirmwareTypes.h" + struct SteeringParams_s { // raw ADC input signals uint32_t min_steering_signal_analog; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration) @@ -15,27 +18,32 @@ struct SteeringParams_s { uint32_t min_steering_signal_digital; //Raw ADC value from digital sensor at minimum (left) steering angle uint32_t max_steering_signal_digital; //Raw ADC value from digital sensor at maximum (right) steering angle + int32_t analog_min_with_margins; int32_t analog_max_with_margins; int32_t digital_min_with_margins; int32_t digital_max_with_margins; + uint32_t span_signal_analog; uint32_t span_signal_digital; int32_t digital_midpoint; int32_t analog_midpoint; + // calibration limits uint32_t min_observed_digital; uint32_t max_observed_digital; uint32_t min_observed_analog; // do we need to do min/max calibration for analog? - uint32_t max_observed_analog; + uint32_t max_observed_analog; + // conversion rates // float deg_per_count_analog = 0.0439f; //hard coded for analog (180) float deg_per_count_analog; float deg_per_count_digital; //based on digital readings + // implausibility values float analog_tol; //+- 0.5% error float analog_tol_deg; @@ -44,19 +52,26 @@ struct SteeringParams_s { // rate of angle change float max_dtheta_threshold; //maximum change in angle since last reading to consider the reading valid + // difference rating float error_between_sensors_tolerance; //maximum difference between digital and analog sensor allowed }; + struct SteeringSystemData_s { + uint32_t analog_raw; + uint32_t digital_raw; + float analog_steering_angle; //in degrees float digital_steering_angle; //in degrees float output_steering_angle; // represents the better output of the two sensors or some combination of the values + float analog_steering_velocity_deg_s; //in degrees per second float digital_steering_velocity_deg_s; + bool digital_oor_implausibility; bool analog_oor_implausibility; bool sensor_disagreement_implausibility; @@ -65,49 +80,57 @@ struct SteeringSystemData_s bool both_sensors_fail; }; + class SteeringSystem { public: SteeringSystem(const SteeringParams_s &steeringParams) : _steeringParams(steeringParams) {} + // Functions - void recalibrate_steering_digital(const SteeringSensorData_s ¤t_steering_data, bool calibration_is_on); - - void evaluate_steering(const SteeringSensorData_s ¤t_steering_data, uint32_t current_millis); + void recalibrate_steering_digital(const uint32_t analog_raw, const uint32_t digital_raw, bool calibration_is_on); + + void evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis); + // Getters const SteeringParams_s &get_steering_params() const { return _steeringParams; } + const SteeringSystemData_s &get_steering_system_data() const { return _steeringSystemData; } + // Setters void set_steering_params(const SteeringParams_s &steeringParams) { _steeringParams = steeringParams; } + void set_steering_system_data(const SteeringSystemData_s &steeringSystemData) { _steeringSystemData = steeringSystemData; } - + private: - void update_observed_steering_limits(const SteeringSensorData_s ¤t_steering_data); + void update_observed_steering_limits(const uint32_t analog_raw, const uint32_t digital_raw); - float _convert_digital_sensor(const SteeringSensorData_s ¤t_steering_data); + float _convert_digital_sensor(const uint32_t digital_raw); - float _convert_analog_sensor(const SteeringSensorData_s ¤t_steering_data); + float _convert_analog_sensor(const uint32_t analog_raw); //returns true if steering_analog is outside of the range defined by min and max sensor values - bool _evaluate_steering_oor_analog(uint32_t steering_analog); + bool _evaluate_steering_oor_analog(const uint32_t steering_analog); //returns true if steering_digital is outside the range defined by min and max sensor values - bool _evaluate_steering_oor_digital(uint32_t steering_digital); + bool _evaluate_steering_oor_digital(const uint32_t steering_digital); + //returns true if change in angle exceeds maximum change per reading ( max_dtheta_threshold ) bool _evaluate_steering_dtheta_exceeded(float dtheta); + SteeringSystemData_s _steeringSystemData {}; SteeringParams_s _steeringParams; //track the state of our system from the previous tick to compare against current state for implausibility checks @@ -118,6 +141,8 @@ class SteeringSystem { bool _first_run = true; // skip dTheta check on the very first tick }; + using SteeringSystemInstance = etl::singleton; + #endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 2537b4f..f368056 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -50,7 +50,7 @@ void SteeringSystem::recalibrate_steering_digital(const uint_32 analog_raw, cons } } -void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis){ +void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis) { // Reset flags _steeringSystemData.digital_oor_implausibility = false; _steeringSystemData.analog_oor_implausibility = false; @@ -59,6 +59,8 @@ void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t _steeringSystemData.dtheta_exceeded_digital = false; _steeringSystemData.both_sensors_fail = false; + _steeringSystemData.analog_raw = analog_raw; + _steeringSystemData.digital_raw = digital_raw; //Conversion from raw ADC to degrees _steeringSystemData.analog_steering_angle = _convert_analog_sensor(analog_raw); _steeringSystemData.digital_steering_angle = _convert_digital_sensor(digital_raw); diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index a6b063f..9e61fc2 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -50,10 +50,7 @@ HT_TASK::TaskResponse run_read_adc1_task(const unsigned long& sysMicros, const H .brake_2 = static_cast(ADCInterfaceInstance::instance().brake_2().conversion), .pedal_pressure = 0 // Currently an unused field }); - SteeringSystemInstance::instance().set_steering_sensor_data(SteeringSensorData_s{ - .analog_steering_degrees = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw), - .digital_steering_analog = 0; /*TODO: Assign digital data*/ - }); + ADCInterfaceInstance::instance().update_filtered_values(VCFTaskConstants::LOADCELL_IIR_FILTER_ALPHA); return HT_TASK::TaskResponse::YIELD; } @@ -96,16 +93,18 @@ HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMic HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { // If we have a button hold or something, maybe add something to only update_observed_steering limits when button held - SteeringSystemInstance::instance().update_observed_steering_limits(SteeringSystemInstance::instance().get_steering_sensor_data()); + const uint32_t analog_raw = SteeringSystemInstance::instance().get_steering_system_data().analog_raw; + const uint32_t digital_raw = SteeringSystemInstance::instance().get_steering_system_data().digital_raw; + SteeringSystemInstance::instance().update_observed_steering_limits(analog_raw, digital_raw); if (false /* TODO: IMPORTANT ADD SOMETHING FOR TRIGGERING CALIBRATION*/) { - SteeringSystemInstance::instance().recalibrate_steering_digital(SteeringSystemInstance::instance().get_steering_sensor_data(), false /* TODO: calibration trigger or something*/); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MIN_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params.min_steering_signal_digital); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params.max_steering_signal_digital); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.analog_min_with_margins); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.analog_max_with_margins); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.digital_min_with_margins); - EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params.digital_max_with_margins); + SteeringSystemInstance::instance().recalibrate_steering_digital(analog_raw, digital_raw, false /* TODO: calibration trigger or something*/); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MIN_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params().min_steering_signal_digital); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::MAX_STEERING_SIGNAL_DIGITAL_ADDR, SteeringSystemInstance::instance().get_steering_params().max_steering_signal_digital); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params().analog_min_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::ANALOG_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params().analog_max_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MIN_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params().digital_min_with_margins); + EEPROMUtilities::write_eeprom_32bit(VCFSystemConstants::DIGITAL_MAX_WITH_MARGINS_ADDR, SteeringSystemInstance::instance().get_steering_params().digital_max_with_margins); } } // bool init_read_gpio_task() @@ -117,7 +116,7 @@ HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysM // pinMode(BTN_MODE_READ, INPUT); // pinMode(BTN_START_READ, INPUT); // pinMode(BTN_DATA_READ, INPUT); - + // return HT_TASK::TaskResponse::YIELD; // } // bool run_read_gpio_task() @@ -129,7 +128,7 @@ HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysM // int modeButton = digitalRead(BTN_MODE_READ); // int startButton = digitalRead(BTN_START_READ); // int dataButton = digitalRead(BTN_DATA_READ); - + // vcf_data.interface_data.dash_input_state.dim_btn_is_pressed = dimButton; // vcf_data.interface_data.dash_input_state.preset_btn_is_pressed = presetButton; // vcf_data.interface_data.dash_input_state.mc_reset_btn_is_pressed = mcCycleButton; @@ -153,7 +152,7 @@ HT_TASK::TaskResponse run_buzzer_control_task(const unsigned long& sysMicros, co bool buzzer_is_active = BuzzerController::getInstance().buzzer_is_active(sys_time::hal_millis()); //NOLINT digitalWrite(VCFInterfaceConstants::BUZZER_CONTROL_PIN, buzzer_is_active); - + return HT_TASK::TaskResponse::YIELD; } @@ -167,14 +166,14 @@ HT_TASK::TaskResponse handle_CAN_send(const unsigned long& sysMicros, const HT_T HT_TASK::TaskResponse handle_CAN_receive(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { VCFCANInterfaceObjects& vcf_interface_objects = VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance(); - CANInterfaces& vcf_can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); + CANInterfaces& vcf_can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); process_ring_buffer(vcf_interface_objects.main_can_rx_buffer, vcf_can_interfaces, sys_time::hal_millis(), vcf_interface_objects.can_recv_switch, CANInterfaceType_e::TELEM); return HT_TASK::TaskResponse::YIELD; } HT_TASK::TaskResponse send_dash_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) -{ - CANInterfaces can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); +{ + CANInterfaces can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); DashInputState_s dash_outputs = can_interfaces.dash_interface.get_dashboard_outputs(); DASH_INPUT_t msg_out; @@ -191,13 +190,13 @@ HT_TASK::TaskResponse send_dash_data(const unsigned long& sysMicros, const HT_TA msg_out.dash_dial_mode = static_cast(DashboardInterfaceInstance::instance().get_dashboard_outputs().dial_state); // Serial.printf("%d %d %d %d %d %d %d %d\n", msg_out.preset_button, msg_out.motor_controller_cycle_button, msg_out.mode_button, msg_out.start_button, msg_out.data_button_is_pressed, msg_out.left_shifter_button, msg_out.right_shifter_button, msg_out.led_dimmer_button); - + CAN_util::enqueue_msg(&msg_out, &Pack_DASH_INPUT_hytech, VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance().main_can_tx_buffer); - + return HT_TASK::TaskResponse::YIELD; } -HT_TASK::TaskResponse enqueue_front_suspension_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) +HT_TASK::TaskResponse enqueue_front_suspension_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { CANInterfaces can_interface = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); FRONT_SUSPENSION_t msg_out; @@ -210,9 +209,30 @@ HT_TASK::TaskResponse enqueue_front_suspension_data(const unsigned long& sysMicr CAN_util::enqueue_msg(&msg_out, &Pack_FRONT_SUSPENSION_hytech, VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance().main_can_tx_buffer); return HT_TASK::TaskResponse::YIELD; } +// HT_TASK::TaskResponse evaluate_steering_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) +// { +// //sample from digital steering +// //steering system interface get_data ->passes in values +// //analog in adc_interface +// const uint32_t analog_raw = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw); +// const uint32_t digital_raw = static_cast(OrbisBRInstance::instance().get_steering_angle_raw()); + + + + +// SteeringSystemInstance::instance().set_steering_sensor_data(SteeringSensorData_s{ +// .analog_steering_degrees = analog_raw, +// .digital_steering_analog = digital_raw +// }); + + +// SteeringSystemInstance::instance().evaluate_steering(SteeringSystemInstance::instance().get_steering_sensor_data(), sys_time::hal_millis()); + +// return HT_TASK::TaskResponse::YIELD; +// } // TODO: update this and add any other sending data stuff -HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) +HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { STEERING_DATA_t msg_out; SteeringSystemData_s steering_system_data = SteeringSystemInstance::instance().get_steering_system_data(); @@ -220,6 +240,7 @@ HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, cons msg_out.steering_analog_raw = steering_system_data.analog_steering_angle; msg_out.steering_digital_raw = steering_system_data.digital_steering_angle; //NOLINT VCFData_sInstance::instance().interface_data.steering_data.digital_steering_analog; + CAN_util::enqueue_msg(&msg_out, &Pack_STEERING_DATA_hytech, VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance().main_can_tx_buffer); return HT_TASK::TaskResponse::YIELD; } @@ -268,7 +289,8 @@ HT_TASK::TaskResponse enqueue_pedals_data(const unsigned long &sys_micros, const pedals_data.mechanical_brake_active = PedalsSystemInstance::instance().get_pedals_system_data().mech_brake_is_active; pedals_data.implaus_exceeded_max_duration = PedalsSystemInstance::instance().get_pedals_system_data().implausibility_has_exceeded_max_duration; - + + pedals_data.accel_pedal_ro = HYTECH_accel_pedal_ro_toS(PedalsSystemInstance::instance().get_pedals_system_data().accel_percent); pedals_data.brake_pedal_ro = HYTECH_brake_pedal_ro_toS(PedalsSystemInstance::instance().get_pedals_system_data().brake_percent); // Serial.println(pedals_data.brake_pedal_ro); @@ -307,7 +329,7 @@ HT_TASK::TaskResponse create_ioexpander(const unsigned long& sys_micros, const H IOExpanderInstance::instance().portMode(MCP23017Port::A, 0b00000000); IOExpanderInstance::instance().portMode(MCP23017Port::B, 0b01111111); - // IOExpanderInstance::instance().writeRegister(MCP23017Register::GPIO_A, 0x00); //Reset port A + // IOExpanderInstance::instance().writeRegister(MCP23017Register::GPIO_A, 0x00); //Reset port A // IOExpanderInstance::instance().writeRegister(MCP23017Register::GPIO_B, 0x00); //Reset port B IOExpanderInstance::instance().writeRegister(MCP23017Register::GPPU_B, 0xFF); //Internal pull-ups @@ -396,34 +418,44 @@ HT_TASK::TaskResponse run_update_neopixels_task(const unsigned long& sys_micros, return HT_TASK::TaskResponse::YIELD; } -namespace async_tasks +namespace async_tasks { // these are async tasks. we want these to run as fast as possible p much void handle_async_CAN_receive() //NOLINT caps for CAN { VCFCANInterfaceObjects& vcf_interface_objects = VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance(); - CANInterfaces& vcf_can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); + CANInterfaces& vcf_can_interfaces = VCFCANInterfaceImpl::CANInterfacesInstance::instance(); process_ring_buffer(vcf_interface_objects.main_can_rx_buffer, vcf_can_interfaces, sys_time::hal_millis(), vcf_interface_objects.can_recv_switch, CANInterfaceType_e::TELEM); } void handle_async_recvs() { // ethernet, etc... - + handle_async_CAN_receive(); } HT_TASK::TaskResponse handle_async_main(const unsigned long& sys_micros, const HT_TASK::TaskInfo& task_info) { handle_async_recvs(); + OrbisBRInstance::instance().sample(); + const uint32_t digital_raw = static_cast(OrbisBRInstance::instance().convert().raw); + const uint32_t analog_raw = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw); + SteeringSystemInstance::instance().evaluate_steering( + analog_raw, + digital_raw, + sys_time::hal_millis() + ); + PedalsSystemInstance::instance().evaluate_pedals( PedalsSystemInstance::instance().get_pedals_sensor_data(), sys_time::hal_millis() - )); + ); // Serial.println(VCFData_sInstance::instance().system_data.pedals_system_data.accel_percent); return HT_TASK::TaskResponse::YIELD; } }; + HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { // Serial.println("accel1 raw accel2 raw"); @@ -444,6 +476,7 @@ HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK: // Serial.print("implaus "); // Serial.println(PedalsSystemInstance::instance().get_pedals_system_data().implausibility_has_exceeded_max_duration); + // Serial.println("accel 1 min/max"); // Serial.print(PedalsSystemInstance::instance().get_accel_params().min_pedal_1); // Serial.print(" "); @@ -465,7 +498,7 @@ HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK: // Serial.print(PedalsSystemInstance::instance().get_brake_params().max_pedal_2); // Serial.println(); // Serial.println(); - + // Serial.print("Load Cell FR: "); // Serial.println(ADCInterfaceInstance::instance().get_filtered_FR_load_cell()); // Serial.print("Load Cell FL: "); @@ -474,7 +507,8 @@ HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK: // Serial.println(ADCInterfaceInstance::instance().get_filtered_FR_sus_pot()); // Serial.print("Suspot FL: "); // Serial.println(ADCInterfaceInstance::instance().get_filtered_FL_sus_pot()); - + + // Serial.print("preset button: "); // Serial.println(DashboardInterfaceInstance::instance().get_dashboard_outputs().preset_btn_is_pressed); @@ -485,19 +519,25 @@ HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK: // Serial.print("data button: "); // Serial.println(DashboardInterfaceInstance::instance().get_dashboard_outputs().data_btn_is_pressed); + // Serial.println("jkkjhhkjkjh"); + return HT_TASK::TaskResponse::YIELD; } + FlexCAN_T4 VCFCANInterfaceImpl::main_can; + void setup_all_interfaces() { SPI.begin(); Serial.begin(VCFTaskConstants::SERIAL_BAUDRATE); // NOLINT + // Initialize all singletons + // Create ADC interface singleton ADCInterfaceInstance::create( ADCPinout_s { @@ -513,6 +553,7 @@ void setup_all_interfaces() { VCFInterfaceConstants::BRAKE_1_CHANNEL, VCFInterfaceConstants::BRAKE_2_CHANNEL, + VCFInterfaceConstants::SHDN_H_CHANNEL, VCFInterfaceConstants::SHDN_D_CHANNEL, VCFInterfaceConstants::FL_LOADCELL_CHANNEL, @@ -522,7 +563,7 @@ void setup_all_interfaces() { VCFInterfaceConstants::BRAKE_PRESSURE_FRONT_CHANNEL, VCFInterfaceConstants::BRAKE_PRESSURE_REAR_CHANNEL }, - ADCScales_s { + ADCScales_s { VCFInterfaceConstants::PEDAL_REF_2V5_SCALE, VCFInterfaceConstants::STEERING_1_SCALE, VCFInterfaceConstants::STEERING_2_SCALE, @@ -531,6 +572,7 @@ void setup_all_interfaces() { VCFInterfaceConstants::BRAKE_1_SCALE, VCFInterfaceConstants::BRAKE_2_SCALE, + VCFInterfaceConstants::SHDN_H_SCALE, VCFInterfaceConstants::SHDN_D_SCALE, VCFInterfaceConstants::FL_LOADCELL_SCALE, @@ -539,7 +581,7 @@ void setup_all_interfaces() { VCFInterfaceConstants::FL_SUS_POT_SCALE, VCFInterfaceConstants::BRAKE_PRESSURE_FRONT_SCALE, VCFInterfaceConstants::BRAKE_PRESSURE_REAR_SCALE - }, + }, ADCOffsets_s { VCFInterfaceConstants::PEDAL_REF_2V5_OFFSET, VCFInterfaceConstants::STEERING_1_OFFSET, @@ -549,6 +591,7 @@ void setup_all_interfaces() { VCFInterfaceConstants::BRAKE_1_OFFSET, VCFInterfaceConstants::BRAKE_2_OFFSET, + VCFInterfaceConstants::SHDN_H_OFFSET, VCFInterfaceConstants::SHDN_D_OFFSET, VCFInterfaceConstants::FL_LOADCELL_OFFSET, @@ -559,8 +602,10 @@ void setup_all_interfaces() { VCFInterfaceConstants::BRAKE_PRESSURE_REAR_OFFSET }); + EthernetIPDefsInstance::create(); + // Create pedals singleton PedalsParams accel_params = { .min_pedal_1 = EEPROMUtilities::read_eeprom_32bit(VCFInterfaceConstants::ACCEL_1_MIN_ADDR), @@ -576,7 +621,7 @@ void setup_all_interfaces() { .implausibility_margin = IMPLAUSIBILITY_PERCENT, .mechanical_activation_percentage = VCFInterfaceConstants::ACCEL_MECHANICAL_ACTIVATION_PERCENTAGE }; - + PedalsParams brake_params = { .min_pedal_1 = EEPROMUtilities::read_eeprom_32bit(VCFInterfaceConstants::BRAKE_1_MIN_ADDR), .min_pedal_2 = EEPROMUtilities::read_eeprom_32bit(VCFInterfaceConstants::BRAKE_2_MIN_ADDR), @@ -592,9 +637,9 @@ void setup_all_interfaces() { .mechanical_activation_percentage = VCFInterfaceConstants::BRAKE_MECHANICAL_ACTIVATION_PERCENTAGE }; + PedalsSystemInstance::create(accel_params, brake_params); //pass in the two different params - -<<<<<<< HEAD + SteeringParams_s steering_params = { .min_steering_signal_analog = VCFSystemConstants::MIN_STEERING_SIGNAL_ANALOG, .max_steering_signal_analog = VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG, @@ -617,11 +662,14 @@ void setup_all_interfaces() { steering_params.analog_tol_deg = static_cast(steering_params.span_signal_analog) * steering_params.analog_tol * steering_params.deg_per_count_analog; steering_params.error_between_sensors_tolerance = steering_params.analog_tol_deg + steering_params.digital_tol_deg; + SteeringSystemInstance::create(steering_params); -======= + + + // Create Digital Steering Sensor singleton OrbisBRInstance::create(&Serial3); // pass in two different params ->>>>>>> origin/main + // Create dashboard singleton DashboardGPIOs_s dashboard_gpios = { @@ -633,20 +681,24 @@ void setup_all_interfaces() { .BUTTON_2 = VCFInterfaceConstants::BUTTON_2 }; + DashboardInterfaceInstance::create(dashboard_gpios); //NOLINT ACUInterfaceInstance::create(); VCRInterfaceInstance::create(); // Create can singletons - VCFCANInterfaceImpl::CANInterfacesInstance::create(DashboardInterfaceInstance::instance(), ACUInterfaceInstance::instance(), VCRInterfaceInstance::instance()); + VCFCANInterfaceImpl::CANInterfacesInstance::create(DashboardInterfaceInstance::instance(), ACUInterfaceInstance::instance(), VCRInterfaceInstance::instance()); auto main_can_recv = etl::delegate::create(); VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::create(main_can_recv, &VCFCANInterfaceImpl::main_can); + const uint32_t CAN_baudrate = 1000000; handle_CAN_setup(VCFCANInterfaceImpl::main_can, CAN_baudrate, &VCFCANInterfaceImpl::on_main_can_recv); + EthernetIPDefsInstance::create(); uint8_t mac[6]; // NOLINT (mac addresses are always 6 bytes) qindesign::network::Ethernet.macAddress(&mac[0]); qindesign::network::Ethernet.begin(mac, EthernetIPDefsInstance::instance().vcf_ip, EthernetIPDefsInstance::instance().default_dns, EthernetIPDefsInstance::instance().default_gateway, EthernetIPDefsInstance::instance().car_subnet); + } From c4d96b67a7caeb7a91bcc7035d199bd291de8c65 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Thu, 12 Mar 2026 00:37:44 -0400 Subject: [PATCH 36/39] changed digital steering raw values to full orbis data struct --- include/VCF_Tasks.h | 1 + lib/systems/include/SteeringSystem.h | 4 ++-- lib/systems/src/SteeringSystem.cpp | 9 +++++++-- src/VCF_Tasks.cpp | 5 +++-- 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/include/VCF_Tasks.h b/include/VCF_Tasks.h index b7f9509..c68283d 100644 --- a/include/VCF_Tasks.h +++ b/include/VCF_Tasks.h @@ -43,6 +43,7 @@ #include "WatchdogSystem.h" #include "ADCInterface.h" #include "CANInterface.h" +#include "SteeringEncoderInterface.h" /** * The read_adc0 task will command the ADCInterface to sample, convert, and store diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index e67c6b2..d54e3d8 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -9,7 +9,7 @@ #include "SharedFirmwareTypes.h" - +#include "SteeringEncoderInterface.h" struct SteeringParams_s { // raw ADC input signals @@ -89,7 +89,7 @@ class SteeringSystem { // Functions void recalibrate_steering_digital(const uint32_t analog_raw, const uint32_t digital_raw, bool calibration_is_on); - void evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis); + void evaluate_steering(const uint32_t analog_raw, const SteeringEncoderConversion_s digital_data, const uint32_t current_millis); // Getters diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index f368056..fce4965 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,8 +1,9 @@ #include #include #include "SteeringSystem.h" +#include "SteeringEncoderInterface.h" -void SteeringSystem::recalibrate_steering_digital(const uint_32 analog_raw, const uint32_t digital_raw, bool calibration_is_on) { +void SteeringSystem::recalibrate_steering_digital(const uint32_t analog_raw, const uint32_t digital_raw, bool calibration_is_on) { //get current raw angles const uint32_t curr_digital_raw = static_cast(digital_raw); //NOLINT will eventually be uint32 @@ -50,7 +51,7 @@ void SteeringSystem::recalibrate_steering_digital(const uint_32 analog_raw, cons } } -void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t digital_raw, const uint32_t current_millis) { +void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const SteeringEncoderConversion_s digital_data, const uint32_t current_millis) { // Reset flags _steeringSystemData.digital_oor_implausibility = false; _steeringSystemData.analog_oor_implausibility = false; @@ -59,6 +60,10 @@ void SteeringSystem::evaluate_steering(const uint32_t analog_raw, const uint32_t _steeringSystemData.dtheta_exceeded_digital = false; _steeringSystemData.both_sensors_fail = false; + const uint32_t digital_raw = digital_data.raw; + SteeringEncoderStatus_e digital_status = digital_data.status; + EncoderErrorFlags_s digital_errors = digital_data.errors;; + _steeringSystemData.analog_raw = analog_raw; _steeringSystemData.digital_raw = digital_raw; //Conversion from raw ADC to degrees diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 9e61fc2..76c28a0 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -21,6 +21,7 @@ #include #include "FlexCAN_T4.h" #include "Orbis_BR.h" +#include "SteeringEncoderInterface.h" #include "WatchdogSystem.h" #include "Arduino.h" @@ -438,11 +439,11 @@ namespace async_tasks { handle_async_recvs(); OrbisBRInstance::instance().sample(); - const uint32_t digital_raw = static_cast(OrbisBRInstance::instance().convert().raw); + const SteeringEncoderConversion_s digital_data = static_cast(OrbisBRInstance::instance().convert()); const uint32_t analog_raw = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw); SteeringSystemInstance::instance().evaluate_steering( analog_raw, - digital_raw, + digital_data, sys_time::hal_millis() ); From 0b6f97c03a365d692ca776931ff135edb943d8d9 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 15 Mar 2026 16:34:49 -0400 Subject: [PATCH 37/39] fixed conversion and access modifier --- lib/systems/include/SteeringSystem.h | 2 +- src/VCF_Tasks.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index d54e3d8..2d0c79e 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -113,8 +113,8 @@ class SteeringSystem { _steeringSystemData = steeringSystemData; } -private: void update_observed_steering_limits(const uint32_t analog_raw, const uint32_t digital_raw); +private: float _convert_digital_sensor(const uint32_t digital_raw); diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 76c28a0..20d5858 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -439,8 +439,8 @@ namespace async_tasks { handle_async_recvs(); OrbisBRInstance::instance().sample(); - const SteeringEncoderConversion_s digital_data = static_cast(OrbisBRInstance::instance().convert()); const uint32_t analog_raw = static_cast(ADCInterfaceInstance::instance().steering_degrees_cw().raw); + const SteeringEncoderConversion_s digital_data = OrbisBRInstance::instance().convert(); SteeringSystemInstance::instance().evaluate_steering( analog_raw, digital_data, From 88a18faaebf216bbe3d288bd5d6bd007f26fa9cd Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 15 Mar 2026 16:47:15 -0400 Subject: [PATCH 38/39] added debug prints for steering system --- src/VCF_Tasks.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 20d5858..0069151 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -523,6 +523,17 @@ HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK: // Serial.println("jkkjhhkjkjh"); + Serial.println("Steering Sensor Data: "); + Serial.print("analog: "); + Serial.print(SteeringSystemInstance::instance().get_steering_system_data().analog_raw); + Serial.print("|"); + Serial.println(SteeringSystemInstance::instance().get_steering_system_data().analog_steering_angle); + Serial.print("digital: "); + Serial.print(SteeringSystemInstance::instance().get_steering_system_data().digital_raw); + Serial.print("|"); + Serial.println(SteeringSystemInstance::instance().get_steering_system_data().digital_steering_angle); + + return HT_TASK::TaskResponse::YIELD; } From aa2730ed92c6781439457a7936936c6fee2ed8e9 Mon Sep 17 00:00:00 2001 From: William Schotte Date: Sun, 15 Mar 2026 17:24:40 -0400 Subject: [PATCH 39/39] debugged sone stuff i think --- src/VCF_Tasks.cpp | 45 +++++++++++++++++++++++---------------------- src/main.cpp | 2 +- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 0069151..6d46efb 100644 --- a/src/VCF_Tasks.cpp +++ b/src/VCF_Tasks.cpp @@ -459,23 +459,23 @@ namespace async_tasks HT_TASK::TaskResponse debug_print(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - // Serial.println("accel1 raw accel2 raw"); - // Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().accel_1); - // Serial.print(" "); - // Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().accel_2); - // Serial.println(); - // Serial.println("brake1 raw brake2 raw"); - // Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().brake_1); - // Serial.print(" "); - // Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().brake_2); - // Serial.println(); - // Serial.println("accel brake percents"); - // Serial.print(PedalsSystemInstance::instance().get_pedals_system_data().accel_percent); - // Serial.print(" "); - // Serial.print(PedalsSystemInstance::instance().get_pedals_system_data().brake_percent); - // Serial.println(); - // Serial.print("implaus "); - // Serial.println(PedalsSystemInstance::instance().get_pedals_system_data().implausibility_has_exceeded_max_duration); + Serial.println("accel1 raw accel2 raw"); + Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().accel_1); + Serial.print(" "); + Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().accel_2); + Serial.println(); + Serial.println("brake1 raw brake2 raw"); + Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().brake_1); + Serial.print(" "); + Serial.print(PedalsSystemInstance::instance().get_pedals_sensor_data().brake_2); + Serial.println(); + Serial.println("accel brake percents"); + Serial.print(PedalsSystemInstance::instance().get_pedals_system_data().accel_percent); + Serial.print(" "); + Serial.print(PedalsSystemInstance::instance().get_pedals_system_data().brake_percent); + Serial.println(); + Serial.print("implaus "); + Serial.println(PedalsSystemInstance::instance().get_pedals_system_data().implausibility_has_exceeded_max_duration); // Serial.println("accel 1 min/max"); @@ -651,7 +651,7 @@ void setup_all_interfaces() { PedalsSystemInstance::create(accel_params, brake_params); //pass in the two different params - + SteeringParams_s steering_params = { .min_steering_signal_analog = VCFSystemConstants::MIN_STEERING_SIGNAL_ANALOG, .max_steering_signal_analog = VCFSystemConstants::MAX_STEERING_SIGNAL_ANALOG, @@ -677,11 +677,12 @@ void setup_all_interfaces() { SteeringSystemInstance::create(steering_params); - - + Serial.println("fff"); + // Create Digital Steering Sensor singleton - OrbisBRInstance::create(&Serial3); // pass in two different params - + OrbisBRInstance::create(&Serial2); // pass in two different params + + Serial.println("fff2"); // Create dashboard singleton DashboardGPIOs_s dashboard_gpios = { diff --git a/src/main.cpp b/src/main.cpp index 15951ba..007fd1c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -84,7 +84,7 @@ void setup() { HT_SCHED::Scheduler::getInstance().schedule(steering_calibration_task); HT_SCHED::Scheduler::getInstance().schedule(ethernet_send_task); - // HT_SCHED::Scheduler::getInstance().schedule(debug_state_print_task); + HT_SCHED::Scheduler::getInstance().schedule(debug_state_print_task); } void loop() {