diff --git a/firmware/config/boards/proteus/mapping.yaml b/firmware/config/boards/proteus/mapping.yaml
index 144d906d63..d639b27fce 100644
--- a/firmware/config/boards/proteus/mapping.yaml
+++ b/firmware/config/boards/proteus/mapping.yaml
@@ -96,7 +96,7 @@ analog_inputs:
   EFI_ADC_8: "Analog Temp 3"
   # PB1, pin #31
   EFI_ADC_9: "Analog Temp 4"
-  # MAP = Analog volt 1 = PC0
+  # MAP = Analog volt 1 = PC0, pin #13
   EFI_ADC_10: "Analog Volt 1"
   # TPS = Analog volt 2 = PC1
   EFI_ADC_11: "Analog Volt 2"
diff --git a/firmware/config/engines/mazda_miata.cpp b/firmware/config/engines/mazda_miata.cpp
index 997d128b06..8f7784d9c1 100644
--- a/firmware/config/engines/mazda_miata.cpp
+++ b/firmware/config/engines/mazda_miata.cpp
@@ -176,77 +176,6 @@ void setMiata1990(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 // todo: idleValvePin
 }
 
-
-static void setMiata1994_common(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
-	commonMiataNa(PASS_CONFIG_PARAMETER_SIGNATURE);
-	engineConfiguration->specs.displacement = 1.839;
-
-	// set cranking_timing_angle 0
-	engineConfiguration->crankingTimingAngle = 0;
-
-	engineConfiguration->crankingChargeAngle = 70;
-
-#if IGN_LOAD_COUNT == DEFAULT_IGN_LOAD_COUNT
-	MEMCPY(config->ignitionTable, miataNA8_maf_advance_table);
-#endif
-
-//	engineConfiguration->triggerSimulatorPins[0] = GPIOD_2; // 2G - YEL/BLU
-//	engineConfiguration->triggerSimulatorPins[1] = GPIOB_3; // 2E - WHT - four times
-//	engineConfiguration->triggerSimulatorPinModes[0] = OM_OPENDRAIN;
-//	engineConfiguration->triggerSimulatorPinModes[1] = OM_OPENDRAIN;
-//
-//	engineConfiguration->triggerInputPins[0] = GPIO_UNASSIGNED;
-//	engineConfiguration->triggerInputPins[1] = GPIO_UNASSIGNED;
-//
-//	engineConfiguration->is_enabled_spi_1 = false;
-//	engineConfiguration->is_enabled_spi_2 = false;
-//	engineConfiguration->is_enabled_spi_3 = false;
-
-	/**
-	 * Outputs
-	 */
-	// Frankenso low out #: PE6
-	// Frankenso low out #: PE5
-	// Frankenso low out #:
-	// Frankenso low out #:
-	// Frankenso low out #5: PE3
-	// Frankenso low out #6: PE4
-	// Frankenso low out #7: PE1 (do not use with discovery!)
-	// Frankenso low out #8:
-	// Frankenso low out #9: PB9
-	// Frankenso low out #10: PE0 (do not use with discovery!)
-	// Frankenso low out #11: PB8
-	// Frankenso low out #12: PB7
-	engineConfiguration->fanPin = GPIOE_6;
-
-	engineConfiguration->o2heaterPin = GPIO_UNASSIGNED;
-
-	engineConfiguration->fuelPumpPin = GPIOE_4;
-
-	engineConfiguration->injectionPins[4] = GPIO_UNASSIGNED;
-	engineConfiguration->injectionPins[5] = GPIO_UNASSIGNED;
-	engineConfiguration->injectionPinMode = OM_DEFAULT;
-
-	engineConfiguration->idle.solenoidPin = GPIOB_9;
-
-	engineConfiguration->ignitionPins[0] = GPIOE_14; // Frankenso high side - pin 1G
-	engineConfiguration->ignitionPins[1] = GPIO_UNASSIGNED;
-	engineConfiguration->ignitionPins[2] = GPIOC_7; // Frankenso high side - pin 1H
-	engineConfiguration->ignitionPins[3] = GPIO_UNASSIGNED;
-	engineConfiguration->ignitionPinMode = OM_DEFAULT;
-
-	setFrankenso_01_LCD(engineConfiguration);
-
-	commonFrankensoAnalogInputs(engineConfiguration);
-
-	engineConfiguration->tps1_1AdcChannel = EFI_ADC_2; // PA2
-	engineConfiguration->map.sensor.hwChannel = EFI_ADC_4;
-	engineConfiguration->mafAdcChannel = EFI_ADC_0;
-	engineConfiguration->clt.adcChannel = EFI_ADC_12;
-	engineConfiguration->iat.adcChannel = EFI_ADC_11;
-// end of 1994 commond
-}
-
 /**
  * Tom tomiata, Frankenstein board
  */
