diff --git a/lib/interfaces/include/AnalogSensorsInterface.h b/lib/interfaces/include/AnalogSensorsInterface.h index f4157dc29..83251f964 100644 --- a/lib/interfaces/include/AnalogSensorsInterface.h +++ b/lib/interfaces/include/AnalogSensorsInterface.h @@ -5,21 +5,15 @@ #include #include -enum AnalogSensorStatus_e -{ - ANALOG_SENSOR_GOOD = 0, - ANALOG_SENSOR_CLAMPED = 1, -}; - -struct AnalogConversion_s +struct AnalogSensorReadingAndStatus_s { int raw; - float conversion; - AnalogSensorStatus_e status; + float convertedReading; + bool dataIsClamped; }; template -struct AnalogConversionPacket_s +struct AnalogDataPacket_s { AnalogSensorReadingAndStatus_s conversions[N]; }; @@ -54,13 +48,14 @@ class AnalogChannel { float conversion = lastSample * scale + offset; float clampedConversion = std::min(std::max(conversion, clampLow), clampHigh); - AnalogSensorStatus_e returnStatus = ANALOG_SENSOR_GOOD; + bool clamped = false; + if (clamp && (conversion > clampHigh || conversion < clampLow)) - returnStatus = ANALOG_SENSOR_CLAMPED; + clamped = true; return { lastSample, clamp ? clampedConversion : conversion, - returnStatus + clamped }; } }; @@ -72,7 +67,7 @@ class AnalogMultiSensor public: // Data AnalogChannel channels[N]; - AnalogConversionPacket_s data; + AnalogDataPacket_s data; // Functions /// @brief Called by the main loop. Allows AnalogMultiSensor devices not owned by a single system to self-actualize sampling & conversion. /// @param tick @@ -80,7 +75,7 @@ class AnalogMultiSensor /// @brief Used by systems to get data out of this device when it's self-actualizing sampling & conversion. /// @return Const ref to last data conversion. - const AnalogConversionPacket_s& get(); + const AnalogDataPacket_s& get(); /// @brief Performs unit conversions on all channels void convert() diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index 717a15e6f..ec38a7ce7 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -28,25 +28,25 @@ class TelemetryInterface /* Update CAN messages (main loop) */ // Interfaces void update_pedal_readings_CAN_msg( - const AnalogConversion_s &accel1, - const AnalogConversion_s &accel2, - const AnalogConversion_s &brake1, - const AnalogConversion_s &brake2 + const AnalogSensorReadingAndStatus_s &accel1, + const AnalogSensorReadingAndStatus_s &accel2, + const AnalogSensorReadingAndStatus_s &brake1, + const AnalogSensorReadingAndStatus_s &brake2 ); void update_load_cells_CAN_msg( - const AnalogConversion_s &lc_fl, - const AnalogConversion_s &lc_fr + const AnalogSensorReadingAndStatus_s &lc_fl, + const AnalogSensorReadingAndStatus_s &lc_fr ); void update_potentiometers_CAN_msg( - const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr + const AnalogSensorReadingAndStatus_s &pots_fl, + const AnalogSensorReadingAndStatus_s &pots_fr ); void update_analog_readings_CAN_msg( const SteeringEncoderConversion_s &steer1, - const AnalogConversion_s &steer2, - const AnalogConversion_s ¤t, - const AnalogConversion_s &reference, - const AnalogConversion_s &glv + const AnalogSensorReadingAndStatus_s &steer2, + const AnalogSensorReadingAndStatus_s ¤t, + const AnalogSensorReadingAndStatus_s &reference, + const AnalogSensorReadingAndStatus_s &glv ); /* Enqueue outbound telemetry CAN messages */ diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 8af093aea..ca2ae840a 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -56,10 +56,10 @@ void TelemetryInterface::enqueue_CAN_mcu_analog_readings() { /* Update CAN messages */ // Main loop // MCP3208 returns structure -void TelemetryInterface::update_pedal_readings_CAN_msg(const AnalogConversion_s &accel1, - const AnalogConversion_s &accel2, - const AnalogConversion_s &brake1, - const AnalogConversion_s &brake2) { +void TelemetryInterface::update_pedal_readings_CAN_msg(const AnalogSensorReadingAndStatus_s &accel1, + const AnalogSensorReadingAndStatus_s &accel2, + const AnalogSensorReadingAndStatus_s &brake1, + const AnalogSensorReadingAndStatus_s &brake2) { // do sth with mcu_pedal_readings_ mcu_pedal_readings_.set_accelerator_pedal_1(accel1.raw); mcu_pedal_readings_.set_accelerator_pedal_2(accel2.raw); @@ -67,8 +67,8 @@ void TelemetryInterface::update_pedal_readings_CAN_msg(const AnalogConversion_s mcu_pedal_readings_.set_brake_pedal_2(brake2.raw); } // MCP3204 returns structure -void TelemetryInterface::update_load_cells_CAN_msg(const AnalogConversion_s &lc_fl, - const AnalogConversion_s &lc_fr) { +void TelemetryInterface::update_load_cells_CAN_msg(const AnalogSensorReadingAndStatus_s &lc_fl, + const AnalogSensorReadingAndStatus_s &lc_fr) { // do sth with mcu_load_cells_ mcu_load_cells_.set_RL_load_cell(lc_fl.raw); mcu_load_cells_.set_RR_load_cell(lc_fr.raw); @@ -76,8 +76,8 @@ void TelemetryInterface::update_load_cells_CAN_msg(const AnalogConversion_s &lc_ // mcu_load_cells_.set_FR_load_cell(lc_rr.raw); } // MCP3204 returns structure -void TelemetryInterface::update_potentiometers_CAN_msg(const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr) { +void TelemetryInterface::update_potentiometers_CAN_msg(const AnalogSensorReadingAndStatus_s &pots_fl, + const AnalogSensorReadingAndStatus_s &pots_fr) { // do sth with mcu_front_potentiometers_ mcu_front_potentiometers_.set_pot1(pots_fl.raw); mcu_front_potentiometers_.set_pot3(pots_fr.raw); @@ -88,10 +88,10 @@ void TelemetryInterface::update_potentiometers_CAN_msg(const AnalogConversion_s } // SteeringDual and MCP3208 return structures void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderConversion_s &steer1, - const AnalogConversion_s &steer2, - const AnalogConversion_s ¤t, - const AnalogConversion_s &reference, - const AnalogConversion_s &glv) { + const AnalogSensorReadingAndStatus_s &steer2, + const AnalogSensorReadingAndStatus_s ¤t, + const AnalogSensorReadingAndStatus_s &reference, + const AnalogSensorReadingAndStatus_s &glv) { // do sth with mcu_analog_readings_ mcu_analog_readings_.set_steering_1(steer1.angle); mcu_analog_readings_.set_steering_2(steer2.raw); diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 93bbcf8e7..bbfa40525 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -22,13 +22,13 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: { - PedalsSystemData_s data; + auto data = pedals_->evaluate_pedals(); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } - if (dashboard_->start_button_pressed() && pedals_->mech_brake_active(data)) + if (dashboard_->startButtonPressed() && pedals_->mech_brake_active(data)) { set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); break; diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index e953f46a5..b858ab355 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -2,14 +2,7 @@ #define PEDALSSYSTEM #include #include - -struct PedalsDriverInterface -{ - int accelPedalPosition1; - int accelPedalPosition2; - int brakePedalPosition1; - int brakePedalPosition2; -}; +#include "AnalogSensorsInterface.h" struct PedalsSystemInterface { @@ -20,7 +13,7 @@ struct PedalsSystemInterface int requestedTorque; }; -/// @brief Pedals params struct that will hold min / max that will be used for evaluateion. +/// @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 { @@ -32,31 +25,37 @@ struct PedalsParams int start_sense_2; int end_sense_1; int end_sense_2; -}; +}; class PedalsSystem { public: - PedalsSystem(){ + PedalsSystem() + { implausibilityStartTime_ = 0; // Setting of min and maxes for pedals via config file }; PedalsSystemInterface evaluate_pedals( - const PedalsDriverInterface &pedal_data, unsigned long curr_time); + const AnalogSensorReadingAndStatus_s &accel_pedal1, + const AnalogSensorReadingAndStatus_s &accel_pedal2, + const AnalogSensorReadingAndStatus_s &brake_pedal1, + const AnalogSensorReadingAndStatus_s &brake_pedal2, + unsigned long curr_time); + bool max_duration_of_implausibility_exceeded(unsigned long curr_time); + bool mech_brake_active(); private: std::tuple linearize_accel_pedal_values_(int accel1, int accel2); bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_differnce); - + bool evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data); - bool pedal_is_active_(int sense1, int sense_2, const PedalsParams& pedalParams, float percent_threshold); + bool pedal_is_active_(int sense1, int sense_2, const PedalsParams &pedalParams, float percent_threshold); PedalsParams accelParams_; PedalsParams brakeParams_; unsigned long implausibilityStartTime_; }; - #endif /* PEDALSSYSTEM */ diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index ce3cf8a39..02df17a79 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -42,7 +42,7 @@ class SteeringSystem /// @return SteeringSystemOutput_s contains steering angle and SteeringSystemStatus_e void tick( const SysTick_s &tick, - const AnalogConversion_s &secondaryConversion + const AnalogSensorReadingAndStatus_s &secondaryConversion ); /// @brief Get a reference to the steering system's data diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 4b4ca1c82..515f1c266 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -60,10 +60,10 @@ class TorqueControllerMux const DrivetrainDynamicReport_s& drivetrainData, const PedalsSystemData_s& pedalsData, const SteeringSystemData_s& steeringData, - const AnalogConversion_s& loadFLData, - const AnalogConversion_s& loadFRData, - const AnalogConversion_s& loadRLData, - const AnalogConversion_s& loadRRData, + const AnalogSensorReadingAndStatus_s& loadFLData, + const AnalogSensorReadingAndStatus_s& loadFRData, + const AnalogSensorReadingAndStatus_s& loadRLData, + const AnalogSensorReadingAndStatus_s& loadRRData, DialMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed ); diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index d4f49ffb1..193fd7667 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -1,17 +1,23 @@ #include "PedalsSystem.h" // TODO parameterize percentages in constructor -PedalsSystemInterface PedalsSystem::evaluate_pedals(const PedalsDriverInterface &data, unsigned long curr_time) +PedalsSystemInterface PedalsSystem::evaluate_pedals(const AnalogSensorReadingAndStatus_s &accel_pedal1, + const AnalogSensorReadingAndStatus_s &accel_pedal2, + const AnalogSensorReadingAndStatus_s &brake_pedal1, + const AnalogSensorReadingAndStatus_s &brake_pedal2, + unsigned long curr_time) { PedalsSystemInterface out; - out.accelImplausible = evaluate_pedal_implausibilities_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); + out.accelImplausible = evaluate_pedal_implausibilities_(accel_pedal1.convertedReading, 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); - if (implausibility && (implausibilityStartTime_ == 0)){ + if (implausibility && (implausibilityStartTime_ == 0)) + { implausibilityStartTime_ = curr_time; - } else if (!implausibility) + } + else if (!implausibility) { implausibilityStartTime_ = 0; } @@ -22,12 +28,14 @@ PedalsSystemInterface PedalsSystem::evaluate_pedals(const PedalsDriverInterface // TODO parameterize duration in constructor bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_time) { - if(implausibilityStartTime_ !=0){ + if (implausibilityStartTime_ != 0) + { return ((curr_time - implausibilityStartTime_) > 100); - } else { + } + else + { return false; } - } bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_diff) @@ -68,9 +76,8 @@ bool PedalsSystem::evaluate_brake_and_accel_pressed_(const PedalsDriverInterface 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 PedalsParams &pedalParams, float percent_threshold) diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 51765f32e..f761abe31 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,6 +1,6 @@ #include "SteeringSystem.h" -void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secondaryConversion) +void SteeringSystem::tick(const SysTick_s &tick, const AnalogSensorReadingAndStatus_s &secondaryConversion) { // System works at 100hz // 1. Polls upper steering sensor @@ -14,7 +14,7 @@ void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secon // Compute internal state // Both sensors are nominal - if ((primaryConversion_.status == STEERING_ENCODER_NOMINAL) && (secondaryConversion.status == ANALOG_SENSOR_GOOD)) + if ((primaryConversion_.status == STEERING_ENCODER_NOMINAL) && (!secondaryConversion.dataIsClamped)) { data_ = { .angle = primaryConversion_.angle, @@ -24,8 +24,8 @@ void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secon // One or both sensors are marginal // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees else if ((primaryConversion_.status == STEERING_ENCODER_MARGINAL) - || (secondaryConversion.status == ANALOG_SENSOR_CLAMPED) - || ((std::abs(primaryConversion_.angle - secondaryConversion.conversion) > STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - secondaryConversion.conversion) < STEERING_DIVERGENCE_ERROR_THRESHOLD))) + || (secondaryConversion.dataIsClamped) + || ((std::abs(primaryConversion_.angle - secondaryConversion.convertedReading) > STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - secondaryConversion.convertedReading) < STEERING_DIVERGENCE_ERROR_THRESHOLD))) { data_ = { .angle = primaryConversion_.angle, @@ -33,10 +33,10 @@ void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secon }; } // Upper steering sensor reports error, lower sensor is nominal - else if ((primaryConversion_.status == STEERING_ENCODER_ERROR) && (secondaryConversion.status == ANALOG_SENSOR_GOOD)) + else if ((primaryConversion_.status == STEERING_ENCODER_ERROR) && (!secondaryConversion.dataIsClamped)) { data_ = { - .angle = secondaryConversion.conversion, + .angle = secondaryConversion.convertedReading, .status = STEERING_SYSTEM_DEGRADED }; } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 35bd86b94..1fe2a675c 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -6,10 +6,10 @@ void TorqueControllerMux::tick( const DrivetrainDynamicReport_s& drivetrainData, const PedalsSystemData_s& pedalsData, const SteeringSystemData_s& steeringData, - const AnalogConversion_s& loadFLData, - const AnalogConversion_s& loadFRData, - const AnalogConversion_s& loadRLData, - const AnalogConversion_s& loadRRData, + const AnalogSensorReadingAndStatus_s& loadFLData, + const AnalogSensorReadingAndStatus_s& loadFRData, + const AnalogSensorReadingAndStatus_s& loadRLData, + const AnalogSensorReadingAndStatus_s& loadRRData, DialMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed )