diff --git a/firmware/controllers/sensors/hella_oil_level.cpp b/firmware/controllers/sensors/hella_oil_level.cpp new file mode 100644 index 0000000000..c182b7faff --- /dev/null +++ b/firmware/controllers/sensors/hella_oil_level.cpp @@ -0,0 +1,94 @@ +#include "pch.h" + +#include "hella_oil_level.h" + +static void hellaSensorExtiCallback(void* arg, efitick_t nowNt) { + reinterpret_cast(arg)->onEdge(nowNt); +} + +void HellaOilLevelSensor::init(brain_pin_e pin) { + if (!isBrainPinValid(pin)) { + return; + } + + m_pin = pin; + +#if EFI_PROD_CODE + efiExtiEnablePin(getSensorName(), pin, + PAL_EVENT_MODE_BOTH_EDGES, + hellaSensorExtiCallback, reinterpret_cast(this)); +#endif // EFI_PROD_CODE + + Register(); +} + +void HellaOilLevelSensor::onEdge(efitick_t nowNt) { + if (efiReadPin(m_pin)) { + // Start pulse width timing at the rising edge + m_pulseTimer.reset(nowNt); + + float timeBetweenPulses = m_betweenPulseTimer.getElapsedSecondsAndReset(nowNt); + + if (timeBetweenPulses > 0.89 * 0.780 && timeBetweenPulses < 1.11 * 0.780) { + // 780ms nominal between Diag and next Temp pulse start, +-10% + + // This was the "long gap" break, next pulse is temperature. + m_nextPulse = NextPulse::Temp; + } else if (timeBetweenPulses > 0.89 * 0.110 && timeBetweenPulses < 1.11 * 0.110) { + // 110ms nominal between each pulse (other than break) + + // Advance the state machine to decode the next pulse in the sequence + switch (m_nextPulse) { + case NextPulse::Temp: + m_nextPulse = NextPulse::Level; + break; + case NextPulse::Level: + m_nextPulse = NextPulse::Diag; + break; + default: + // We don't know how we got here, reset to safe state + m_nextPulse = NextPulse::None; + break; + } + } else { + // The break was too long, ignore it for now. + m_nextPulse = NextPulse::None; + } + } else { + // Stop timing at the falling edge + float lastPulseMs = 1000 * m_pulseTimer.getElapsedSeconds(nowNt); + + if (m_nextPulse == NextPulse::Diag) { + // TODO: decode diag pulse? + return; + } else if (m_nextPulse = NextPulse::Temp) { + // 22ms = Short circuit temp sensor + // 23ms = -40C + // 87ms = 160C + // 88ms = Temp sensor defective + + if (lastPulseMs < 22.8) { + // Short circuit + // invalidate(UnexpectedCode::Low); + } else if (lastPulseMs > 87.2) { + // Defective + // invalidate(UnexpectedCode::High); + } else { + float tempC = interpolateClamped(23, -40, 87, 160, lastPulseMs); + // setValidValue(tempC, nowNt); + } + } else if (m_nextPulse = NextPulse::Level) { + // 22ms = Unreliable signal + // 23ms = level 0mm + // 87.86ms = level 150mm + + if (lastPulseMs < 22.8f) { + // Unreliable + invalidate(UnexpectedCode::Low); + } else { + float levelMm = interpolateClamped(23, 0, 87.86, 150, lastPulseMs); + setValidValue(levelMm, nowNt); + } + } + } +} diff --git a/firmware/controllers/sensors/hella_oil_level.h b/firmware/controllers/sensors/hella_oil_level.h new file mode 100644 index 0000000000..0687f0f1f7 --- /dev/null +++ b/firmware/controllers/sensors/hella_oil_level.h @@ -0,0 +1,24 @@ +#pragma once + +#include "stored_value_sensor.h" + +class HellaOilLevelSensor : public StoredValueSensor { +public: + HellaOilLevelSensor(SensorType type) : StoredValueSensor(type, MS2NT(2000)) {} + + void init(brain_pin_e pin); + + void onEdge(efitick_t nowNt); + +private: + brain_pin_e m_pin = Gpio::Unassigned; + + // Measures the width of positive pulses (rising -> falling) + Timer m_pulseTimer; + + // Measures the time between pulses (rising -> rising) + Timer m_betweenPulseTimer; + + enum class NextPulse { None, Temp, Level, Diag }; + NextPulse m_nextPulse = NextPulse::None; +}; diff --git a/firmware/controllers/sensors/sensors.mk b/firmware/controllers/sensors/sensors.mk index 2bcbfba238..2dc642ab5d 100644 --- a/firmware/controllers/sensors/sensors.mk +++ b/firmware/controllers/sensors/sensors.mk @@ -16,6 +16,7 @@ CONTROLLERS_SENSORS_SRC_CPP = \ $(PROJECT_DIR)/controllers/sensors/redundant_sensor.cpp \ $(PROJECT_DIR)/controllers/sensors/redundant_ford_tps.cpp \ $(PROJECT_DIR)/controllers/sensors/frequency_sensor.cpp \ + $(PROJECT_DIR)/controllers/sensors/hella_oil_level.cpp \ $(PROJECT_DIR)/controllers/sensors/impl/software_knock.cpp \ $(PROJECT_DIR)/controllers/sensors/converters/linear_func.cpp \ $(PROJECT_DIR)/controllers/sensors/converters/resistance_func.cpp \