Feat/current steering system first version is done

include/VCF_Constants.h

@@ -130,6 +130,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 = 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
+
+    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;
+
+    // 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
@@ -177,11 +204,14 @@ 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;
 
     constexpr unsigned long WATCHDOG_PRIORITY = 1;
     constexpr unsigned long WATCHDOG_KICK_PERIOD = 10000;          // 10 000 us = 100 Hz
 }
 
-#endif /* VCF_CONSTANTS */
+#endif /* VCF_CONSTANTS */

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"
@@ -42,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 
@@ -60,6 +62,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);
 /**
  * The buzzer_control task will control the buzzer control pin. This function
  * relies on the buzzer_control pin definition in VCF_Constants.h;

lib/interfaces/src/ADCInterface.cpp

@@ -172,4 +172,4 @@ AnalogConversion_s ADCInterface::brake_pressure_front() {
 
 AnalogConversion_s ADCInterface::brake_pressure_rear() {
     return _adc1.data.conversions[_adc_parameters.channels.brake_pressure_rear_channel];
-}
+}

lib/systems/include/SteeringSystem.h

@@ -0,0 +1,148 @@
+#ifndef STEERING_SYSTEM_H
+#define STEERING_SYSTEM_H
+
+
+#include <etl/singleton.h>
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+
+
+#include "SharedFirmwareTypes.h"
+#include "SteeringEncoderInterface.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)
+    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
+
+
+    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;
+
+
+    // 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;
+    float digital_tol_deg; // +- 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
+};
+
+
+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;
+    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 uint32_t analog_raw, const uint32_t digital_raw, bool calibration_is_on);
+
+    void evaluate_steering(const uint32_t analog_raw, const SteeringEncoderConversion_s digital_data, 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 uint32_t analog_raw, const uint32_t digital_raw);
+
+    float _convert_digital_sensor(const uint32_t digital_raw);
+
+    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(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(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
+    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<SteeringSystem>;
+
+
+#endif

lib/systems/src/SteeringSystem.cpp

@@ -0,0 +1,150 @@
+#include <cmath>
+#include <cstdint>
+#include "SteeringSystem.h"
+#include "SteeringEncoderInterface.h"
+
+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<uint32_t>(digital_raw); //NOLINT will eventually be uint32
+
+    //button just pressed ->recalibration window
+    if (calibration_is_on && !_calibrating){
+        _calibrating = true;
+        _steeringParams.min_observed_digital = UINT32_MAX; //establishes a big number that will be greater than the readings
+        _steeringParams.max_observed_digital = 0;
+    }
+
+    if (calibration_is_on && _calibrating) {
+        update_observed_steering_limits(analog_raw, digital_raw);
+    }
+
+    //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;
+        // 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);
+        }
+        _steeringParams.span_signal_digital = _steeringParams.max_steering_signal_digital-_steeringParams.min_steering_signal_digital;
+        _steeringParams.analog_tol_deg = static_cast<float>(_steeringParams.span_signal_analog) * _steeringParams.analog_tol * _steeringParams.deg_per_count_analog;
+        _steeringParams.digital_midpoint = static_cast<int32_t>((_steeringParams.max_steering_signal_digital + _steeringParams.min_steering_signal_digital) / 2); //NOLINT
+        _steeringParams.analog_midpoint = static_cast<int32_t>((_steeringParams.max_steering_signal_analog + _steeringParams.min_steering_signal_analog) / 2); //NOLINT
+        const int32_t analog_margin_counts = static_cast<int32_t>(_steeringParams.analog_tol * static_cast<float>(_steeringParams.span_signal_analog)); //NOLINT
+        const int32_t digital_margin_counts = static_cast<int32_t>(_steeringParams.digital_tol_deg /_steeringParams.deg_per_count_digital); //NOLINT
+        _steeringParams.analog_min_with_margins = static_cast<int32_t>(_steeringParams.min_steering_signal_analog) - analog_margin_counts;
+        _steeringParams.analog_max_with_margins = static_cast<int32_t>(_steeringParams.max_steering_signal_analog) + analog_margin_counts;
+        _steeringParams.digital_min_with_margins = static_cast<int32_t>(_steeringParams.min_steering_signal_digital) - digital_margin_counts;
+        _steeringParams.digital_max_with_margins = static_cast<int32_t>(_steeringParams.max_steering_signal_digital) + digital_margin_counts;
+        _steeringParams.error_between_sensors_tolerance = _steeringParams.analog_tol_deg + _steeringParams.digital_tol_deg;
+    } 
+}
+
+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;
+    _steeringSystemData.sensor_disagreement_implausibility = false;
+    _steeringSystemData.dtheta_exceeded_analog = false;
+    _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
+    _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
+
+    if (!_first_run && dt > 0) { //check that we not on the first run which would mean no previous data
+        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
+        _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)
+        _steeringSystemData.analog_oor_implausibility = _evaluate_steering_oor_analog(static_cast<uint32_t>(analog_raw));
+        _steeringSystemData.digital_oor_implausibility = _evaluate_steering_oor_digital(static_cast<uint32_t>(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);
+        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,
+        //or, if we should have an algorithm to have a weighted calculation based on both values
+        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) {
+                _steeringSystemData.output_steering_angle = _steeringSystemData.digital_steering_angle;
+            } else {
+                _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) {
+            _steeringSystemData.output_steering_angle = _steeringSystemData.analog_steering_angle;
+        } else if (digital_valid) {
+            _steeringSystemData.output_steering_angle = _steeringSystemData.digital_steering_angle;
+        } else { // if both sensors fail
+            _steeringSystemData.output_steering_angle = _prev_digital_angle;
+            _steeringSystemData.both_sensors_fail = true;
+        }
+    }
+    //Update states
+    _prev_analog_angle = _steeringSystemData.analog_steering_angle;
+    _prev_digital_angle = _steeringSystemData.digital_steering_angle;
+    _prev_timestamp = current_millis;
+    _first_run = false;
+}
+
+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<uint32_t>(digital_raw)); //NOLINT should both be uint32_t
+    _steeringParams.max_observed_digital = std::max(_steeringParams.max_observed_digital, static_cast<uint32_t>(digital_raw)); //NOLINT ^
+}
+
+float SteeringSystem::_convert_digital_sensor(const uint32_t digital_raw) {
+    // Same logic for digital
+    const int32_t offset = static_cast<int32_t>(digital_raw) - _steeringParams.digital_midpoint; //NOLINT
+    return static_cast<float>(offset) * _steeringParams.deg_per_count_digital;
+}
+
+float SteeringSystem::_convert_analog_sensor(const uint32_t analog_raw) {
+    // Get the raw value
+    const int32_t offset = static_cast<int32_t>(analog_raw) - _steeringParams.analog_midpoint; //NOLINT
+    return static_cast<float>(offset) * _steeringParams.deg_per_count_analog;
+}
+
+bool SteeringSystem::_evaluate_steering_oor_analog(const uint32_t steering_analog_raw) { // RAW
+    return (static_cast<int32_t>(steering_analog_raw) < _steeringParams.analog_min_with_margins || static_cast<int32_t>(steering_analog_raw) > _steeringParams.analog_max_with_margins);
+}
+
+bool SteeringSystem::_evaluate_steering_oor_digital(const uint32_t steering_digital_raw) {// RAW
+    return (static_cast<int32_t>(steering_digital_raw) < _steeringParams.digital_min_with_margins || static_cast<int32_t>(steering_digital_raw) > _steeringParams.digital_max_with_margins);
+}
+
+bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){
+    return (fabs(dtheta) > _steeringParams.max_dtheta_threshold);
+}
+