Add new sensor function types (#1065)

* add functions

* add tests

* add chain tests

* float suffix

firmware/controllers/sensors/converters/func_chain.h

@@ -0,0 +1,82 @@
+/**
+ * @author Matthew Kennedy, (c) 2019
+ * 
+ * This lets us compose multiple functions in to a single function. If we have
+ * conversion functions F(x), G(x), and H(x), we can define a new function
+ * FuncChain<F, G, H> that will compute H(G(F(X))).  F first, then G, then H.
+ */
+
+#pragma once
+
+#include "sensor_converter_func.h"
+
+#include <type_traits>
+#include <utility>
+
+namespace priv {
+template <class... _Types>
+class FuncChain;
+
+template <>
+class FuncChain<> {
+protected:
+	SensorResult convert(float input) const {
+		// Base case is the identity function
+		return {true, input};
+	}
+};
+
+template <typename TFirst, typename... TRest>
+class FuncChain<TFirst, TRest...> : private FuncChain<TRest...> {
+	static_assert(std::is_base_of_v<SensorConverter, TFirst>, "Template parameters must inherit from SensorConverter");
+
+private:
+	using TBase = FuncChain<TRest...>;
+
+public:
+	SensorResult convert(float input) const {
+		// Convert the current step
+		SensorResult currentStep = m_f.convert(input);
+
+		// if it was valid, pass this result to the chain of (n-1) functions that remain
+		if (currentStep.Valid) {
+			return TBase::convert(currentStep.Value);
+		} else {
+			return {false, 0};
+		}
+	}
+
+	// Get the element in the current level
+	template <class TGet>
+	std::enable_if_t<std::is_same_v<TGet, TFirst>, TGet &> get() {
+		return m_f;
+	}
+
+	// We don't have it - check level (n - 1)
+	template <class TGet>
+	std::enable_if_t<!std::is_same_v<TGet, TFirst>, TGet &> get() {
+		return TBase::template get<TGet>();
+	}
+
+private:
+	TFirst m_f;
+};
+} // namespace priv
+
+template <typename... TFuncs>
+class FuncChain : public SensorConverter {
+public:
+	// Perform chained conversion of all functions in TFuncs
+	SensorResult convert(float input) const override {
+		return m_fs.convert(input);
+	}
+
+	// Access the sub-function of type TGet
+	template <typename TGet>
+	TGet &get() {
+		return m_fs.template get<TGet>();
+	}
+
+private:
+	priv::FuncChain<TFuncs...> m_fs;
+};

firmware/controllers/sensors/converters/resistance_func.cpp

@@ -0,0 +1,27 @@
+/**
+ * @author Matthew Kennedy, (c) 2019
+ */
+
+#include "resistance_func.h"
+
+void ResistanceFunc::configure(float supplyVoltage, float pullupResistor) {
+	m_pullupResistor = pullupResistor;
+	m_supplyVoltage = supplyVoltage;
+}
+
+SensorResult ResistanceFunc::convert(float raw) const {
+	// If the voltage is very low, the sensor is a dead short.
+	if (raw < 0.05f) {
+		return {false, 0.0f};
+	}
+
+	// If the voltage is very high (95% VCC), the sensor is open circuit.
+	if (raw > (m_supplyVoltage * 0.95f)) {
+		return {false, 1e6};
+	}
+
+	// Voltage is in a sensible range - convert
+	float resistance = m_pullupResistor / (m_supplyVoltage / raw - 1);
+
+	return {true, resistance};
+}

firmware/controllers/sensors/converters/resistance_func.h

@@ -0,0 +1,22 @@
+/**
+ * @author Matthew Kennedy, (c) 2019
+ * 
+ * A function to convert input voltage to resistance in a voltage divider.
+ * Configured with the value of the pullup resistor, and the voltage to which
+ * it's connected.
+ */
+
+#pragma once
+
+#include "sensor_converter_func.h"
+
+class ResistanceFunc final : public SensorConverter {
+public:
+	void configure(float supplyVoltage, float pullupResistor);
+
+	SensorResult convert(float inputValue) const override;
+
+private:
+	float m_supplyVoltage = 5.0f;
+	float m_pullupResistor = 1000.0f;
+};

firmware/controllers/sensors/converters/thermistor_func.cpp

@@ -0,0 +1,47 @@
+/**
+ * @author Matthew Kennedy, (c) 2019
+ */
+
+#include "thermistor_func.h"
+
+#include "thermistors.h"
+
+#include <math.h>
+
+SensorResult ThermistorFunc::convert(float ohms) const {
+	// This resistance should have already been validated - only
+	// thing we can check is that it's non-negative
+	if (ohms <= 0) {
+		return {false, 0};
+	}
+
+	float lnR = logf(ohms);
+
+	float lnR3 = lnR * lnR * lnR;
+
+	float recip = m_a + m_b * lnR + m_c * lnR3;
+
+	float kelvin = 1 / recip;
+
+	float celsius = convertKelvinToCelcius(kelvin);
+
+	return {true, celsius};
+}
+
+void ThermistorFunc::configure(thermistor_conf_s &cfg) {
+	// https://en.wikipedia.org/wiki/Steinhart%E2%80%93Hart_equation
+	float l1 = logf(cfg.resistance_1);
+	float l2 = logf(cfg.resistance_2);
+	float l3 = logf(cfg.resistance_3);
+
+	float y1 = 1 / convertCelsiusToKelvin(cfg.tempC_1);
+	float y2 = 1 / convertCelsiusToKelvin(cfg.tempC_2);
+	float y3 = 1 / convertCelsiusToKelvin(cfg.tempC_3);
+
+	float u2 = (y2 - y1) / (l2 - l1);
+	float u3 = (y3 - y1) / (l3 - l1);
+
+	m_c = ((u3 - u2) / (l3 - l2)) / (l1 + l2 + l3);
+	m_b = u2 - m_c * (l1 * l1 + l1 * l2 + l2 * l2);
+	m_a = y1 - (m_b + l1 * l1 * m_c) * l1;
+}

firmware/controllers/sensors/converters/thermistor_func.h

@@ -0,0 +1,24 @@
+/**
+ * @author Matthew Kennedy, (c) 2019
+ * 
+ * A function to convert resistance to thermistor temperature (NTC). Uses the
+ * Steinhart-Hart equation to prevent having to compute many logarithms at runtime.
+ */
+
+#pragma once
+
+#include "engine_configuration_generated_structures.h"
+#include "sensor_converter_func.h"
+
+class ThermistorFunc final : public SensorConverter {
+public:
+	SensorResult convert(float ohms) const override;
+
+	void configure(thermistor_conf_s &cfg);
+
+private:
+	// Steinhart-Hart coefficients
+	float m_a;
+	float m_b;
+	float m_c;
+};

firmware/controllers/sensors/sensors.mk

@@ -12,4 +12,6 @@ CONTROLLERS_SENSORS_SRC_CPP = 	$(PROJECT_DIR)/controllers/sensors/thermistors.cp
 	$(PROJECT_DIR)/controllers/sensors/hip9011_lookup.cpp \
 	$(PROJECT_DIR)/controllers/sensors/sensor.cpp \
 	$(PROJECT_DIR)/controllers/sensors/functional_sensor.cpp \
-	$(PROJECT_DIR)/controllers/sensors/converters/linear_func.cpp
+	$(PROJECT_DIR)/controllers/sensors/converters/linear_func.cpp \
+	$(PROJECT_DIR)/controllers/sensors/convreters/resistance_func.cpp \
+	$(PROJECT_DIR)/controllers/sensors/convreters/thermistor_func.cpp

unit_tests/tests/sensor/func_chain.cpp

@@ -0,0 +1,84 @@
+#include "func_chain.h"
+
+#include <gtest/gtest.h>
+
+struct AddOne final : public SensorConverter {
+    SensorResult convert(float input) const {
+        return {true, input + 1};
+    }
+};
+
+struct SubOne final : public SensorConverter {
+    SensorResult convert(float input) const {
+        return {true, input - 1};
+    }
+};
+
+struct Doubler final : public SensorConverter {
+    SensorResult convert(float input) const {
+        return {true, input * 2};
+    }
+};
+
+TEST(FunctionChain, TestSingle)
+{
+    FuncChain<AddOne> fc;
+
+    {
+        auto r = fc.convert(5);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 6);
+    }
+
+    {
+        auto r = fc.convert(10);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 11);
+    }
+}
+
+TEST(FunctionChain, TestDouble)
+{
+    // This computes fc(x) = (x + 1) * 2
+    FuncChain<AddOne, Doubler> fc;
+
+    {
+        auto r = fc.convert(5);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 12);
+    }
+
+    {
+        auto r = fc.convert(10);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 22);
+    }
+}
+
+TEST(FunctionChain, TestTriple)
+{
+    // This computes fc(x) = ((x + 1) * 2) - 1
+    FuncChain<AddOne, Doubler, SubOne> fc;
+
+    {
+        auto r = fc.convert(5);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 11);
+    }
+
+    {
+        auto r = fc.convert(10);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_EQ(r.Value, 21);
+    }
+}
+
+TEST(FunctionChain, TestGet)
+{
+    // No logic here - the test is that it compiles
+    FuncChain<AddOne, Doubler, SubOne> fc;
+
+    fc.get<AddOne>();
+    fc.get<Doubler>();
+    fc.get<SubOne>();
+}

