diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp
index 98176d89f2..5ec25830d6 100644
--- a/firmware/controllers/engine_controller.cpp
+++ b/firmware/controllers/engine_controller.cpp
@@ -123,6 +123,8 @@ void initDataStructures(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 
 static void mostCommonInitEngineController(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) {
 #if !EFI_UNIT_TEST
+	// This is tested independently - don't configure sensors for tests.
+	// This lets us selectively mock them for each test.
 	initNewSensors(sharedLogger);
 #endif /* EFI_UNIT_TEST */
 
diff --git a/firmware/hw_layer/adc_subscription.cpp b/firmware/hw_layer/adc_subscription.cpp
index 1cd912cce1..37a24eb62f 100644
--- a/firmware/hw_layer/adc_subscription.cpp
+++ b/firmware/hw_layer/adc_subscription.cpp
@@ -8,7 +8,15 @@
 
 EXTERN_ENGINE;
 
-#if !EFI_UNIT_TEST
+#if EFI_UNIT_TEST
+
+void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
+									  adc_channel_e channel,
+									  float voltsPerAdcVolt /*= 0.0f*/)
+{
+}
+
+#else
 
 struct AdcSubscriptionEntry {
 	FunctionalSensor *Sensor;
diff --git a/firmware/init/init.h b/firmware/init/init.h
index 4de1de25bf..c97a9c9d01 100644
--- a/firmware/init/init.h
+++ b/firmware/init/init.h
@@ -4,13 +4,25 @@
 
 #pragma once
 
+#include "globalaccess.h"
+
 class Logging;
 
 // Call this once at startup to initialize, configure, and subscribe sensors
-void initNewSensors(Logging* logger);
+void initNewSensors(Logging* logger DECLARE_ENGINE_PARAMETER_SUFFIX);
 
 // Call this whenever the configuration may have changed, so any sensors
 // can be reconfigured with the new settings.
 // Note: this may not be necessarily possible for all sensors, so some may
 // do nothing when this is called.
-void reconfigureSensors();
+void reconfigureSensors(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+
+// Internal init functions for individual systems
+// Sensor init/config
+void initTps(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+void initOilPressure(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+void initCanSensors(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+
+// Sensor reconfiguration
+void reconfigureTps(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+void reconfigureOilPressure(DECLARE_ENGINE_PARAMETER_SIGNATURE);
diff --git a/firmware/init/sensor/init_oil_pressure.cpp b/firmware/init/sensor/init_oil_pressure.cpp
index cbc6905723..84adeaf9ef 100644
--- a/firmware/init/sensor/init_oil_pressure.cpp
+++ b/firmware/init/sensor/init_oil_pressure.cpp
@@ -1,3 +1,4 @@
+#include "init.h"
 #include "adc_subscription.h"
 #include "engine.h"
 #include "error_handling.h"
@@ -26,9 +27,9 @@ void configureOilPressure(LinearFunc func, const oil_pressure_config_s& cfg)
 	func.configure(cfg.v1, val1, cfg.v2, val2, /*minOutput*/ -5, greaterOutput);
 }
 
-void initOilPressure() {
+void initOilPressure(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	// Only register if we have a sensor
-	auto channel = engineConfiguration->oilPressure.hwChannel;
+	auto channel = CONFIG(oilPressure.hwChannel);
 	if (channel == EFI_ADC_NONE) {
 		return;
 	}
@@ -44,6 +45,6 @@ void initOilPressure() {
 	}
 }
 
-void reconfigureOilPressure() {
+void reconfigureOilPressure(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	configureOilPressure(oilpSensorFunc, CONFIG(oilPressure));
 }
diff --git a/firmware/init/sensor/init_sensors.cpp b/firmware/init/sensor/init_sensors.cpp
index 3c4745a0ba..cb76859d46 100644
--- a/firmware/init/sensor/init_sensors.cpp
+++ b/firmware/init/sensor/init_sensors.cpp
@@ -2,37 +2,27 @@
  * @file init_sensorss.cpp
  */
 
-#include "global.h"
-#include "cli_registry.h"
 #include "init.h"
+#include "cli_registry.h"
 #include "sensor.h"
 
 static void initSensorCli(Logging* logger);
 
-// Sensor init/config
-void initTps();
-void initOilPressure();
-void initCanSensors();
-
-void initNewSensors(Logging* logger) {
+void initNewSensors(Logging* logger DECLARE_ENGINE_PARAMETER_SUFFIX) {
 #if EFI_CAN_SUPPORT
 	initCanSensors();
 #endif
 
-	initTps();
-	initOilPressure();
+	initTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+	initOilPressure(PASS_ENGINE_PARAMETER_SIGNATURE);
 
 	// Init CLI functionality for sensors (mocking)
 	initSensorCli(logger);
 }
 
-// Sensor reconfiguration
-void reconfigureTps();
-void reconfigureOilPressure();
-
-void reconfigureSensors() {
-	reconfigureTps();
-	reconfigureOilPressure();
+void reconfigureSensors(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	reconfigureTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+	reconfigureOilPressure(PASS_ENGINE_PARAMETER_SIGNATURE);
 }
 
 static Logging* s_logger;
diff --git a/firmware/init/sensor/init_tps.cpp b/firmware/init/sensor/init_tps.cpp
index d353c97edd..e737e4a0ff 100644
--- a/firmware/init/sensor/init_tps.cpp
+++ b/firmware/init/sensor/init_tps.cpp
@@ -25,22 +25,21 @@ FunctionalSensor pedalSensor(SensorType::AcceleratorPedal, MS2NT(10));
 // This sensor indicates the driver's throttle intent - Pedal if we have one, TPS if not.
 ProxySensor driverIntent(SensorType::DriverThrottleIntent);
 
-static void configureTps(LinearFunc& func, float closed, float open) {
+static void configureTps(LinearFunc& func, float closed, float open, float min, float max) {
 	func.configure(
 		closed, 0,
 		open, 100, 
-		CONFIG(tpsErrorDetectionTooLow),
-		CONFIG(tpsErrorDetectionTooHigh)
+		min, max
 	);
 }
 
-static void initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open) {
+static void initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open, float min, float max) {
 	// Only register if we have a sensor
 	if (channel == EFI_ADC_NONE) {
 		return;
 	}
 
-	configureTps(func, closed, open);
+	configureTps(func, closed, open, min, max);
 
 	sensor.setFunction(func);
 
@@ -51,10 +50,13 @@ static void initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_
 	}
 }
 
-void initTps() {
-	initTpsFunc(tpsFunc1p, tpsSens1p, CONFIG(tps1_1AdcChannel), CONFIG(tpsMin), CONFIG(tpsMax));
-	initTpsFunc(tpsFunc2p, tpsSens2p, CONFIG(tps2_1AdcChannel), CONFIG(tps2Min), CONFIG(tps2Max));
-	initTpsFunc(pedalFunc, pedalSensor, CONFIG(throttlePedalPositionAdcChannel), CONFIG(throttlePedalUpVoltage), CONFIG(throttlePedalWOTVoltage));
+void initTps(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	float min = CONFIG(tpsErrorDetectionTooLow);
+	float max = CONFIG(tpsErrorDetectionTooHigh);
+
+	initTpsFunc(tpsFunc1p, tpsSens1p, CONFIG(tps1_1AdcChannel), CONFIG(tpsMin), CONFIG(tpsMax), min, max);
+	initTpsFunc(tpsFunc2p, tpsSens2p, CONFIG(tps2_1AdcChannel), CONFIG(tps2Min), CONFIG(tps2Max), min, max);
+	initTpsFunc(pedalFunc, pedalSensor, CONFIG(throttlePedalPositionAdcChannel), CONFIG(throttlePedalUpVoltage), CONFIG(throttlePedalWOTVoltage), min, max);
 
 	// Route the pedal or TPS to driverIntent as appropriate
 	if (CONFIG(throttlePedalPositionAdcChannel) != EFI_ADC_NONE) {
@@ -68,8 +70,11 @@ void initTps() {
 	}
 }
 
-void reconfigureTps() {
-	configureTps(tpsFunc1p, CONFIG(tpsMin), CONFIG(tpsMax));
-	configureTps(tpsFunc2p, CONFIG(tps2Min), CONFIG(tps2Max));
-	configureTps(pedalFunc, CONFIG(throttlePedalUpVoltage), CONFIG(throttlePedalWOTVoltage));
+void reconfigureTps(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+	float min = CONFIG(tpsErrorDetectionTooLow);
+	float max = CONFIG(tpsErrorDetectionTooHigh);
+
+	configureTps(tpsFunc1p, CONFIG(tpsMin), CONFIG(tpsMax), min, max);
+	configureTps(tpsFunc2p, CONFIG(tps2Min), CONFIG(tps2Max), min, max);
+	configureTps(pedalFunc, CONFIG(throttlePedalUpVoltage), CONFIG(throttlePedalWOTVoltage), min, max);
 }
diff --git a/unit_tests/Makefile b/unit_tests/Makefile
index 54857e6cf7..7f50c59f1c 100644
--- a/unit_tests/Makefile
+++ b/unit_tests/Makefile
@@ -102,6 +102,7 @@ include $(PROJECT_DIR)/hw_layer/hw_layer.mk
 include $(PROJECT_DIR)/hw_layer/drivers/drivers.mk
 include $(PROJECT_DIR)/hw_layer/sensors/sensors.mk
 include $(PROJECT_DIR)/util/util.mk
+include $(PROJECT_DIR)/init/init.mk
 include test.mk
 include tests/tests.mk
 
@@ -138,6 +139,7 @@ CPPSRC = $(UTILSRC_CPP) \
 	$(HW_LAYER_EMS_CPP) \
 	$(HW_SENSORS_SRC) \
 	$(TRIGGER_SRC_CPP) \
+	$(INIT_SRC_CPP) \
 	$(PROJECT_DIR)/../unit_tests/main.cpp
 
 
@@ -175,6 +177,7 @@ INCDIR = . \
 	$(PROJECT_DIR)/hw_layer \
 	$(PROJECT_DIR)/hw_layer/algo \
 	$(PROJECT_DIR)/hw_layer/sensors/ \
+	$(PROJECT_DIR)/init/ \
 	$(HW_LAYER_DRIVERS_INC) \
 	test_data_structures \
 	googletest/googlemock/include \
diff --git a/unit_tests/engine_test_helper.cpp b/unit_tests/engine_test_helper.cpp
index ef768c110b..1627f8690d 100644
--- a/unit_tests/engine_test_helper.cpp
+++ b/unit_tests/engine_test_helper.cpp
@@ -79,6 +79,10 @@ EngineTestHelper::EngineTestHelper(engine_type_e engineType, configuration_callb
 EngineTestHelper::EngineTestHelper(engine_type_e engineType) : EngineTestHelper(engineType, &emptyCallbackWithConfiguration) {
 }
 
+EngineTestHelper::~EngineTestHelper() {
+	Sensor::resetRegistry();
+}
+
 /**
  * mock a change of time and fire single RISE front event
  */
diff --git a/unit_tests/engine_test_helper.h b/unit_tests/engine_test_helper.h
index 7c636e0184..54e65d81bc 100644
--- a/unit_tests/engine_test_helper.h
+++ b/unit_tests/engine_test_helper.h
@@ -28,8 +28,10 @@ public:
  */
 class EngineTestHelper : public EngineTestHelperBase {
 public:
-	EngineTestHelper(engine_type_e engineType);
+	explicit EngineTestHelper(engine_type_e engineType);
 	EngineTestHelper(engine_type_e engineType, configuration_callback_t boardCallback);
+	~EngineTestHelper();
+
 	void applyTriggerWaveform();
 	void setTriggerType(trigger_type_e trigger DECLARE_ENGINE_PARAMETER_SUFFIX);
 	void fireRise(float delayMs);
diff --git a/unit_tests/tests/sensor/test_sensor_init.cpp b/unit_tests/tests/sensor/test_sensor_init.cpp
new file mode 100644
index 0000000000..4d4fbd119c
--- /dev/null
+++ b/unit_tests/tests/sensor/test_sensor_init.cpp
@@ -0,0 +1,110 @@
+#include "unit_test_framework.h"
+#include "init.h"
+#include "sensor.h"
+#include "functional_sensor.h"
+
+#include "engine_test_helper.h"
+#include <gtest/gtest.h>
+
+static void postToFuncSensor(Sensor* s, float value) {
+	static_cast<FunctionalSensor*>(s)->postRawValue(value, getTimeNowNt());
+}
+
+#define EXPECT_POINT_VALID(s, raw, expect) \
+	{\
+		postToFuncSensor(s, raw); \
+		auto res = s->get(); \
+		EXPECT_TRUE(res.Valid); \
+		EXPECT_NEAR(res.Value, expect, EPS4D); \
+	}
+
+#define EXPECT_POINT_INVALID(s, raw) \
+	{\
+		postToFuncSensor(s, raw); \
+		auto res = s->get(); \
+		EXPECT_FALSE(res.Valid); \
+	}
+
+TEST(SensorInit, Tps) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	CONFIG(tpsMin) = 200;	// 1 volt
+	CONFIG(tpsMax) = 800;	// 4 volts
+
+	initTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// Ensure the sensors were registered
+	auto s = const_cast<Sensor*>(Sensor::getSensorOfType(SensorType::Tps1));
+	ASSERT_NE(nullptr, s);
+
+	// Test in range
+	EXPECT_POINT_VALID(s, 1.0f, 0.0f);	// closed throttle
+	EXPECT_POINT_VALID(s, 2.5f, 50.0f);	// half throttle
+	EXPECT_POINT_VALID(s, 4.0f, 100.0f) // full throttle
+
+	// Test out of range
+	EXPECT_POINT_INVALID(s, 0.0f);
+	EXPECT_POINT_INVALID(s, 5.0f);
+}
+
+TEST(SensorInit, Pedal) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	CONFIG(throttlePedalPositionAdcChannel) = EFI_ADC_0;
+	CONFIG(throttlePedalUpVoltage) = 1;
+	CONFIG(throttlePedalWOTVoltage) = 4;
+
+	initTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// Ensure the sensors were registered
+	auto s = const_cast<Sensor*>(Sensor::getSensorOfType(SensorType::AcceleratorPedal));
+	ASSERT_NE(nullptr, s);
+
+	// Test in range
+	EXPECT_POINT_VALID(s, 1.0f, 0.0f);	// closed throttle
+	EXPECT_POINT_VALID(s, 2.5f, 50.0f);	// half throttle
+	EXPECT_POINT_VALID(s, 4.0f, 100.0f) // full throttle
+
+	// Test out of range
+	EXPECT_POINT_INVALID(s, 0.0f);
+	EXPECT_POINT_INVALID(s, 5.0f);
+}
+
+TEST(SensorInit, DriverIntentNoPedal) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	// We have no pedal - so we should get the TPS
+	CONFIG(throttlePedalPositionAdcChannel) = EFI_ADC_NONE;
+
+	initTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// Ensure a sensor got set
+	ASSERT_TRUE(Sensor::hasSensor(SensorType::DriverThrottleIntent));
+
+	// Set values so we can identify which one got proxied
+	Sensor::setMockValue(SensorType::Tps1, 25);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 75);
+
+	// Should get the TPS
+	EXPECT_EQ(Sensor::get(SensorType::DriverThrottleIntent).Value, 25);
+}
+
+
+TEST(SensorInit, DriverIntentWith) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	// We have a pedal, so we should get it
+	CONFIG(throttlePedalPositionAdcChannel) = EFI_ADC_0;
+
+	initTps(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// Ensure a sensor got set
+	ASSERT_TRUE(Sensor::hasSensor(SensorType::DriverThrottleIntent));
+
+	// Set values so we can identify which one got proxied
+	Sensor::setMockValue(SensorType::Tps1, 25);
+	Sensor::setMockValue(SensorType::AcceleratorPedal, 75);
+
+	// Should get the pedal
+	EXPECT_EQ(Sensor::get(SensorType::DriverThrottleIntent).Value, 75);
+}
diff --git a/unit_tests/tests/tests.mk b/unit_tests/tests/tests.mk
index 4a7c8a081c..4d0ab0ed98 100644
--- a/unit_tests/tests/tests.mk
+++ b/unit_tests/tests/tests.mk
@@ -45,4 +45,6 @@ TESTS_SRC_CPP = \
 	tests/sensor/resist_func.cpp \
 	tests/sensor/therm_func.cpp \
 	tests/sensor/func_chain.cpp \
-	tests/sensor/redundant.cpp
+	tests/sensor/redundant.cpp \
+	tests/sensor/test_sensor_init.cpp \
+