diff --git a/firmware/config/engines/mazda_miata_vvt.cpp b/firmware/config/engines/mazda_miata_vvt.cpp
index 9177d02253..3faa2de23b 100644
--- a/firmware/config/engines/mazda_miata_vvt.cpp
+++ b/firmware/config/engines/mazda_miata_vvt.cpp
@@ -793,7 +793,7 @@ void setMiataNB2_ProteusEngineConfiguration(DECLARE_CONFIG_PARAMETER_SIGNATURE)
     // 13.8
 	engineConfiguration->knockBandCustom = 2 * BAND(engineConfiguration->cylinderBore);
 
-    engineConfiguration->malfunctionIndicatorPin = GPIOB_6;
+    engineConfiguration->malfunctionIndicatorPin = GPIOB_6; // "Lowside 10"    # pin 20/black35
 
     engineConfiguration->map.sensor.hwChannel = EFI_ADC_10;
 
diff --git a/firmware/console/eficonsole.cpp b/firmware/console/eficonsole.cpp
index 563df05e9a..ebb4505f12 100644
--- a/firmware/console/eficonsole.cpp
+++ b/firmware/console/eficonsole.cpp
@@ -119,12 +119,13 @@ static void sayHello(void) {
 }
 
 void validateStack(const char*msg, obd_code_e code, int desiredStackUnusedSize) {
-
 #if CH_DBG_THREADS_PROFILING && CH_DBG_FILL_THREADS
 	int unusedStack = CountFreeStackSpace(chThdGetSelfX()->wabase);
 	if (unusedStack < desiredStackUnusedSize) {
 		warning(code, "Stack low on %s: %d", msg, unusedStack);
 	}
+#else
+	(void)msg; (void)code; (void)desiredStackUnusedSize;
 #endif
 }
 
diff --git a/firmware/console/status_loop.cpp b/firmware/console/status_loop.cpp
index 0ec10fd335..ef91b33dff 100644
--- a/firmware/console/status_loop.cpp
+++ b/firmware/console/status_loop.cpp
@@ -157,6 +157,8 @@ void writeLogLine(Writer& buffer) {
 	}
 
 	binaryLogCount++;
+#else
+	(void)buffer;
 #endif /* EFI_FILE_LOGGING */
 }
 
diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp
index baa89dda88..7f0fc4cdb6 100644
--- a/firmware/controllers/actuators/idle_thread.cpp
+++ b/firmware/controllers/actuators/idle_thread.cpp
@@ -565,6 +565,10 @@ float getIdleTimingAdjustment(int rpm) {
 	return idleControllerInstance.getIdleTimingAdjustment(rpm);
 }
 
