diff --git a/firmware/controllers/sensors/sensor_type.h b/firmware/controllers/sensors/sensor_type.h
index 29cadbaba9..8b44077e89 100644
--- a/firmware/controllers/sensors/sensor_type.h
+++ b/firmware/controllers/sensors/sensor_type.h
@@ -28,6 +28,10 @@ enum class SensorType : unsigned char {
 	// This is the second sensor
 	Tps1Secondary,
 
+	Tps2,
+	Tps2Primary,
+	Tps2Secondary,
+
 	// Leave me at the end!
 	PlaceholderLast
 };
diff --git a/firmware/controllers/sensors/tps.cpp b/firmware/controllers/sensors/tps.cpp
index 6db135f675..73b20e0ce1 100644
--- a/firmware/controllers/sensors/tps.cpp
+++ b/firmware/controllers/sensors/tps.cpp
@@ -11,9 +11,6 @@
 
 EXTERN_ENGINE;
 
-// Scaled to 1000 counts = 5.0 volts
-#define TPS_TS_CONVERSION 200
-
 /**
  * set mock_pedal_position X
  * See also directPwmValue
diff --git a/firmware/controllers/sensors/tps.h b/firmware/controllers/sensors/tps.h
index a88fc6b737..a0a708bbf9 100644
--- a/firmware/controllers/sensors/tps.h
+++ b/firmware/controllers/sensors/tps.h
@@ -12,6 +12,9 @@
 #include "global.h"
 #include "engine_configuration.h"
 
+// Scaled to 1000 counts = 5.0 volts
+#define TPS_TS_CONVERSION 200
+
 bool hasPedalPositionSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 percent_t getPedalPosition(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 /**
diff --git a/firmware/init/init.mk b/firmware/init/init.mk
index 84f9d96b8f..84b74474d7 100644
--- a/firmware/init/init.mk
+++ b/firmware/init/init.mk
@@ -1,3 +1,4 @@
 
 INIT_SRC_CPP =	$(PROJECT_DIR)/init/sensor/init_sensors.cpp \
-				$(PROJECT_DIR)/init/sensor/init_oil_pressure.cpp
+				$(PROJECT_DIR)/init/sensor/init_oil_pressure.cpp \
+				$(PROJECT_DIR)/init/sensor/init_tps.cpp
diff --git a/firmware/init/sensor/init_sensors.cpp b/firmware/init/sensor/init_sensors.cpp
index 06e5f7d80d..6a7e975e96 100644
--- a/firmware/init/sensor/init_sensors.cpp
+++ b/firmware/init/sensor/init_sensors.cpp
@@ -6,11 +6,15 @@
 #include "init.h"
 #include "sensor.h"
 
+void initTps();
 void initOilPressure();
 
 void initSensorCli();
 
 void initSensors() {
+	// TPS
+	initTps();
+
 	// aux sensors
 	initOilPressure();
 
diff --git a/firmware/init/sensor/init_tps.cpp b/firmware/init/sensor/init_tps.cpp
new file mode 100644
index 0000000000..223555f405
--- /dev/null
+++ b/firmware/init/sensor/init_tps.cpp
@@ -0,0 +1,46 @@
+#include "adc_subscription.h"
+#include "engine.h"
+#include "error_handling.h"
+#include "global.h"
+#include "functional_sensor.h"
+#include "linear_func.h"
+#include "tps.h"
+
+EXTERN_ENGINE;
+
+LinearFunc tpsFunc1p;
+//LinearFunc tpsFunc1s;
+LinearFunc tpsFunc2p;
+//LinearFunc tpsFunc2s;
+
+FunctionalSensor tpsSens1p(SensorType::Tps1, MS2NT(10));
+//FunctionalSensor tpsSens1s(SensorType::Tps1Secondary, MS2NT(10));
+FunctionalSensor tpsSens2p(SensorType::Tps2, MS2NT(10));
+//FunctionalSensor tpsSens2s(SensorType::Tps2Secondary, MS2NT(10));
+
+static void initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open) {
+	// Only register if we have a sensor
+	if (channel == EFI_ADC_NONE) {
+		return;
+	}
+
+	func.configure(
+		closed / TPS_TS_CONVERSION, 0,
+		open / TPS_TS_CONVERSION, 100, 
+		CONFIG(tpsErrorDetectionTooLow),
+		CONFIG(tpsErrorDetectionTooHigh)
+	);
+
+	sensor.setFunction(func);
+
+	AdcSubscription::SubscribeSensor(sensor, channel);
+
+	if (!sensor.Register()) {
+		firmwareError(CUSTOM_INVALID_TPS_SETTING, "Duplicate TPS registration for TPS sensor");
+	}
+}
+
+void initTps() {
+	initTpsFunc(tpsFunc1p, tpsSens1p, CONFIG(tps1_1AdcChannel), CONFIG(tpsMin), CONFIG(tpsMax));
+	initTpsFunc(tpsFunc2p, tpsSens2p, CONFIG(tps2_1AdcChannel), CONFIG(tps2Min), CONFIG(tps2Max));
+}