Pedals system cleanup and testing

lib/state_machine/include/MCUStateMachine.tpp

@@ -28,7 +28,7 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
             set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis);
             break;
         }
-        if (dashboard_->startButtonPressed() && (data.pedalsCommand == PedalsCommanded_e::PEDALS_BRAKE_PRESSED))
+        if (dashboard_->startButtonPressed() && (data.brakePressed))
         {
             set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis);
             break;

lib/systems/include/PedalsSystem.h

@@ -1,106 +1,77 @@
-#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
-{
-    PEDALS_NONE_PRESSED = 0,
-    PEDALS_ACCEL_PRESSED = 1,
-    PEDALS_BRAKE_PRESSED = 2,
-    PEDALS_BOTH_PRESSED = 3,
-};
 
+#include "SysClock.h"
 struct PedalsSystemData_s
 {
-    PedalsCommanded_e pedalsCommand;
-    PedalsStatus_e accelStatus;
-    PedalsStatus_e brakeStatus;
+
+    bool accelImplausible;
+    bool brakeImplausible;
+    bool brakePressed;
+    bool brakeAndAccelPressedImplausibility;
     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;
+    float activation_percentage;
 };
 
 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 update_parameters(const PedalsSystemParameters_s &new_params)
+    const PedalsSystemData_s &getPedalsSystemData()
     {
-        parameters_ = new_params;
+        return data_;
     }
-    PedalsSystem()
+    PedalsSystem(const PedalsParams &accelParams, const PedalsParams &brakeParams)
     {
-        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;
-
-    }
-
-// Functions
+        accelParams_ = accelParams;
+        brakeParams_ = brakeParams;
+        implausibilityStartTime_ = 0; // ms
+        // Setting of min and maxes for pedals via config file
+    };
 
-    // 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
-    );
+    void tick(const SysTick_s &tick,
+              const AnalogConversion_s &accel1,
+              const AnalogConversion_s &accel2,
+              const AnalogConversion_s &brake1,
+              const AnalogConversion_s &brake2);
 
-
-    const PedalsSystemData_s& getPedalsSystemData()
-    {
-        return data_;
-    }
-};
+    PedalsSystemData_s evaluate_pedals(const AnalogConversion_s &accel1,
+                                       const AnalogConversion_s &accel2,
+                                       const AnalogConversion_s &brake1,
+                                       const AnalogConversion_s &brake2,
+                                       unsigned long curr_time);
 
-#endif /* __PEDALSSYSTEM_H__ */
+private:
+    PedalsSystemData_s data_;
+
+    bool max_duration_of_implausibility_exceeded_(unsigned long curr_time);
+    bool evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData1,
+                                          const AnalogConversion_s &pedalData2,
+                                          const PedalsParams &params,
+                                          float max_percent_diff);
+
+    bool evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1,
+                                           const AnalogConversion_s &accelPedalData2,
+                                           const AnalogConversion_s &brakePedalData1,
+                                           const AnalogConversion_s &brakePedalData2);
+    bool pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, float percent_threshold);
+    PedalsParams accelParams_;
+    PedalsParams brakeParams_;
+    unsigned long implausibilityStartTime_;
+};
 
+#endif /* PEDALSSYSTEM */

lib/systems/include/SysClock.h

@@ -14,8 +14,8 @@ struct TriggerBits_s
 
 struct SysTick_s
 {
-    long millis;
-    long micros;
+    unsigned long millis;
+    unsigned long micros;
     TriggerBits_s triggers;
 };
 

lib/systems/src/PedalsSystem.cpp

@@ -1,111 +1,103 @@
 #include "PedalsSystem.h"
