-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
5 changed files
with
115 additions
and
179 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,97 +1,62 @@ | ||
| #ifndef __PEDALSSYSTEM_H__ | ||
| #define __PEDALSSYSTEM_H__ | ||
| #ifndef PEDALSSYSTEM | ||
| #define PEDALSSYSTEM | ||
| #include <math.h> | ||
| #include <tuple> | ||
|
|
||
| #include "AnalogSensorsInterface.h" | ||
| #include <SysClock.h> | ||
|
|
||
| // Definitions | ||
| const int PEDALS_IMPLAUSIBLE_DURATION = 100; // Implausibility must be caught within 100ms | ||
| const float PEDALS_IMPLAUSIBLE_PERCENT = 0.10; // 10% allowed deviation FSAE T.4.2.4 | ||
| const float PEDALS_MARGINAL_PERCENT = 0.07; // Report pedals are marginal. Allows us to detect pedals may need recalibration | ||
| const float PEDALS_RAW_TOO_LOW = 0.5 / 5 * 4096; // FSAE T.4.2.10 Pedals are implausible below 0.5V raw reading | ||
| const float PEDALS_RAW_TOO_HIGH = 4.5 / 5 * 4096; // FSAE T.4.2.10 Pedals are implausible above 4.5V raw reading | ||
|
|
||
| // Enums | ||
| enum PedalsStatus_e | ||
| { | ||
| PEDALS_NOMINAL = 0, | ||
| PEDALS_MARGINAL = 1, | ||
| PEDALS_IMPLAUSIBLE = 2, | ||
| }; | ||
|
|
||
| enum PedalsCommanded_e | ||
| struct PedalsDriverInterface | ||
| { | ||
| PEDALS_NONE_PRESSED = 0, | ||
| PEDALS_ACCEL_PRESSED = 1, | ||
| PEDALS_BRAKE_PRESSED = 2, | ||
| PEDALS_BOTH_PRESSED = 3, | ||
| int accelPedalPosition1; | ||
| int accelPedalPosition2; | ||
| int brakePedalPosition1; | ||
| int brakePedalPosition2; | ||
| }; | ||
|
|
||
| struct PedalsSystemData_s | ||
| struct PedalsSystemInterface | ||
| { | ||
| PedalsCommanded_e pedalsCommand; | ||
| PedalsStatus_e accelStatus; | ||
| PedalsStatus_e brakeStatus; | ||
| bool persistentImplausibilityDetected; | ||
| float accelPercent; | ||
| float brakePercent; | ||
| bool accelImplausible; | ||
| bool brakeImplausible; | ||
| bool brakeAndAccelPressedImplausibility; | ||
| bool isBraking; | ||
| int requestedTorque; | ||
| }; | ||
|
|
||
| struct PedalsSystemParameters_s | ||
| /// @brief Pedals params struct that will hold min / max that will be used for evaluateion. | ||
| // NOTE: min and max may be need to be flipped depending on the sensor. (looking at you brake pedal sensor 2) | ||
| struct PedalsParams | ||
| { | ||
| float pedalsImplausiblePercent; | ||
| float pedalsMarginalPercent; | ||
| float pedalsRawTooLow; | ||
| float pedalsRawTooHigh; | ||
| float accelPressedThreshold = 0.10; | ||
| float brakePressedThreshold = 0.05; | ||
| }; | ||
| int min_sense_1; | ||
| int min_sense_2; | ||
| int max_sense_1; | ||
| int max_sense_2; | ||
| int start_sense_1; | ||
| int start_sense_2; | ||
| int end_sense_1; | ||
| int end_sense_2; | ||
| }; | ||
|
|
||
| class PedalsSystem | ||
| { | ||
| private: | ||
| // Data | ||
| PedalsSystemParameters_s parameters_; | ||
| int implausibilityDetectedTime_; | ||
| PedalsSystemData_s data_; | ||
|
|
||
| bool evaluate_brake_and_accel_pressed_(); | ||
|
|
||
| bool pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold); | ||
|
|
||
| public: | ||
| // Constructors | ||
| PedalsSystem(PedalsSystemParameters_s* parametersExt) | ||
| { | ||
| parameters_ = *parametersExt; | ||
| } | ||
| PedalsSystem() | ||
| { | ||
| parameters_ = { | ||
| .pedalsImplausiblePercent = PEDALS_IMPLAUSIBLE_PERCENT, | ||
| .pedalsMarginalPercent = PEDALS_MARGINAL_PERCENT, | ||
| .pedalsRawTooLow = PEDALS_RAW_TOO_LOW, | ||
| .pedalsRawTooHigh = PEDALS_RAW_TOO_HIGH, | ||
| }; | ||
| } | ||
| PedalsSystem(){ | ||
| implausibilityStartTime_ = 0; | ||
| // Setting of min and maxes for pedals via config file | ||
| }; | ||
| PedalsSystemInterface evaluate_pedals( | ||
| const PedalsDriverInterface &pedal_data, unsigned long curr_time); | ||
| bool max_duration_of_implausibility_exceeded(unsigned long curr_time); | ||
| bool mech_brake_active(); | ||
|
|
||
| private: | ||
| std::tuple<int, int> linearize_accel_pedal_values_(int accel1, int accel2); | ||
|
|
||
| // Functions | ||
| bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_differnce); | ||
|
|
||
| bool max_duration_of_implausibility_exceeded(unsigned long t); | ||
| void tick( | ||
| const SysTick_s &sysClock, | ||
| const AnalogConversion_s &accel1, | ||
| const AnalogConversion_s &accel2, | ||
| const AnalogConversion_s &brake1, | ||
| const AnalogConversion_s &brake2 | ||
| ); | ||
| const PedalsSystemData_s& getPedalsSystemData() | ||
| { | ||
| return data_; | ||
| } | ||
| bool evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data); | ||
| bool pedal_is_active_(int sense1, int sense_2, const PedalsParams& pedalParams, float percent_threshold); | ||
| PedalsParams accelParams_; | ||
| PedalsParams brakeParams_; | ||
| unsigned long implausibilityStartTime_; | ||
| }; | ||
|
|
||
| #endif /* __PEDALSSYSTEM_H__ */ | ||
|
|
||
| #endif /* PEDALSSYSTEM */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,111 +1,82 @@ | ||
| #include <PedalsSystem.h> | ||
| #include "PedalsSystem.h" | ||
|
|
||
| void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) | ||
| // TODO parameterize percentages in constructor | ||
| PedalsSystemInterface PedalsSystem::evaluate_pedals(const PedalsDriverInterface &data, unsigned long curr_time) | ||
| { | ||
| float accelAverage = (accel1.conversion + accel2.conversion) / 2.0; | ||
| float brakeAverage = (brake1.conversion + brake2.conversion) / 2.0; | ||
| PedalsSystemInterface out; | ||
| out.accelImplausible = evaluate_pedal_implausibilities_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); | ||
| out.brakeImplausible = evaluate_pedal_implausibilities_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.25); | ||
| out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(data); | ||
| bool implausibility = (out.brakeAndAccelPressedImplausibility || out.brakeImplausible || out.accelImplausible); | ||
|
|
||
| bool accelInRange1 = (accel1.raw < parameters_.pedalsRawTooHigh) && (accel1.raw > parameters_.pedalsRawTooLow); | ||
| bool accelInRange2 = (accel2.raw < parameters_.pedalsRawTooHigh) && (accel2.raw > parameters_.pedalsRawTooLow); | ||
| bool brakeInRange1 = (brake1.raw < parameters_.pedalsRawTooHigh) && (brake1.raw > parameters_.pedalsRawTooLow); | ||
| bool brakeInRange2 = (brake2.raw < parameters_.pedalsRawTooHigh) && (brake2.raw > parameters_.pedalsRawTooLow); | ||
| if (implausibility && (implausibilityStartTime_ == 0)){ | ||
| implausibilityStartTime_ = curr_time; | ||
| } else if (!implausibility) | ||
| { | ||
| implausibilityStartTime_ = 0; | ||
| } | ||
|
|
||
| if (accelInRange1 && accelInRange2) | ||
| data_.accelPercent = accelAverage; | ||
| else if (accelInRange1) | ||
| data_.accelPercent = accel1.conversion; | ||
| else if (accelInRange2) | ||
| data_.accelPercent = accel2.conversion; | ||
| else | ||
| data_.accelPercent = 0.0; | ||
| return out; | ||
| } | ||
|
|
||
| if (brakeInRange1 && brakeInRange2) | ||
| data_.brakePercent = brakeAverage; | ||
| else if (brakeInRange1) | ||
| data_.brakePercent = brake1.conversion; | ||
| else if (brakeInRange2) | ||
| data_.brakePercent = brake2.conversion; | ||
| else | ||
| data_.brakePercent = 0.0; | ||
| // TODO parameterize duration in constructor | ||
| bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_time) | ||
| { | ||
| if(implausibilityStartTime_ !=0){ | ||
| return ((curr_time - implausibilityStartTime_) > 100); | ||
| } else { | ||
| return false; | ||
| } | ||
|
|
||
| } | ||
|
|
||
| bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_diff) | ||
| { | ||
| // FSAE EV.5.5 | ||
| // FSAE T.4.2.10 | ||
| bool pedal_1_less_than_min = (sense_1 < params.min_sense_1); | ||
| bool pedal_2_less_than_min = (sense_2 < params.min_sense_2); | ||
| bool pedal_1_greater_than_max = (sense_1 > params.max_sense_1); | ||
| bool pedal_2_greater_than_max = (sense_2 > params.max_sense_2); | ||
|
|
||
| // Check instantaneous implausibility | ||
| // check that the pedals are reading within 10% of each other | ||
| // T.4.2.4 | ||
| // T.4.2.10 | ||
| float accelDeviation = abs(accel1.conversion - accel2.conversion) / accelAverage; | ||
| if ((accelDeviation >= parameters_.pedalsImplausiblePercent) || !accelInRange1 || !accelInRange2) | ||
| { | ||
| data_.accelStatus = PEDALS_IMPLAUSIBLE; | ||
| } | ||
| else if (accelDeviation >= parameters_.pedalsMarginalPercent && accelDeviation < parameters_.pedalsImplausiblePercent) | ||
| { | ||
| data_.accelStatus = PEDALS_MARGINAL; | ||
| } | ||
| else | ||
| { | ||
| data_.accelStatus = PEDALS_NOMINAL; | ||
| } | ||
| float scaled_pedal_1 = (sense_1 - params.start_sense_1) / (params.end_sense_1 - params.start_sense_1); | ||
| float scaled_pedal_2 = (sense_2 - params.start_sense_2) / (params.end_sense_2 - params.start_sense_2); | ||
| bool sens_not_within_req_percent = (fabs(scaled_pedal_1 - scaled_pedal_2) > max_percent_diff); | ||
|
|
||
| float brakeDeviation = abs(brake1.conversion - brake2.conversion) / brakeAverage; | ||
| if ((brakeDeviation >= parameters_.pedalsImplausiblePercent) || !brakeInRange1 || !brakeInRange2) | ||
| { | ||
| data_.brakeStatus = PEDALS_IMPLAUSIBLE; | ||
| } | ||
| else if (brakeDeviation >= parameters_.pedalsMarginalPercent && brakeDeviation < parameters_.pedalsImplausiblePercent) | ||
| { | ||
| data_.brakeStatus = PEDALS_MARGINAL; | ||
| } | ||
| else | ||
| if ( | ||
| pedal_1_less_than_min || | ||
| pedal_2_less_than_min || | ||
| pedal_1_greater_than_max || | ||
| pedal_2_greater_than_max) | ||
| { | ||
| data_.brakeStatus = PEDALS_NOMINAL; | ||
| return true; | ||
| } | ||
|
|
||
| // Check if both pedals are pressed | ||
| bool accelPressed = data_.accelPercent > parameters_.accelPressedThreshold; | ||
| bool brakePressed = data_.brakePercent > parameters_.brakePressedThreshold; | ||
| if (accelPressed && brakePressed) | ||
| data_.pedalsCommand = PEDALS_BOTH_PRESSED; | ||
| else if (accelPressed) | ||
| data_.pedalsCommand = PEDALS_ACCEL_PRESSED; | ||
| else if (brakePressed) | ||
| data_.pedalsCommand = PEDALS_BRAKE_PRESSED; | ||
| else | ||
| data_.pedalsCommand = PEDALS_NONE_PRESSED; | ||
|
|
||
| // Check for persistent implausibility | ||
| if ((data_.accelStatus == PEDALS_IMPLAUSIBLE) || (data_.brakeStatus == PEDALS_IMPLAUSIBLE)) | ||
| else if (sens_not_within_req_percent) | ||
| { | ||
| if (implausibilityDetectedTime_ == 0) | ||
| implausibilityDetectedTime_ = tick.millis; | ||
| if (tick.millis - implausibilityDetectedTime_ >= PEDALS_IMPLAUSIBLE_DURATION) | ||
| data_.persistentImplausibilityDetected = true; | ||
| return true; | ||
| } | ||
| else | ||
| { | ||
| implausibilityDetectedTime_ = 0; | ||
| data_.persistentImplausibilityDetected = false; | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| // | ||
| // bool PedalsSystem::mech_brake_active() | ||
| // { | ||
| // return pedal_is_active_(); | ||
| // } | ||
|
|
||
| bool PedalsSystem::evaluate_brake_and_accel_pressed_() | ||
| bool PedalsSystem::evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data) | ||
| { | ||
|
|
||
| // bool accel_pressed = pedal_is_active_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); | ||
| // bool brake_pressed = pedal_is_active_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.05); | ||
| bool accel_pressed = pedal_is_active_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); | ||
| bool brake_pressed = pedal_is_active_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.05); | ||
|
|
||
| // return (accel_pressed && brake_pressed); | ||
| return (accel_pressed && brake_pressed); | ||
|
|
||
| } | ||
|
|
||
| bool PedalsSystem::pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold) | ||
| bool PedalsSystem::pedal_is_active_(int sense_1, int sense_2, const PedalsParams &pedalParams, float percent_threshold) | ||
| { | ||
| // bool pedal_1_is_active = (sense_1 > (((pedalParams.end_sense_1 - pedalParams.start_sense_1) * percent_threshold) + pedalParams.start_sense_1)); | ||
| // bool pedal_2_is_active = (sense_2 > (((pedalParams.end_sense_2 - pedalParams.start_sense_2) * percent_threshold) + pedalParams.start_sense_2)); | ||
| bool pedal_1_is_active = (sense_1 > (((pedalParams.end_sense_1 - pedalParams.start_sense_1) * percent_threshold) + pedalParams.start_sense_1)); | ||
| bool pedal_2_is_active = (sense_2 > (((pedalParams.end_sense_2 - pedalParams.start_sense_2) * percent_threshold) + pedalParams.start_sense_2)); | ||
|
|
||
| // return (pedal_1_is_active || pedal_2_is_active); | ||
| } | ||
| return (pedal_1_is_active || pedal_2_is_active); | ||
| } |