diff --git a/include/VCF_Constants.h b/include/VCF_Constants.h index 86b903d..db025dd 100644 --- a/include/VCF_Constants.h +++ b/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,6 +204,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; @@ -184,4 +214,4 @@ namespace VCFTaskConstants { constexpr unsigned long WATCHDOG_KICK_PERIOD = 10000; // 10 000 us = 100 Hz } -#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 54837ad..c68283d 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" @@ -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; diff --git a/lib/interfaces/src/ADCInterface.cpp b/lib/interfaces/src/ADCInterface.cpp index 15d39f3..1ed3c64 100644 --- a/lib/interfaces/src/ADCInterface.cpp +++ b/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]; -} \ No newline at end of file +} diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h new file mode 100644 index 0000000..2d0c79e --- /dev/null +++ b/lib/systems/include/SteeringSystem.h @@ -0,0 +1,148 @@ +#ifndef STEERING_SYSTEM_H +#define STEERING_SYSTEM_H + + +#include +#include +#include +#include + + +#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; + } + + 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); + + 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; + + +#endif diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp new file mode 100644 index 0000000..fce4965 --- /dev/null +++ b/lib/systems/src/SteeringSystem.cpp @@ -0,0 +1,150 @@ +#include +#include +#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(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(_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; + _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(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); + 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(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 uint32_t digital_raw) { + // Same logic for digital + 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 uint32_t analog_raw) { + // Get the raw value + 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(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(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); +} + +bool SteeringSystem::_evaluate_steering_dtheta_exceeded(float dtheta){ + return (fabs(dtheta) > _steeringParams.max_dtheta_threshold); +} + diff --git a/platformio.ini b/platformio.ini index 20c3f46..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 @@ -214,7 +214,7 @@ check_src_filters = + + - -platform = teensy +platform = teensyit board = teensy41 framework = arduino monitor_speed = 115200 @@ -229,4 +229,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/src/VCF_Tasks.cpp b/src/VCF_Tasks.cpp index 360bb18..6d46efb 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" @@ -20,6 +21,7 @@ #include #include "FlexCAN_T4.h" #include "Orbis_BR.h" +#include "SteeringEncoderInterface.h" #include "WatchdogSystem.h" #include "Arduino.h" @@ -42,6 +44,13 @@ HT_TASK::TaskResponse run_read_adc1_task(const unsigned long& sysMicros, const H { // Samples all eight channels. ADCInterfaceInstance::instance().adc1_tick(); + PedalsSystemInstance::instance().set_pedals_sensor_data(PedalSensorData_s{ + .accel_1 = static_cast(ADCInterfaceInstance::instance().acceleration_1().conversion), + .accel_2 = static_cast(ADCInterfaceInstance::instance().acceleration_2().conversion), + .brake_1 = static_cast(ADCInterfaceInstance::instance().brake_1().conversion), + .brake_2 = static_cast(ADCInterfaceInstance::instance().brake_2().conversion), + .pedal_pressure = 0 // Currently an unused field + }); ADCInterfaceInstance::instance().update_filtered_values(VCFTaskConstants::LOADCELL_IIR_FILTER_ALPHA); return HT_TASK::TaskResponse::YIELD; } @@ -55,7 +64,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; } @@ -83,6 +92,22 @@ 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 + 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(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() // { // // Setting digital/analog buttons D10-D6, A8 as inputs @@ -92,7 +117,7 @@ HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMic // 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() @@ -104,7 +129,7 @@ HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMic // 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; @@ -128,7 +153,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; } @@ -142,14 +167,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; @@ -166,13 +191,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; @@ -185,13 +210,37 @@ 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()); + + -HT_TASK::TaskResponse enqueue_steering_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) + +// 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) { STEERING_DATA_t msg_out; + 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; - msg_out.steering_analog_raw = ADCInterfaceInstance::instance().steering_degrees_cw().conversion; - msg_out.steering_digital_raw = 0; //NOLINT TODO: once digital steering sensor works, this needs to be changed accordingly CAN_util::enqueue_msg(&msg_out, &Pack_STEERING_DATA_hytech, VCFCANInterfaceImpl::VCFCANInterfaceObjectsInstance::instance().main_can_tx_buffer); return HT_TASK::TaskResponse::YIELD; @@ -241,7 +290,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); @@ -280,7 +330,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 @@ -369,25 +419,34 @@ 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 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, + sys_time::hal_millis() + ); + PedalsSystemInstance::instance().evaluate_pedals( PedalsSystemInstance::instance().get_pedals_sensor_data(), sys_time::hal_millis() @@ -397,25 +456,27 @@ 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"); // Serial.print(PedalsSystemInstance::instance().get_accel_params().min_pedal_1); @@ -438,7 +499,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: "); @@ -447,7 +508,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); @@ -458,19 +520,36 @@ 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"); + 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; } + 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 { @@ -486,6 +565,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, @@ -495,7 +575,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, @@ -504,6 +584,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, @@ -512,7 +593,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, @@ -522,6 +603,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, @@ -532,8 +614,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), @@ -549,7 +633,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), @@ -565,10 +649,40 @@ void setup_all_interfaces() { .mechanical_activation_percentage = VCFInterfaceConstants::BRAKE_MECHANICAL_ACTIVATION_PERCENTAGE }; + 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(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, + .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); + + 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 = { @@ -580,20 +694,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); -} \ No newline at end of file + +} diff --git a/src/main.cpp b/src/main.cpp index a373c9e..007fd1c 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,11 +81,12 @@ 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); + HT_SCHED::Scheduler::getInstance().schedule(debug_state_print_task); } void loop() { HT_SCHED::Scheduler::getInstance().run(); -} \ No newline at end of file +} 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 new file mode 100644 index 0000000..2ca7bb9 --- /dev/null +++ b/test/test_systems/steering_system_test.h @@ -0,0 +1,405 @@ +#define STEERING_SYSTEM_TEST +#include +#include +#include "SteeringSystem.h" +#include "SharedFirmwareTypes.h" +#include + +#include + +SteeringParams_s gen_default_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 + + params.min_steering_signal_digital = 25; + 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; + + 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; + + + 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; +} + +void debug_print_steering(const SteeringSystemData_s& data){ + std::cout<<"analog_steering_angle: "<(raw) - static_cast(midpoint); + return static_cast(offset) * deg_per_count +} + +*/ + +//FIXED +TEST(SteeringSystemTesting, test_adc_to_degree_conversion) +{ + 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; + + //midpoints + 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_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; + + steering.evaluate_steering(min_val, 1010); + data = steering.get_steering_system_data(); + + 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); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_min, 0.001f); + + //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; + + steering.evaluate_steering(max_val, 1020); + data = steering.get_steering_system_data(); + 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); + EXPECT_NEAR(data.digital_steering_angle, expected_digital_max, 0.001f); + +} + +//FIXED +TEST(SteeringSystemTesting, test_out_of_bounds_raw_signals){ + auto params = gen_default_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; + + + SteeringSystem steering(params); + + //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}; + 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(params.min_steering_signal_analog)-50, static_cast(params.min_steering_signal_digital)-10}; + steering.evaluate_steering(low_val, 1020); + 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); + +} + +TEST(SteeringSystemTesting, test_detect_jumps_dtheta){ + auto params = gen_default_params(); + SteeringSystem steering(params); + + 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) + 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}; + steering.evaluate_steering(massive_jump, 1005); + data = steering.get_steering_system_data(); + 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}; + 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 + 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); +} + + +TEST(SteeringSystemTesting, test_sensor_disagreement) +{ + 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 + + steering.evaluate_steering(disagree, 1100); + auto data = steering.get_steering_system_data(); + EXPECT_TRUE(data.sensor_disagreement_implausibility); +} + +TEST(SteeringSystemTesting,test_sensor_output_logic){ + auto params = gen_default_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 + 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); //fail + EXPECT_FALSE(data.sensor_disagreement_implausibility); +} + // Prevent dtheta exceeded for the next test + +{ + //When both valid but disagreeing, we default to digital + 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 + 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 + 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 + 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); +} + + +} + +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); +} +