+bool isIdling() {
+	return idleControllerInstance.isIdling();
+}
+
 static void applyPidSettings(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->updateFactors(engineConfiguration->idleRpmPid.pFactor, engineConfiguration->idleRpmPid.iFactor, engineConfiguration->idleRpmPid.dFactor);
 	iacPidMultMap.init(CONFIG(iacPidMultTable), CONFIG(iacPidMultLoadBins), CONFIG(iacPidMultRpmBins));
diff --git a/firmware/controllers/actuators/idle_thread.h b/firmware/controllers/actuators/idle_thread.h
index c3a0ec5f9c..a2f797cd9b 100644
--- a/firmware/controllers/actuators/idle_thread.h
+++ b/firmware/controllers/actuators/idle_thread.h
@@ -53,6 +53,11 @@ public:
 	float getIdleTimingAdjustment(int rpm);
 	float getIdleTimingAdjustment(int rpm, int targetRpm, Phase phase);
 
+	// Allow querying state from outside
+	bool isIdling() {
+		return m_lastPhase == Phase::Idling;
+	}
+
 private:
 	// These are stored by getIdlePosition() and used by getIdleTimingAdjustment()
 	Phase m_lastPhase = Phase::Cranking;
@@ -66,6 +71,8 @@ percent_t getIdlePosition();
 
 float getIdleTimingAdjustment(int rpm);
 
+bool isIdling();
+
 void applyIACposition(percent_t position DECLARE_ENGINE_PARAMETER_SUFFIX);
 void setManualIdleValvePosition(int positionPercent);
 
diff --git a/firmware/controllers/algo/advance_map.cpp b/firmware/controllers/algo/advance_map.cpp
index 9d3c474e3d..5233094003 100644
--- a/firmware/controllers/algo/advance_map.cpp
+++ b/firmware/controllers/algo/advance_map.cpp
@@ -82,7 +82,7 @@ static angle_t getRunningAdvance(int rpm, float engineLoad DECLARE_ENGINE_PARAME
 	float advanceAngle = advanceMap.getValue((float) rpm, engineLoad);
 
 	// get advance from the separate table for Idle
-	if (CONFIG(useSeparateAdvanceForIdle)) {
+	if (CONFIG(useSeparateAdvanceForIdle) && isIdling()) {
 		float idleAdvance = interpolate2d("idleAdvance", rpm, config->idleAdvanceBins, config->idleAdvance);
 
 		auto [valid, tps] = Sensor::get(SensorType::DriverThrottleIntent);
@@ -92,7 +92,6 @@ static angle_t getRunningAdvance(int rpm, float engineLoad DECLARE_ENGINE_PARAME
 		}
 	}
 
-	
 #if EFI_LAUNCH_CONTROL
 	if (engine->isLaunchCondition && CONFIG(enableLaunchRetard)) {
         if (CONFIG(launchSmoothRetard)) {
diff --git a/firmware/controllers/algo/airmass/airmass.cpp b/firmware/controllers/algo/airmass/airmass.cpp
index a7116ee2db..f4d6940699 100644
--- a/firmware/controllers/algo/airmass/airmass.cpp
+++ b/firmware/controllers/algo/airmass/airmass.cpp
@@ -1,5 +1,6 @@
 #include "airmass.h"
 #include "sensor.h"
+#include "idle_thread.h"
 
 EXTERN_ENGINE;
 
@@ -23,8 +24,8 @@ float AirmassModelBase::getVe(int rpm, float load) const {
 	float ve = m_veTable->getValue(rpm, load);
 
 	auto tps = Sensor::get(SensorType::Tps1);
-	// get VE from the separate table for Idle
-	if (tps.Valid && CONFIG(useSeparateVeForIdle)) {
+	// get VE from the separate table for Idle if idling
+	if (isIdling() && tps && CONFIG(useSeparateVeForIdle)) {
 		float idleVe = interpolate2d("idleVe", rpm, config->idleVeBins, config->idleVe);
 		// interpolate between idle table and normal (running) table using TPS threshold
 		ve = interpolateClamped(0.0f, idleVe, CONFIG(idlePidDeactivationTpsThreshold), ve, tps.Value);
diff --git a/firmware/controllers/algo/event_registry.h b/firmware/controllers/algo/event_registry.h
index 2d7c932b17..bf417ebbb2 100644
--- a/firmware/controllers/algo/event_registry.h
+++ b/firmware/controllers/algo/event_registry.h
@@ -58,6 +58,9 @@ class FuelSchedule {
 public:
 	FuelSchedule();
 
+	// Call this function if something happens that requires a rebuild, like a change to the trigger pattern
+	void invalidate();
+
 	// Call this every trigger tooth.  It will schedule all required injector events.
 	void onTriggerTooth(size_t toothIndex, int rpm, efitick_t nowNt DECLARE_ENGINE_PARAMETER_SUFFIX);
 
@@ -73,10 +76,7 @@ public:
 	 * injection events, per cylinder
 	 */
 	InjectionEvent elements[MAX_INJECTION_OUTPUT_COUNT];
-	bool isReady;
-
-private:
-	void clear();
+	bool isReady = false;
 };
 
 class AngleBasedEvent {
diff --git a/firmware/controllers/engine_cycle/fuel_schedule.cpp b/firmware/controllers/engine_cycle/fuel_schedule.cpp
index 335f356d91..79a7e62084 100644
--- a/firmware/controllers/engine_cycle/fuel_schedule.cpp
+++ b/firmware/controllers/engine_cycle/fuel_schedule.cpp
@@ -14,14 +14,13 @@ EXTERN_ENGINE;
 #if EFI_ENGINE_CONTROL
 
 FuelSchedule::FuelSchedule() {
-	clear();
 	for (int cylinderIndex = 0; cylinderIndex < MAX_INJECTION_OUTPUT_COUNT; cylinderIndex++) {
 		InjectionEvent *ev = &elements[cylinderIndex];
 		ev->ownIndex = cylinderIndex;
 	}
 }
 
-void FuelSchedule::clear() {
+void FuelSchedule::invalidate() {
 	isReady = false;
 }
 
@@ -142,19 +141,26 @@ bool FuelSchedule::addFuelEventsForCylinder(int i  DECLARE_ENGINE_PARAMETER_SUFF
 }
 
 void FuelSchedule::addFuelEvents(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
-	clear();
-
 	for (int cylinderIndex = 0; cylinderIndex < CONFIG(specs.cylindersCount); cylinderIndex++) {
 		InjectionEvent *ev = &elements[cylinderIndex];
 		ev->ownIndex = cylinderIndex;  // todo: is this assignment needed here? we now initialize in constructor
 		bool result = addFuelEventsForCylinder(cylinderIndex PASS_ENGINE_PARAMETER_SUFFIX);
-		if (!result)
+		if (!result) {
+			invalidate();
 			return;
+		}
 	}
+
+	// We made it through all cylinders, mark the schedule as ready so it can be used
 	isReady = true;
 }
 
 void FuelSchedule::onTriggerTooth(size_t toothIndex, int rpm, efitick_t nowNt DECLARE_ENGINE_PARAMETER_SUFFIX) {
+	// Wait for schedule to be built - this happens the first time we get RPM
+	if (!isReady) {
+		return;
+	}
+
 	for (int i = 0; i < CONFIG(specs.cylindersCount); i++) {
 		elements[i].onTriggerTooth(toothIndex, rpm, nowNt);
 	}
diff --git a/firmware/controllers/math/engine_math.cpp b/firmware/controllers/math/engine_math.cpp
index 7be0ef4740..c08cf73c22 100644
--- a/firmware/controllers/math/engine_math.cpp
+++ b/firmware/controllers/math/engine_math.cpp
@@ -360,6 +360,7 @@ static int getIgnitionPinForIndex(int cylinderIndex DECLARE_ENGINE_PARAMETER_SUF
 }
 
 void prepareIgnitionPinIndices(ignition_mode_e ignitionMode DECLARE_ENGINE_PARAMETER_SUFFIX) {
+	(void)ignitionMode;
 #if EFI_ENGINE_CONTROL
 	for (int cylinderIndex = 0; cylinderIndex < CONFIG(specs.cylindersCount); cylinderIndex++) {
 		ENGINE(ignitionPin[cylinderIndex]) = getIgnitionPinForIndex(cylinderIndex PASS_ENGINE_PARAMETER_SUFFIX);
@@ -412,6 +413,9 @@ void prepareOutputSignals(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	prepareIgnitionPinIndices(CONFIG(ignitionMode) PASS_ENGINE_PARAMETER_SUFFIX);
 
 	TRIGGER_WAVEFORM(prepareShape(&ENGINE(triggerCentral.triggerFormDetails) PASS_ENGINE_PARAMETER_SUFFIX));
+
+	// Fuel schedule may now be completely wrong, force a reset
+	ENGINE(injectionEvents).invalidate();
 }
 
 void setTimingRpmBin(float from, float to DECLARE_CONFIG_PARAMETER_SUFFIX) {
diff --git a/firmware/controllers/sensors/software_knock.cpp b/firmware/controllers/sensors/software_knock.cpp
index 10d444c393..8ab4558a66 100644
--- a/firmware/controllers/sensors/software_knock.cpp
+++ b/firmware/controllers/sensors/software_knock.cpp
@@ -118,6 +118,8 @@ const ADCConversionGroup* getConversionGroup(uint8_t cylinderIndex) {
 	if (cylinderUsesChannel2(cylinderIndex)) {
 		return &adcConvGroupCh2;
 	}
+#else
+	(void)cylinderIndex;
 #endif // KNOCK_HAS_CH2
 
 	return &adcConvGroupCh1;
diff --git a/firmware/controllers/trigger/decoders/trigger_structure.cpp b/firmware/controllers/trigger/decoders/trigger_structure.cpp
index 35f7feefe3..1e78814811 100644
--- a/firmware/controllers/trigger/decoders/trigger_structure.cpp
+++ b/firmware/controllers/trigger/decoders/trigger_structure.cpp
@@ -418,8 +418,13 @@ void findTriggerPosition(TriggerWaveform *triggerShape,
 		return;
 	}
 
-	position->triggerEventIndex = triggerEventIndex;
-	position->angleOffsetFromTriggerEvent = angle - triggerEventAngle;
+	{
+		// This must happen under lock so that the tooth and offset don't get partially read and mismatched
+		chibios_rt::CriticalSectionLocker csl;
+
+		position->triggerEventIndex = triggerEventIndex;
+		position->angleOffsetFromTriggerEvent = angle - triggerEventAngle;
+	}
 }
 
 void TriggerWaveform::prepareShape(TriggerFormDetails *details DECLARE_ENGINE_PARAMETER_SUFFIX) {
diff --git a/firmware/hw_layer/mmc_card_access.cpp b/firmware/hw_layer/mmc_card_access.cpp
index 84e86c631b..12cb878378 100644
--- a/firmware/hw_layer/mmc_card_access.cpp
+++ b/firmware/hw_layer/mmc_card_access.cpp
@@ -359,7 +359,8 @@ void handleTsW(ts_channel_s *tsChannel, char *input) {
 					if (isLogFile(fileName)) {
 						int dotIndex = indexOf(fileName, DOT);
 						if (0 == strncmp(input + 6, &fileName[dotIndex - 4], 4)) {
-							FRESULT err = f_open(&uploading, fileName, FA_READ);// This file has the index for next log file name
+							/* FRESULT err = */
+							f_open(&uploading, fileName, FA_READ);// This file has the index for next log file name
 							break;
 						}
 					}
diff --git a/firmware/hw_layer/ports/stm32/stm32_pins.cpp b/firmware/hw_layer/ports/stm32/stm32_pins.cpp
index bdc1d0663a..baa5d1cc72 100644
--- a/firmware/hw_layer/ports/stm32/stm32_pins.cpp
+++ b/firmware/hw_layer/ports/stm32/stm32_pins.cpp
@@ -121,6 +121,8 @@ int getPortPinIndex(ioportid_t port, ioportmask_t pin) {
 }
 
 ioportid_t getHwPort(const char *msg, brain_pin_e brainPin) {
+	(void)msg;
+
 	if (!isBrainPinValid(brainPin)) {
 /*
  *  https://github.com/dron0gus please help
diff --git a/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash.c b/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash.c
index bd2b159f58..f13368436b 100644
--- a/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash.c
+++ b/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash.c
@@ -406,6 +406,7 @@ void HAL_FLASH_IRQHandler(void)
   */
 __weak void HAL_FLASH_EndOfOperationCallback(uint32_t ReturnValue)
 {
+  (void)ReturnValue;
   /* NOTE : This function Should not be modified, when the callback is needed,
             the HAL_FLASH_EndOfOperationCallback could be implemented in the user file
    */ 
@@ -421,6 +422,7 @@ __weak void HAL_FLASH_EndOfOperationCallback(uint32_t ReturnValue)
   */
 __weak void HAL_FLASH_OperationErrorCallback(uint32_t ReturnValue)
 {
+  (void)ReturnValue;
   /* NOTE : This function Should not be modified, when the callback is needed,
             the HAL_FLASH_OperationErrorCallback could be implemented in the user file
    */ 
@@ -565,9 +567,9 @@ uint32_t HAL_FLASH_GetError(void)
   * @retval HAL Status
   */
 HAL_StatusTypeDef FLASH_WaitForLastOperation(uint32_t Timeout)
-{ 
+{
+  (void)Timeout;
 
-  
   /* Clear Error Code */
   pFlash.ErrorCode = HAL_FLASH_ERROR_NONE;
   
diff --git a/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash_ex.c b/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash_ex.c
index 9a53d9324d..75cb66857f 100644
--- a/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash_ex.c
+++ b/firmware/hw_layer/ports/stm32/stm32f4/stm32f4xx_hal_flash_ex.c
@@ -1001,6 +1001,9 @@ static HAL_StatusTypeDef FLASH_OB_DisablePCROP(uint32_t SectorBank1, uint32_t Se
   */
 static void FLASH_MassErase(uint8_t VoltageRange, uint32_t Banks)
 {
+  (void)VoltageRange;
+  (void)Banks;
+
   uint32_t tmp_psize = 0;
   
   /* Check the parameters */
@@ -1083,6 +1086,8 @@ void FLASH_Erase_Sector(uint32_t Sector, uint8_t VoltageRange)
   */
 static HAL_StatusTypeDef FLASH_OB_EnableWRP(uint32_t WRPSector, uint32_t Banks)
 {
+  (void)Banks;
+
   HAL_StatusTypeDef status = HAL_OK;
   
   /* Check the parameters */
@@ -1119,6 +1124,8 @@ static HAL_StatusTypeDef FLASH_OB_EnableWRP(uint32_t WRPSector, uint32_t Banks)
   */
 static HAL_StatusTypeDef FLASH_OB_DisableWRP(uint32_t WRPSector, uint32_t Banks)
 {
+  (void)Banks;
+
   HAL_StatusTypeDef status = HAL_OK;
   
   /* Check the parameters */
diff --git a/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_hal_flash.c b/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_hal_flash.c
index 06fc28f13b..331ed2c896 100644
--- a/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_hal_flash.c
+++ b/firmware/hw_layer/ports/stm32/stm32f7/stm32f7xx_hal_flash.c
@@ -454,7 +454,7 @@ void HAL_FLASH_IRQHandler(void)
 __weak void HAL_FLASH_EndOfOperationCallback(uint32_t ReturnValue)
 {
   /* Prevent unused argument(s) compilation warning */
-  //UNUSED(ReturnValue);
+  (void)ReturnValue;
   /* NOTE : This function Should not be modified, when the callback is needed,
   the HAL_FLASH_EndOfOperationCallback could be implemented in the user file
   */ 
@@ -472,7 +472,7 @@ __weak void HAL_FLASH_EndOfOperationCallback(uint32_t ReturnValue)
 __weak void HAL_FLASH_OperationErrorCallback(uint32_t ReturnValue)
 {
   /* Prevent unused argument(s) compilation warning */
-  //UNUSED(ReturnValue);
+  (void)ReturnValue;
   /* NOTE : This function Should not be modified, when the callback is needed,
   the HAL_FLASH_OperationErrorCallback could be implemented in the user file
    */ 
@@ -616,8 +616,9 @@ uint32_t HAL_FLASH_GetError(void)
   * @retval HAL Status
   */
 HAL_StatusTypeDef FLASH_WaitForLastOperation(uint32_t Timeout)
-{ 
-  uint32_t tickstart = 0;
+{
+  (void)Timeout;
+  //uint32_t tickstart = 0;
   
   /* Clear Error Code */
   pFlash.ErrorCode = HAL_FLASH_ERROR_NONE;
diff --git a/firmware/util/math/interpolation.cpp b/firmware/util/math/interpolation.cpp
index cb0f95fc48..6f08144fb1 100644
--- a/firmware/util/math/interpolation.cpp
+++ b/firmware/util/math/interpolation.cpp
@@ -191,30 +191,6 @@ int findIndex(const float array[], int size, float value) {
 	return findIndexMsg("", array, size, value);
 }
 
-namespace priv
-{
-/**
- * @brief	One-dimensional table lookup with linear interpolation
- *
- * @see setLinearCurve()
- */
-float interpolate2d(const char *msg, float value, const float bin[], const float values[], int size) {
-	if (isnan(value)) {
-	    // this unfortunately sometimes happens during functional tests on real hardware
-		warning(CUSTOM_INTERPOLATE_NAN, "NaN in interpolate2d %s", msg);
-		return NAN;
-	}
-	int index = findIndexMsg(msg, bin, size, value);
-
-	if (index == -1)
-		return values[0];
-	if (index == size - 1)
-		return values[size - 1];
-
-	return interpolateMsg(msg, bin[index], values[index], bin[index + 1], values[index + 1], value);
-}
-}
-
 /**
  * Sets specified value for specified key in a correction curve
  * see also setLinearCurve()
diff --git a/firmware/util/math/interpolation.h b/firmware/util/math/interpolation.h
index 8e1267d9e0..b94ddbc756 100644
--- a/firmware/util/math/interpolation.h
+++ b/firmware/util/math/interpolation.h
@@ -13,6 +13,8 @@
 #include "obd_error_codes.h"
 #include "error_handling.h"
 
+#include <type_traits>
+
 #ifndef DEBUG_INTERPOLATION
 #define DEBUG_INTERPOLATION FALSE
 #endif
@@ -27,12 +29,87 @@ float interpolateClamped(float x1, float y1, float x2, float y2, float x);
 float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x);
 
 namespace priv {
-float interpolate2d(const char *msg, float value, const float bin[], const float values[], int size);
+struct BinResult
+{
+	size_t Idx;
+	float Frac;
+};
+
+/**
+ * @brief Finds the location of a value in the bin array.
+ * 
+ * @param value The value to find in the bins.
+ * @return A result containing the index to the left of the value,
+ * and how far from (idx) to (idx + 1) the value is located.
+ */
+template<class TBin, int TSize>
+BinResult getBin(float value, const TBin (&bins)[TSize]) {
+	// Enforce numeric only (int, float, uintx_t, etc)
+	static_assert(std::is_arithmetic_v<TBin>, "Table bins must be an arithmetic type");
+
+	// Enforce that there are enough bins to make sense (what does one bin even mean?)
+	static_assert(TSize >= 2);
+
+	// Handle NaN
+	if (cisnan(value)) {
+		return { 0, 0.0f };
+	}
+
+	// Handle off-scale low
+	if (value <= bins[0]) {
+		return { 0, 0.0f };
+	}
+
+	// Handle off-scale high
+	if (value >= bins[TSize - 1]) {
+		return { TSize - 2, 1.0f };
+	}
+
+	size_t idx = 0;
+
+	// Find the last index less than the searched value
+	// Linear search for now, maybe binary search in future
+	// after collecting real perf data
+	for (idx = 0; idx < TSize - 1; idx++) {
+		if (bins[idx + 1] > value) {
+			break;
+		}
+	}
+
+	float low = bins[idx];
+	float high = bins[idx + 1];
+
+	// Compute how far along the bin we are
+	// (0.0f = left side, 1.0f = right side)
+	float fraction = (value - low) / (high - low);
+	
+	return { idx, fraction };
 }
 
-template <int TSize>
-float interpolate2d(const char *msg, const float value, const float (&bin)[TSize], const float (&values)[TSize]) {
-	return priv::interpolate2d(msg, value, bin, values, TSize);
+static float linterp(float low, float high, float frac)
+{
+	return high * frac + low * (1 - frac);
+}
+} // namespace priv
+
+template <class TBin, class TValue, int TSize>
+float interpolate2d(const char *msg, const float value, const TBin (&bin)[TSize], const TValue (&values)[TSize]) {
+	// Enforce numeric only (int, float, uintx_t, etc)
+	static_assert(std::is_arithmetic_v<TBin>, "Table values must be an arithmetic type");
+
+	auto b = priv::getBin(value, bin);
+
+	// Convert to float as we read it out
+	float low = static_cast<float>(values[b.Idx]);
+	float high = static_cast<float>(values[b.Idx + 1]);
+	float frac = b.Frac;
+
+	return priv::linterp(low, high, frac);
+}
+
+template <class TBin, class TValue, int TSize>
+float interpolate2d(const float value, const TBin (&bin)[TSize], const TValue (&values)[TSize]) {
+	return interpolate2d("", value, bin, values);
 }
 
 int needInterpolationLogging(void);
diff --git a/unit_tests/test_basic_math/test_find_index.cpp b/unit_tests/test_basic_math/test_find_index.cpp
index 6101ee9e8d..25d543008e 100644
--- a/unit_tests/test_basic_math/test_find_index.cpp
+++ b/unit_tests/test_basic_math/test_find_index.cpp
@@ -110,3 +110,171 @@ TEST(misc, testSetTableValue) {
 	ASSERT_FLOAT_EQ(1.4, config.cltFuelCorr[0]);
 
 }
+
+class TestTable2dSmall : public ::testing::Test
+{
+protected:
+	float bins[2];
+	float values[2];
+
+	void SetUp() override
+	{
+		// This test maps [20,30] -> [100,200]
+		copyArray(bins, { 20.0f, 30.0f });
+		copyArray(values, { 100.0f, 200.0f });
+	}
+};
+
+TEST_F(TestTable2dSmall, OffScaleLow)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(10, bins, values), 100);
+}
+
+TEST_F(TestTable2dSmall, OffScaleHigh)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(40, bins, values), 200);
+}
+
+TEST_F(TestTable2dSmall, EdgeLeft)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(20, bins, values), 100);
+}
+
+TEST_F(TestTable2dSmall, EdgeRight)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(30, bins, values), 200);
+}
+
+TEST_F(TestTable2dSmall, Middle)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(25, bins, values), 150);
+}
+
+TEST_F(TestTable2dSmall, NanInput)
+{
+    EXPECT_FLOAT_EQ(interpolate2d(NAN, bins, values), 100);
+}
+
+class Test2dTableMassive : public ::testing::Test
+{
+	static constexpr int Count = 2500;
+
+protected:
+	float bins[Count];
+	float values[Count];
+
+	void SetUp() override
+	{
+		float x = 0;
+
+		for (size_t i = 0; i < std::size(bins); i++)
+		{
+			x += 0.1f;
+			bins[i] = x;
+			values[i] = x * x;
+		}
+	}
+};
+
+TEST_F(Test2dTableMassive, t)
+{
+    float x = 0;
+    float maxErr = -1;
+
+    for (size_t i = 0; i < 25000; i++)
+    {
+        x += 0.01f;
+
+        float actual = x * x;
+        float lookup = interpolate2d(x, bins, values);
+
+        float err = std::abs(actual - lookup);
+
+        if (err > maxErr)
+        {
+            maxErr = err;
+        }
+    }
+
+    EXPECT_LT(maxErr, 0.01);
+}
+
+// Helper for BinResult type
+#define EXPECT_BINRESULT(actual, expectedIdx, expectedFrac) \
+	{ \
+		auto ___temp___ = actual; \
+		EXPECT_EQ(___temp___.Idx, expectedIdx); \
+		EXPECT_NEAR(___temp___.Frac, expectedFrac, expectedFrac / 1e4); \
+	}
+
+// Test with small bins: only two values
+static const float smallBins[] = { 10, 20 };
+
+TEST(TableBinsSmall, OffScaleLeft)
+{
+    EXPECT_BINRESULT(priv::getBin(5, smallBins), 0, 0);
+}
+
+TEST(TableBinsSmall, OffScaleRight)
+{
+    EXPECT_BINRESULT(priv::getBin(25, smallBins), 0, 1);
+}
+
+TEST(TableBinsSmall, EdgeLeft)
+{
+    EXPECT_BINRESULT(priv::getBin(10, smallBins), 0, 0);
+}
+
+TEST(TableBinsSmall, EdgeRight)
+{
+    EXPECT_BINRESULT(priv::getBin(10, smallBins), 0, 0);
+}
+
+TEST(TableBinsSmall, Middle)
+{
+    EXPECT_BINRESULT(priv::getBin(15, smallBins), 0, 0.5f);
+}
+
+TEST(TableBinsSmall, NanInput)
+{
+	EXPECT_BINRESULT(priv::getBin(NAN, smallBins), 0, 0);
+}
+
+// Test with medium bins, 3 items
+static const float bigBins[] = { 10, 20, 30 };
+
+TEST(TableBinsBig, OffScaleLow)
+{
+    EXPECT_BINRESULT(priv::getBin(5, bigBins), 0, 0);
+}
+
+TEST(TableBinsBig, OffScaleHigh)
+{
+    EXPECT_BINRESULT(priv::getBin(35, bigBins), 1, 1.0f);
+}
+
+
+TEST(TableBinsBig, NearMiddleLow)
+{
+    EXPECT_BINRESULT(priv::getBin(19.99f, bigBins), 0, 0.999f);
+}
+
+TEST(TableBinsBig, NearMiddleExact)
+{
+    EXPECT_BINRESULT(priv::getBin(20.0f, bigBins), 1, 0);
+}
+
+TEST(TableBinsBig, NearMiddleHigh)
+{
+    EXPECT_BINRESULT(priv::getBin(20.01f, bigBins), 1, 0.001f);
+}
+
+TEST(TableBinsBig, LeftMiddle)
+{
+    EXPECT_BINRESULT(priv::getBin(15.0f, bigBins), 0, 0.5f);
+}
+
+TEST(TableBinsBig, RightMiddle)
+{
+    EXPECT_BINRESULT(priv::getBin(25.0f, bigBins), 1, 0.5f);
+}