Skip to content

Commit

Permalink
going back for now
Browse files Browse the repository at this point in the history
  • Loading branch information
@RCMast3r
RCMast3r committed Feb 7, 2024
1 parent 60e07f5 commit aba88c6
Show file tree
Hide file tree
Showing 3 changed files with 112 additions and 153 deletions.
125 changes: 55 additions & 70 deletions lib/systems/include/PedalsSystem.h
View file
Original file line number Diff line number Diff line change
@@ -1,106 +1,91 @@
#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 class PedalsStatus_e
{
PEDALS_NOMINAL = 0,
PEDALS_MARGINAL = 1,
PEDALS_IMPLAUSIBLE = 2,
};

enum class PedalsCommanded_e
struct PedalsDriverInterface
{
int accelPedalPosition1;
int accelPedalPosition2;
int brakePedalPosition1;
int brakePedalPosition2;
};

struct PedalsSystemInterface
{
PEDALS_NONE_PRESSED = 0,
PEDALS_ACCEL_PRESSED = 1,
PEDALS_BRAKE_PRESSED = 2,
PEDALS_BOTH_PRESSED = 3,
bool accelImplausible;
bool brakeImplausible;
bool brakeAndAccelPressedImplausibility;
bool isBraking;
int requestedTorque;
};

struct PedalsSystemData_s
{
PedalsCommanded_e pedalsCommand;
PedalsStatus_e accelStatus;
PedalsStatus_e brakeStatus;

bool accelImplausible;
bool brakeImplausible;
bool implausibilityExceededMaxDuration;
bool persistentImplausibilityDetected;
float accelPercent;
float brakePercent;
};

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;
}
void tick(
const SysTick_s &sysClock,
const AnalogConversion_s &accel1,
const AnalogConversion_s &accel2,
const AnalogConversion_s &brake1,
const AnalogConversion_s &brake2);

void update_parameters(const PedalsSystemParameters_s &new_params)
const PedalsSystemData_s &getPedalsSystemData()
{
parameters_ = new_params;
return data_;
}
PedalsSystem()
{
parameters_.pedalsImplausiblePercent = PEDALS_IMPLAUSIBLE_PERCENT;
parameters_.pedalsMarginalPercent = PEDALS_MARGINAL_PERCENT;
parameters_.pedalsRawTooLow = PEDALS_RAW_TOO_LOW;
parameters_.pedalsRawTooHigh = PEDALS_RAW_TOO_HIGH;
parameters_.accelPressedThreshold = 0.1;
parameters_.brakePressedThreshold = 0.05;

}
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();

// Functions

// 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
);
private:
std::tuple<int, int> linearize_accel_pedal_values_(int accel1, int accel2);


const PedalsSystemData_s& getPedalsSystemData()
{
return data_;
}
};
bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams &params, float max_percent_differnce);

#endif /* __PEDALSSYSTEM_H__ */
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 */
4 changes: 2 additions & 2 deletions lib/systems/include/SysClock.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@ struct TriggerBits_s

struct SysTick_s
{
long millis;
long micros;
unsigned long millis;
unsigned long micros;
TriggerBits_s triggers;
};

Expand Down
136 changes: 55 additions & 81 deletions lib/systems/src/PedalsSystem.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,111 +1,85 @@
#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 (accelInRange1 && accelInRange2)
data_.accelPercent = accelAverage;
else if (accelInRange1)
data_.accelPercent = accel1.conversion;
else if (accelInRange2)
data_.accelPercent = accel2.conversion;
else
data_.accelPercent = 0.0;

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;

// Check instantaneous implausibility
// T.4.2.4
// T.4.2.10
float accelDeviation = abs(accel1.conversion - accel2.conversion) / accelAverage;
if ((accelDeviation >= parameters_.pedalsImplausiblePercent) || !accelInRange1 || !accelInRange2)
if (implausibility && (implausibilityStartTime_ == 0))
{
data_.accelStatus = PedalsStatus_e::PEDALS_IMPLAUSIBLE;
implausibilityStartTime_ = curr_time;
}
else if (accelDeviation >= parameters_.pedalsMarginalPercent && accelDeviation < parameters_.pedalsImplausiblePercent)
else if (!implausibility)
{
data_.accelStatus = PedalsStatus_e::PEDALS_MARGINAL;
}
else
{
data_.accelStatus = PedalsStatus_e::PEDALS_NOMINAL;
implausibilityStartTime_ = 0;
}

float brakeDeviation = abs(brake1.conversion - brake2.conversion) / brakeAverage;
if ((brakeDeviation >= parameters_.pedalsImplausiblePercent) || !brakeInRange1 || !brakeInRange2)
{
data_.brakeStatus = PedalsStatus_e::PEDALS_IMPLAUSIBLE;
}
else if (brakeDeviation >= parameters_.pedalsMarginalPercent && brakeDeviation < parameters_.pedalsImplausiblePercent)
return out;
}

// TODO parameterize duration in constructor
bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_time)
{
if (implausibilityStartTime_ != 0)
{
data_.brakeStatus = PedalsStatus_e::PEDALS_MARGINAL;
return ((curr_time - implausibilityStartTime_) > 100);
}
else
{
data_.brakeStatus = PedalsStatus_e::PEDALS_NOMINAL;
return false;
}
}

// Check if both pedals are pressed
bool accelPressed = data_.accelPercent > parameters_.accelPressedThreshold;
bool brakePressed = data_.brakePercent > parameters_.brakePressedThreshold;
if (accelPressed && brakePressed)
data_.pedalsCommand = PedalsCommanded_e::PEDALS_BOTH_PRESSED;
else if (accelPressed)
data_.pedalsCommand = PedalsCommanded_e::PEDALS_ACCEL_PRESSED;
else if (brakePressed)
data_.pedalsCommand = PedalsCommanded_e::PEDALS_BRAKE_PRESSED;
else
data_.pedalsCommand = PedalsCommanded_e::PEDALS_NONE_PRESSED;
bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams &params, 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 for persistent implausibility
if ((data_.accelStatus == PedalsStatus_e::PEDALS_IMPLAUSIBLE) || (data_.brakeStatus == PedalsStatus_e::PEDALS_IMPLAUSIBLE))
// check that the pedals are reading within 10% of each other
// T.4.2.4
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);

if (
pedal_1_less_than_min ||
pedal_2_less_than_min ||
pedal_1_greater_than_max ||
pedal_2_greater_than_max)
{
return true;
}
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);

// return (accel_pressed && brake_pressed);
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);
}

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);
}

0 comments on commit aba88c6

Please sign in to comment.