Add timeout + raw value access for sensors (#1098)

* timeout

* inject stamp

* allow getting raw value

* plumb timeout

* fix tests

firmware/controllers/sensors/functional_sensor.cpp

@@ -1,19 +1,21 @@
 #include "functional_sensor.h"
 
-void FunctionalSensor::postRawValue(float inputValue) {
+void FunctionalSensor::postRawValue(float inputValue, efitick_t timestamp) {
 	// If no function is set, this sensor isn't valid.
 	if (!m_function) {
 		invalidate();
 		return;
 	}
 
+	m_rawValue = inputValue;
+
 	auto r = m_function->convert(inputValue);
 
 	// This has to happen so that we set the valid bit after
 	// the value is stored, to prevent the data race of reading
 	// an old invalid value
 	if (r.Valid) {
-		setValidValue(r.Value);
+		setValidValue(r.Value, timestamp);
 	} else {
 		invalidate();
 	}

firmware/controllers/sensors/functional_sensor.h

@@ -25,16 +25,22 @@
  */
 class FunctionalSensor final : public StoredValueSensor {
 public:
-	explicit FunctionalSensor(SensorType type)
-		: StoredValueSensor(type) { }
+	explicit FunctionalSensor(SensorType type, efitick_t timeoutPeriod)
+		: StoredValueSensor(type, timeoutPeriod) { }
 
-	void postRawValue(float inputValue);
+	void postRawValue(float inputValue, efitick_t timestamp);
 
 	void setFunction(SensorConverter& func) {
 		m_function = &func;
 	}
 
+	float getRaw() const override final {
+		return m_rawValue;
+	}
+
 private:
 	// Conversion function for this sensor
 	SensorConverter* m_function = nullptr;
+
+	float m_rawValue = 0;
 };

firmware/controllers/sensors/sensor.cpp

@@ -79,6 +79,23 @@ bool Sensor::Register() {
 	return {false, 0};
 }
 
+/*static*/ float Sensor::getRaw(SensorType type) {
+	const auto entry = getEntryForType(type);
+
+	// Check if this is a valid sensor entry
+	if (!entry) {
+		return 0;
+	}
+
+	const auto s = entry->sensor;
+	if (s) {
+		return s->getRaw();
+	}
+
+	// We've exhausted all valid ways to return something - sensor not found.
+	return 0;
+}
+
 /*static*/ void Sensor::setMockValue(SensorType type, float value) {
 	auto entry = getEntryForType(type);
 

firmware/controllers/sensors/sensor.h

@@ -81,6 +81,11 @@ public:
 	 */
 	static SensorResult get(SensorType type);
 
+	/*
+	 * Get a raw (unconverted) value from the sensor, if available.
+	 */
+	static float getRaw(SensorType type);
+
 	/*
 	 * Mock a value for a particular sensor.
 	 */
@@ -114,6 +119,13 @@ private:
 	// this should be field lookup and simple math.
 	virtual SensorResult get() const = 0;
 
+	/*
+	 * Get an unconverted value from the sensor, if available.
+	 */
+	virtual float getRaw() const {
+		return 0;
+	}
+
 	SensorType m_type;
 
 	// Get this sensor's index in the list

firmware/controllers/sensors/stored_value_sensor.h

@@ -4,13 +4,15 @@
  * set, then later retrieved by a consumer.
  *
  * @date September 12, 2019
- * @author Matthew Kennedy, (c) 2019
+ * @author Matthew Kennedy, (c) 2019-2020
  */
 
 #pragma once
 
 #include "sensor.h"
 
+#include "efitime.h"
+
 /**
  * @brief Base class for sensors that compute a value on one thread, and want
  * to make it available to consumers asynchronously.
@@ -31,12 +33,21 @@ public:
 		bool valid = m_isValid;
 		float value = m_value;
 
-		return {valid, value};
+		if (!valid) {
+			return {false, value};
+		}
+
+		if (getTimeNowNt() - m_timeoutPeriod > m_lastUpdate) {
+			return {false, value};
+		}
+
+		return {true, value};
 	}
 
 protected:
-	explicit StoredValueSensor(SensorType type)
+	explicit StoredValueSensor(SensorType type, efitick_t timeoutNt)
 		: Sensor(type)
+		, m_timeoutPeriod(timeoutNt)
 	{
 	}
 
@@ -46,13 +57,17 @@ protected:
 	}
 
 	// A new reading is available: set and validate a new value for the sensor.
-	void setValidValue(float value) {
+	void setValidValue(float value, efitick_t timestamp) {
 		// Set value before valid - so we don't briefly have the valid bit set on an invalid value
 		m_value = value;
 		m_isValid = true;
+		m_lastUpdate = timestamp;
 	}
 
 private:
 	bool m_isValid = false;
 	float m_value = 0.0f;
+
+	const efitick_t m_timeoutPeriod;
+	efitick_t m_lastUpdate = 0;
 };

firmware/hw_layer/adc_subscription.cpp

@@ -49,13 +49,15 @@ void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 void AdcSubscription::UpdateSubscribers() {
 	ScopePerf perf(PE::AdcSubscriptionUpdateSubscribers);
 
+	auto timestamp = getTimeNowNt();
+
 	for (size_t i = 0; i < s_nextEntry; i++) {
 		auto &entry = s_entries[i];
 
 		float mcuVolts = getVoltage("sensor", entry.Channel);
 		float sensorVolts = mcuVolts * entry.VoltsPerAdcVolt;
 
-		entry.Sensor->postRawValue(sensorVolts);
+		entry.Sensor->postRawValue(sensorVolts, timestamp);
 	}
 }
 

firmware/init/sensor/init_oil_pressure.cpp

@@ -11,7 +11,7 @@
 EXTERN_ENGINE;
 
 LinearFunc oilpSensorFunc;
-FunctionalSensor oilpSensor(SensorType::OilPressure);
+FunctionalSensor oilpSensor(SensorType::OilPressure, /* timeout = */ MS2NT(50));
 
 void initOilPressure() {
 	// Only register if we have a sensor

unit_tests/tests/sensor/func_sensor.cpp

@@ -1,4 +1,5 @@
 #include "functional_sensor.h"
+#include "global.h"
 
 #include <gtest/gtest.h>
 
@@ -14,7 +15,7 @@ struct DoublerFunc final : public SensorConverter {
 class SensorConverted : public ::testing::Test {
 protected:
 	SensorConverted()
-		: dut(SensorType::Clt) {}
+		: dut(SensorType::Clt, MS2NT(50)) {}
 
 	void SetUp() override {
 		dut.setFunction(func);
@@ -38,7 +39,7 @@ TEST_F(SensorConverted, TestValid) {
 		EXPECT_FALSE(s.Valid);
 	}
 
-	dut.postRawValue(25);
+	dut.postRawValue(25, 0);
 
 	// Should be valid, with a value of 25*2 = 50
 	{
@@ -57,7 +58,7 @@ TEST_F(SensorConverted, TestInvalid) {
 		EXPECT_FALSE(s.Valid);
 	}
 
-	dut.postRawValue(-25);
+	dut.postRawValue(-25, 0);
 
 	// Should be invalid, with a value of -25*2 = 0
 	{

unit_tests/tests/sensor/mock/mock_sensor.h

@@ -1,16 +1,17 @@
 #pragma once
 
 #include "stored_value_sensor.h"
+#include "global.h"
 
 struct MockSensor final : public StoredValueSensor
 {
-	MockSensor(SensorType type) : StoredValueSensor(type)
+	MockSensor(SensorType type) : StoredValueSensor(type, MS2NT(50))
 	{
 	}
 
 	void set(float value)
 	{
-		setValidValue(value);
+		setValidValue(value, 0);
 	}
 
 	void invalidate()