unit_tests/tests/sensor/resist_func.cpp

@@ -0,0 +1,77 @@
+/*
+ * @author Matthew Kennedy, (c) 2019
+ */
+
+#include "unit_test_framework.h"
+#include "resistance_func.h"
+
+TEST(resistance, OutOfRange)
+{
+    ResistanceFunc f;
+    f.configure(5, 10000);
+
+    // Something in the middle should be valid
+    {
+        auto r = f.convert(2.5f);
+        ASSERT_TRUE(r.Valid);
+    }
+
+    // Something near 0.05v should be valid
+    {
+        auto r = f.convert(0.051f);
+        EXPECT_TRUE(r.Valid);
+    }
+
+    // Something just under 0.05v should be invalid
+    {
+        auto r = f.convert(0.049f);
+        EXPECT_FALSE(r.Valid);
+    }
+
+    // Something near 0.95 * 5v should be valid
+    {
+        auto r = f.convert(0.94f * 5);
+        EXPECT_TRUE(r.Valid);
+    }
+
+    // Something just above 0.95 * 5v should be invalid
+    {
+        auto r = f.convert(0.96f * 5);
+        EXPECT_FALSE(r.Valid);
+    }
+}
+
+TEST(resistance, InRange)
+{
+    ResistanceFunc f;
+    f.configure(5, 10000);
+
+    // 1 volt -> 2500 ohms low side
+    {
+        auto r = f.convert(1.0f);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_FLOAT_EQ(r.Value, 2500);
+    }
+
+    // 2 volt -> 6666.667 ohm ohms low side
+    // 20k/3 gives us an exact result
+    {
+        auto r = f.convert(2.0f);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_FLOAT_EQ(r.Value, 20000.0f / 3);
+    }
+
+    // 3 volt -> 15000 ohms low side
+    {
+        auto r = f.convert(3.0f);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_FLOAT_EQ(r.Value, 15000);
+    }
+
+    // 4 volt -> 40000 ohms low side
+    {
+        auto r = f.convert(4.0f);
+        EXPECT_TRUE(r.Valid);
+        EXPECT_FLOAT_EQ(r.Value, 40000);
+    }
+}