-
+#include <iostream>
+// TODO parameterize percentages in constructor
 void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2)
 {
-    float accelAverage = (accel1.conversion + accel2.conversion) / 2.0;
-    float brakeAverage = (brake1.conversion + brake2.conversion) / 2.0;
-
-    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;
+    data_ = evaluate_pedals(accel1, accel2, brake1, brake2, tick.millis);
+}
+PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel1,
+                                                 const AnalogConversion_s &accel2,
+                                                 const AnalogConversion_s &brake1,
+                                                 const AnalogConversion_s &brake2,
+                                                 unsigned long curr_time)
+{
+    PedalsSystemData_s out;
+    out.accelImplausible = evaluate_pedal_implausibilities_(accel1, accel2, accelParams_, 0.1);
+    out.brakeImplausible = evaluate_pedal_implausibilities_(brake1, brake2, brakeParams_, 0.25);
+    out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(accel1, accel2, brake1, brake2);
+    bool implausibility = (out.brakeAndAccelPressedImplausibility || out.brakeImplausible || out.accelImplausible);
 
-    // 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)
+    out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0;
+    out.brakePercent = (brake1.conversion + brake2.conversion) / 2.0;
+    out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_.activation_percentage);
+    out.implausibilityExceededMaxDuration = max_duration_of_implausibility_exceeded_(curr_time);
+    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_(const AnalogConversion_s &pedalData1,
+                                                    const AnalogConversion_s &pedalData2,
+                                                    const PedalsParams &params,
+                                                    float max_percent_diff)
+{
+    // FSAE EV.5.5
+    // FSAE T.4.2.10
+    bool pedal_1_less_than_min = (pedalData1.raw < params.min_sense_1);
+    bool pedal_2_less_than_min = (pedalData2.raw < params.min_sense_2);
+    bool pedal_1_greater_than_max = (pedalData1.raw > params.max_sense_1);
+    bool pedal_2_greater_than_max = (pedalData2.raw > 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
+    bool sens_not_within_req_percent = (fabs(pedalData1.conversion - pedalData2.conversion) > max_percent_diff);
+
+    bool pedalsClamped = (pedalData1.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED || pedalData2.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED);
+    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 || pedalsClamped)
     {
-        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 AnalogConversion_s &accelPedalData1,
+                                                     const AnalogConversion_s &accelPedalData2,
+                                                     const AnalogConversion_s &brakePedalData1,
+                                                     const AnalogConversion_s &brakePedalData2)
 {
 
-    // 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_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_.activation_percentage); // .1
+    bool brake_pressed = pedal_is_active_(brakePedalData2.conversion, brakePedalData1.conversion, brakeParams_.activation_percentage); // 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_(float pedal1ConvertedData, float pedal2ConvertedData, 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 = pedal1ConvertedData >= percent_threshold;
+    bool pedal_2_is_active = pedal2ConvertedData >= percent_threshold;
 
-    // return (pedal_1_is_active || pedal_2_is_active);
-}
+    return (pedal_1_is_active || pedal_2_is_active);
+}

lib/systems/src/TorqueControllers.cpp

@@ -27,7 +27,7 @@ void TorqueControllerSimple::tick(const SysTick_s& tick, const PedalsSystemData_
     // Calculate torque commands at 100hz
     if (tick.triggers.trigger100)
     {
-        if ((pedalsData.pedalsCommand != PedalsCommanded_e::PEDALS_BOTH_PRESSED) && (pedalsData.persistentImplausibilityDetected == false))
+        if ((!pedalsData.brakeAndAccelPressedImplausibility) && (pedalsData.implausibilityExceededMaxDuration== false))
         {
             // Both pedals are not pressed and no implausibility has been detected
             // accelRequest goes between 1.0 and -1.0

src/main.cpp

@@ -61,7 +61,7 @@ InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND, 9, 8);
 SysClock sys_clock;
 BuzzerController buzzer(BUZZER_ON_INTERVAL);
 SafetySystem safety_system(&ams_interface, &wd_interface);  // Tie ams and wd interface to safety system (by pointers)
-PedalsSystem pedals;
+PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.05});
 SteeringSystem steering_system(&steering1);  // Unify member reference and pointers? tied by reference in this case
 using DrivetrainSystemType = DrivetrainSystem<InverterInterfaceType>;
 auto drivetrain = DrivetrainSystemType({&fl_inv, &fr_inv, &rl_inv, &rr_inv}, INVERTER_ENABLING_TIMEOUT_INTERVAL); // Tie inverter interfaces to drivetrain system (by pointers)