unit_tests/tests/sensor/therm_func.cpp

@@ -0,0 +1,36 @@
+/*
+ * @author Matthew Kennedy, (c) 2019
+ */
+
+#include "unit_test_framework.h"
+#include "thermistor_func.h"
+#include "thermistors.h"
+
+TEST(thermistor, Thermistor1) {
+	ThermistorFunc tf;
+	thermistor_conf_s tc = {32, 75, 120, 9500, 2100, 1000, 0};
+	tf.configure(tc);
+
+	SensorResult t = tf.convert(2100);
+	ASSERT_TRUE(t.Valid);
+	ASSERT_FLOAT_EQ(75, t.Value);
+
+	ASSERT_NEAR(-0.003, tf.m_a, EPS4D);
+	ASSERT_NEAR(0.001, tf.m_b, EPS4D);
+	ASSERT_NEAR(0.0, tf.m_c, EPS5D);
+}
+
+TEST(thermistor, ThermistorNeon) {
+	ThermistorFunc tf;
+	// 2003 Neon sensor
+	thermistor_conf_s tc = {0, 30, 100, 32500, 7550, 700, 0};
+	tf.configure(tc);
+
+	SensorResult t = tf.convert(38000);
+	ASSERT_TRUE(t.Valid);
+	ASSERT_NEAR(-2.7983, t.Value, EPS4D);
+
+	assertEqualsM("A", 0.0009, tf.m_a);
+	assertEqualsM("B", 0.0003, tf.m_b);
+	ASSERT_NEAR(0.0, tf.m_c, EPS4D);
+}

unit_tests/tests/tests.mk

@@ -38,4 +38,7 @@ TESTS_SRC_CPP = \
 	tests/sensor/function_pointer_sensor.cpp \
 	tests/sensor/mock_sensor.cpp \
 	tests/sensor/sensor_reader.cpp \
-	tests/sensor/lin_func.cpp
+	tests/sensor/lin_func.cpp \
+	tests/sensor/resist_func.cpp \
+	tests/sensor/therm_func.cpp \
+	tests/sensor/func_chain.cpp