From 1c72ba3bd7b0f456472a2387b7a6a94ee2868838 Mon Sep 17 00:00:00 2001
From: Paul Whitehead <whiteheadpauld@gmail.com>
Date: Sat, 31 Jan 2026 11:47:58 -0500
Subject: [PATCH 1/9] Enhance motor temperature handling in ESC

Introduced a constant for disconnected motor temperature and updated telemetry processing to set motor temperature to MOTOR_TEMP_DISCONNECTED (-40.0) when the sensor is invalid. Adjusted related code in telemetry updates and monitoring to ensure proper handling of disconnected sensors, improving robustness and clarity in BLE telemetry reporting.
---
 inc/sp140/esc.h               | 13 +++++++-----
 src/sp140/ble/esc_service.cpp |  2 ++
 src/sp140/esc.cpp             | 40 ++++++++++++++++++-----------------
 src/sp140/esc_monitors.cpp    |  8 +++++--
 4 files changed, 37 insertions(+), 26 deletions(-)

diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h
index bcba449..72935eb 100644
--- a/inc/sp140/esc.h
+++ b/inc/sp140/esc.h
@@ -2,20 +2,23 @@
 #define INC_SP140_ESC_H_
 
 #include <Arduino.h>
+
+// Motor temp when sensor is disconnected (sent over BLE; display shows "-")
+constexpr float MOTOR_TEMP_DISCONNECTED = -40.0f;
 #include "sp140/structs.h"
 #include "../../inc/sp140/esp32s3-config.h"
 #include <SineEsc.h>
 #include <CanardAdapter.h>
 
-void initESC();
+void initESC();
 void setESCThrottle(int throttlePWM);
 void readESCTelemetry();
 bool setupTWAI();
 
-// Get the highest temperature from all ESC sensors
-float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry);
-
-// ESC Error Decoding Functions
+// Get the highest temperature from all ESC sensors
+float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry);
+
+// ESC Error Decoding Functions
 String decodeRunningError(uint16_t errorCode);
 String decodeSelfCheckError(uint16_t errorCode);
 bool hasRunningError(uint16_t errorCode);
diff --git a/src/sp140/ble/esc_service.cpp b/src/sp140/ble/esc_service.cpp
index 74a666e..179d6b3 100644
--- a/src/sp140/ble/esc_service.cpp
+++ b/src/sp140/ble/esc_service.cpp
@@ -92,6 +92,7 @@ void updateESCPackedTelemetry(const STR_ESC_TELEMETRY_140& telemetry) {
   packet.mos_temp = telemetry.mos_temp;
   packet.cap_temp = telemetry.cap_temp;
   packet.mcu_temp = telemetry.mcu_temp;
+  // When sensor disconnected, motor_temp is MOTOR_TEMP_DISCONNECTED (-40.0); client can show "N/A"
   packet.motor_temp = telemetry.motor_temp;
   packet.eRPM = static_cast<int32_t>(telemetry.eRPM);
   packet.inPWM = static_cast<uint16_t>(telemetry.inPWM);
@@ -118,6 +119,7 @@ void updateESCTelemetryBLE(const STR_ESC_TELEMETRY_140& telemetry) {
   float voltage = telemetry.volts;
   float current = telemetry.amps;
   int32_t rpm = static_cast<int32_t>(telemetry.eRPM);
+  // motor_temp is MOTOR_TEMP_DISCONNECTED (-40.0) when sensor unplugged
   EscTempsPacket temps = {
     telemetry.mos_temp,
     telemetry.cap_temp,
diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp
index 3de2aa1..2a966ca 100644
--- a/src/sp140/esc.cpp
+++ b/src/sp140/esc.cpp
@@ -94,23 +94,25 @@ void readESCTelemetry() {
       escTelemetryData.amps = res->bus_current / 10.0f;
       escTelemetryData.mos_temp = res->mos_temp / 10.0f;
       escTelemetryData.cap_temp = res->cap_temp / 10.0f;
-      escTelemetryData.mcu_temp = res->mcu_temp / 10.0f;
-      // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
-      // Disconnected sensor reads ~149°C (thermistor pulled high)
-      float rawMotorTemp = res->motor_temp / 10.0f;
-      if (rawMotorTemp > -20.0f && rawMotorTemp <= 140.0f) {
-        escTelemetryData.motor_temp = rawMotorTemp;
-      }
-      // else: keep previous value (sensor disconnected or invalid)
+      escTelemetryData.mcu_temp = res->mcu_temp / 10.0f;
+      // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
+      // Disconnected sensor reads ~149°C (thermistor pulled high)
+      float rawMotorTemp = res->motor_temp / 10.0f;
+      if (rawMotorTemp > -20.0f && rawMotorTemp <= 140.0f) {
+        escTelemetryData.motor_temp = rawMotorTemp;
+      } else {
+        // Set invalid sentinel so UI shows "-" and BLE sends MOTOR_TEMP_DISCONNECTED
+        escTelemetryData.motor_temp = MOTOR_TEMP_DISCONNECTED;
+      }
       escTelemetryData.eRPM = res->speed;
       escTelemetryData.inPWM = res->recv_pwm / 10.0f;
       watts = escTelemetryData.amps * escTelemetryData.volts;
 
-      // Store error bitmasks
-      escTelemetryData.running_error = res->running_error;
-      escTelemetryData.selfcheck_error = res->selfcheck_error;
-
-      // Record the time of this successful communication using the local clock
+      // Store error bitmasks
+      escTelemetryData.running_error = res->running_error;
+      escTelemetryData.selfcheck_error = res->selfcheck_error;
+
+      // Record the time of this successful communication using the local clock
       lastSuccessfulCommTimeMs = millis();
     }  // else: Timestamp hasn't changed, treat as stale data, don't update local timer or telemetry
 
@@ -304,12 +306,12 @@ double mapDouble(double x, double in_min, double in_max, double out_min, double
  * @param telemetry ESC telemetry data structure
  * @return The highest temperature value among motor, MOSFET, and capacitor temps
  */
-float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry) {
-  return max(telemetry.motor_temp, max(telemetry.mos_temp, telemetry.cap_temp));
-}
-
-/**
- * Decode running error bitmask into human-readable string
+float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry) {
+  return max(telemetry.motor_temp, max(telemetry.mos_temp, telemetry.cap_temp));
+}
+
+/**
+ * Decode running error bitmask into human-readable string
  * @param errorCode 16-bit running error code from ESC
  * @return String containing decoded error messages
  */
diff --git a/src/sp140/esc_monitors.cpp b/src/sp140/esc_monitors.cpp
index 82ce713..2b8c004 100644
--- a/src/sp140/esc_monitors.cpp
+++ b/src/sp140/esc_monitors.cpp
@@ -51,12 +51,16 @@ void addESCMonitors() {
   monitors.push_back(escCapTemp);
 
   // Motor Temperature Monitor - with hysteresis
-  // Note: Invalid readings (>140°C = disconnected) are filtered in esc.cpp
+  // When sensor is disconnected esc.cpp sets motor_temp to -40; don't trigger low-temp alert
   static HysteresisSensorMonitor* motorTemp = new HysteresisSensorMonitor(
     SensorID::Motor_Temp,
     SensorCategory::ESC,
     motorTempThresholds,
-    []() { return monitoringEscData.motor_temp; },
+    []() {
+      float t = monitoringEscData.motor_temp;
+      if (t <= -20.0f || t > 140.0f) return 50.0f;  // disconnected/invalid: report nominal, no alert
+      return t;
+    },
     &multiLogger);
   monitors.push_back(motorTemp);
 

From 54852ea331d87f8a19ff6856d5908a575c48b745 Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Wed, 11 Feb 2026 19:40:03 -0600
Subject: [PATCH 2/9] Handle NaN sensor readings and motor temp invalids

Include <math.h> and add isnan checks in SensorMonitor and HysteresisSensorMonitor so NaN readings are treated as OK (log/reset state and skip alert logic). Change motor temperature supplier to return NAN for out-of-range/disconnected values instead of a nominal 50. This prevents spurious alerts when sensors are disconnected or invalid.
---
 inc/sp140/simple_monitor.h | 17 +++++++++++++++++
 src/sp140/esc_monitors.cpp |  5 ++---
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/inc/sp140/simple_monitor.h b/inc/sp140/simple_monitor.h
index 3795cde..a24f1a4 100644
--- a/inc/sp140/simple_monitor.h
+++ b/inc/sp140/simple_monitor.h
@@ -3,6 +3,7 @@
 
 #include <vector>
 #include <functional>
+#include <math.h>
 #include <Arduino.h>
 #include "sp140/structs.h"
 
@@ -148,6 +149,14 @@ struct SensorMonitor : public IMonitor {
 
   void check() override {
     float v = read();
+    if (isnan(v)) {
+      if (last != AlertLevel::OK) {
+        logger->log(id, AlertLevel::OK, v);
+        last = AlertLevel::OK;
+      }
+      return;
+    }
+
     AlertLevel now = AlertLevel::OK;
 
     if      (v <= thr.critLow)  now = AlertLevel::CRIT_LOW;
@@ -182,6 +191,14 @@ struct HysteresisSensorMonitor : public SensorMonitor {
 
   void check() override {
     float v = read();
+    if (isnan(v)) {
+      if (last != AlertLevel::OK) {
+        logger->log(id, AlertLevel::OK, v);
+        last = AlertLevel::OK;
+      }
+      return;
+    }
+
     AlertLevel now = last;  // Start with current state
 
     // Hysteresis logic to prevent bouncing
diff --git a/src/sp140/esc_monitors.cpp b/src/sp140/esc_monitors.cpp
index 2b8c004..544a20d 100644
--- a/src/sp140/esc_monitors.cpp
+++ b/src/sp140/esc_monitors.cpp
@@ -51,15 +51,14 @@ void addESCMonitors() {
   monitors.push_back(escCapTemp);
 
   // Motor Temperature Monitor - with hysteresis
-  // When sensor is disconnected esc.cpp sets motor_temp to -40; don't trigger low-temp alert
+  // Return NaN for disconnected/invalid sensor so monitor logic skips it.
   static HysteresisSensorMonitor* motorTemp = new HysteresisSensorMonitor(
     SensorID::Motor_Temp,
     SensorCategory::ESC,
     motorTempThresholds,
     []() {
       float t = monitoringEscData.motor_temp;
-      if (t <= -20.0f || t > 140.0f) return 50.0f;  // disconnected/invalid: report nominal, no alert
-      return t;
+      return (t > -20.0f && t <= 140.0f) ? t : NAN;
     },
     &multiLogger);
   monitors.push_back(motorTemp);

From c1c999ebbbf2f4ad6583929163c4b28c860d1051 Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Wed, 11 Feb 2026 20:26:47 -0600
Subject: [PATCH 3/9] Use NaN for invalid motor temperature

Replace the previous sentinel value for a disconnected motor temp with NaN and centralize the validity check. Added MOTOR_TEMP_VALID_MIN_C / MAX_C and inline isMotorTempValidC() in esc.h, updated the telemetry struct comment to indicate motor_temp may be NaN, and changed readESCTelemetry() to store NAN for invalid readings. BLE and LVGL code were updated to treat NaN as an invalid/missing reading (and math.h was included). The monitor callback now returns the stored motor_temp directly. This makes handling of disconnected/invalid motor temperature readings explicit and consistent across components.
---
 inc/sp140/esc.h                 |  9 +++++++--
 inc/sp140/structs.h             |  2 +-
 src/sp140/ble/esc_service.cpp   |  4 ++--
 src/sp140/esc.cpp               | 18 +++++++++---------
 src/sp140/esc_monitors.cpp      |  5 +----
 src/sp140/lvgl/lvgl_updates.cpp |  6 +++---
 6 files changed, 23 insertions(+), 21 deletions(-)

diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h
index 72935eb..84831ad 100644
--- a/inc/sp140/esc.h
+++ b/inc/sp140/esc.h
@@ -3,8 +3,13 @@
 
 #include <Arduino.h>
 
-// Motor temp when sensor is disconnected (sent over BLE; display shows "-")
-constexpr float MOTOR_TEMP_DISCONNECTED = -40.0f;
+// Motor temp validity range (disconnected/invalid readings are represented as NaN)
+constexpr float MOTOR_TEMP_VALID_MIN_C = -20.0f;
+constexpr float MOTOR_TEMP_VALID_MAX_C = 140.0f;
+
+inline bool isMotorTempValidC(float tempC) {
+  return tempC > MOTOR_TEMP_VALID_MIN_C && tempC <= MOTOR_TEMP_VALID_MAX_C;
+}
 #include "sp140/structs.h"
 #include "../../inc/sp140/esp32s3-config.h"
 #include <SineEsc.h>
diff --git a/inc/sp140/structs.h b/inc/sp140/structs.h
index a2031f3..29715c0 100644
--- a/inc/sp140/structs.h
+++ b/inc/sp140/structs.h
@@ -147,7 +147,7 @@ typedef struct {
   float mos_temp;               // MOSFET temperature (°C)
   float cap_temp;               // Capacitor temperature (°C)
   float mcu_temp;               // MCU temperature (°C)
-  float motor_temp;             // Motor temperature (°C)
+  float motor_temp;             // Motor temperature (°C), NaN if sensor invalid/disconnected
   int32_t eRPM;                 // Electrical RPM
   uint16_t inPWM;               // Input PWM value
   uint16_t running_error;       // Runtime error bitmask
diff --git a/src/sp140/ble/esc_service.cpp b/src/sp140/ble/esc_service.cpp
index 179d6b3..ca6272c 100644
--- a/src/sp140/ble/esc_service.cpp
+++ b/src/sp140/ble/esc_service.cpp
@@ -92,7 +92,7 @@ void updateESCPackedTelemetry(const STR_ESC_TELEMETRY_140& telemetry) {
   packet.mos_temp = telemetry.mos_temp;
   packet.cap_temp = telemetry.cap_temp;
   packet.mcu_temp = telemetry.mcu_temp;
-  // When sensor disconnected, motor_temp is MOTOR_TEMP_DISCONNECTED (-40.0); client can show "N/A"
+  // motor_temp is NaN when sensor is disconnected/invalid.
   packet.motor_temp = telemetry.motor_temp;
   packet.eRPM = static_cast<int32_t>(telemetry.eRPM);
   packet.inPWM = static_cast<uint16_t>(telemetry.inPWM);
@@ -119,7 +119,7 @@ void updateESCTelemetryBLE(const STR_ESC_TELEMETRY_140& telemetry) {
   float voltage = telemetry.volts;
   float current = telemetry.amps;
   int32_t rpm = static_cast<int32_t>(telemetry.eRPM);
-  // motor_temp is MOTOR_TEMP_DISCONNECTED (-40.0) when sensor unplugged
+  // motor_temp is NaN when sensor is disconnected/invalid.
   EscTempsPacket temps = {
     telemetry.mos_temp,
     telemetry.cap_temp,
diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp
index 2a966ca..1ac72c1 100644
--- a/src/sp140/esc.cpp
+++ b/src/sp140/esc.cpp
@@ -95,15 +95,15 @@ void readESCTelemetry() {
       escTelemetryData.mos_temp = res->mos_temp / 10.0f;
       escTelemetryData.cap_temp = res->cap_temp / 10.0f;
       escTelemetryData.mcu_temp = res->mcu_temp / 10.0f;
-      // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
-      // Disconnected sensor reads ~149°C (thermistor pulled high)
-      float rawMotorTemp = res->motor_temp / 10.0f;
-      if (rawMotorTemp > -20.0f && rawMotorTemp <= 140.0f) {
-        escTelemetryData.motor_temp = rawMotorTemp;
-      } else {
-        // Set invalid sentinel so UI shows "-" and BLE sends MOTOR_TEMP_DISCONNECTED
-        escTelemetryData.motor_temp = MOTOR_TEMP_DISCONNECTED;
-      }
+      // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
+      // Disconnected sensor reads ~149°C (thermistor pulled high)
+      float rawMotorTemp = res->motor_temp / 10.0f;
+      if (isMotorTempValidC(rawMotorTemp)) {
+        escTelemetryData.motor_temp = rawMotorTemp;
+      } else {
+        // Store invalid motor temp as NaN. Downstream consumers can skip on isnan().
+        escTelemetryData.motor_temp = NAN;
+      }
       escTelemetryData.eRPM = res->speed;
       escTelemetryData.inPWM = res->recv_pwm / 10.0f;
       watts = escTelemetryData.amps * escTelemetryData.volts;
diff --git a/src/sp140/esc_monitors.cpp b/src/sp140/esc_monitors.cpp
index 544a20d..c5c9dd5 100644
--- a/src/sp140/esc_monitors.cpp
+++ b/src/sp140/esc_monitors.cpp
@@ -56,10 +56,7 @@ void addESCMonitors() {
     SensorID::Motor_Temp,
     SensorCategory::ESC,
     motorTempThresholds,
-    []() {
-      float t = monitoringEscData.motor_temp;
-      return (t > -20.0f && t <= 140.0f) ? t : NAN;
-    },
+    []() { return monitoringEscData.motor_temp; },
     &multiLogger);
   monitors.push_back(motorTemp);
 
diff --git a/src/sp140/lvgl/lvgl_updates.cpp b/src/sp140/lvgl/lvgl_updates.cpp
index afe8ad8..8897c67 100644
--- a/src/sp140/lvgl/lvgl_updates.cpp
+++ b/src/sp140/lvgl/lvgl_updates.cpp
@@ -4,6 +4,7 @@
 #include "../../../inc/sp140/globals.h"
 #include "../../../inc/sp140/vibration_pwm.h"
 #include "../../../inc/sp140/shared-config.h"
+#include <math.h>
 
 // Flash timer globals - definitions
 lv_timer_t* cruise_flash_timer = NULL;
@@ -704,9 +705,8 @@ void updateLvglMainScreen(
     lv_obj_remove_style(motor_temp_bg, &style_warning, 0);
     lv_obj_remove_style(motor_temp_bg, &style_critical, 0);
 
-    // Show motor temp if ESC connected and reading is valid (not disconnected)
-    // Disconnected sensor reads ~149°C, so treat > 140°C as invalid
-    if (escTelemetry.escState == TelemetryState::CONNECTED && motorTemp > -20.0f && motorTemp <= 140.0f) {
+    // Show motor temp only for a valid numeric reading while ESC is connected.
+    if (escTelemetry.escState == TelemetryState::CONNECTED && !isnan(motorTemp)) {
       lv_label_set_text_fmt(motor_temp_label, "%d", static_cast<int>(motorTemp));
 
       if (motorTemp >= motorTempThresholds.critHigh) {

From 9f69e6f2d9dc4e7443d972d34354bef3108871dc Mon Sep 17 00:00:00 2001
From: Paul Whitehead <whiteheadpauld@gmail.com>
Date: Thu, 12 Feb 2026 12:11:11 -0500
Subject: [PATCH 4/9] Enhance throttle control with mode-aware PWM mapping (Now
 uses full physical throttle range in chill mode)

Added new function potRawToModePwm for converting raw pot readings to PWM values based on performance mode (CHILL or SPORT). Updated getSmoothedThrottlePwm and afterCruiseEnd to utilize this new mapping. Adjusted comments for clarity and consistency. Enhanced unit tests to verify mode-specific behavior in throttle calculations, ensuring correct PWM output across different performance modes.
---
 inc/sp140/throttle.h                 | 24 ++++++--
 src/sp140/sp140.ino                  |  4 +-
 src/sp140/throttle.cpp               | 36 ++++++-----
 test/test_throttle/test_throttle.cpp | 92 ++++++++++++++++------------
 4 files changed, 96 insertions(+), 60 deletions(-)

diff --git a/inc/sp140/throttle.h b/inc/sp140/throttle.h
index 6b8f6a6..01b9997 100644
--- a/inc/sp140/throttle.h
+++ b/inc/sp140/throttle.h
@@ -44,9 +44,22 @@ uint16_t readThrottleRaw();
 /** Get the most recently sampled raw throttle value (0..4095). */
 uint16_t getLastThrottleRaw();
 
-/** Convert raw pot reading (0..4095) to PWM microseconds. */
+/** Convert raw pot reading (0..4095) to PWM microseconds (full range). */
 int potRawToPwm(uint16_t raw);
 
+/**
+ * Convert raw pot reading (0..4095) to PWM microseconds using the
+ * mode-specific maximum.  In chill mode the full physical range maps to
+ * ESC_MIN_PWM..CHILL_MODE_MAX_PWM; in sport mode it maps to the full
+ * ESC_MIN_PWM..ESC_MAX_PWM range.  This gives chill mode full granular
+ * control over its limited output range instead of a hard clamp.
+ *
+ * @param raw              Raw ADC value (0..4095)
+ * @param performance_mode 0 = CHILL, 1 = SPORT
+ * @return PWM microseconds in the mode's range
+ */
+int potRawToModePwm(uint16_t raw, uint8_t performance_mode);
+
 /**
  * Apply mode-based ramp (us/tick) and clamp to the mode's max PWM.
  * Updates `prevPwm` with the final value.
@@ -86,10 +99,13 @@ bool throttleEngaged();
 /**
  * Read throttle input and return smoothed PWM value.
  * This is the core throttle processing pipeline without any state logic.
+ * Uses mode-aware mapping so the full physical range covers the mode's
+ * PWM output range.
  *
+ * @param performance_mode 0 = CHILL, 1 = SPORT
  * @return Smoothed PWM value from throttle input
  */
-int getSmoothedThrottlePwm();
+int getSmoothedThrottlePwm(uint8_t performance_mode);
 
 /**
  * Reset throttle state for clean startup/disarm.
@@ -107,8 +123,8 @@ void handleThrottle();
 
 /**
  * Calculate the cruise control PWM value from a raw pot reading.
- * Uses the same mapping as normal throttle (full range then clamp to mode max).
- * Also applies the absolute cruise max cap.
+ * Uses the same mode-aware mapping as normal throttle, then applies
+ * the absolute cruise max cap.
  *
  * @param potVal           Raw potentiometer value (0..4095)
  * @param performance_mode 0 = CHILL, 1 = SPORT
diff --git a/src/sp140/sp140.ino b/src/sp140/sp140.ino
index 1a2b3e2..381e289 100644
--- a/src/sp140/sp140.ino
+++ b/src/sp140/sp140.ino
@@ -1102,7 +1102,7 @@ void handleThrottle() {
       break;
 
     case ARMED:
-      int smoothedPwm = getSmoothedThrottlePwm();
+      int smoothedPwm = getSmoothedThrottlePwm(deviceData.performance_mode);
       finalPwm = applyModeRampClamp(smoothedPwm, prevPwm, deviceData.performance_mode);
       break;
   }
@@ -1178,7 +1178,7 @@ void afterCruiseEnd() {
   // Instead of clearing the buffer which causes throttle to drop to 0,
   // pre-populate it with the current throttle position to ensure smooth transition
   int currentPotVal = readThrottleRaw();
-  int currentPwmVal = potRawToPwm(currentPotVal);
+  int currentPwmVal = potRawToModePwm(currentPotVal, deviceData.performance_mode);
 
   // Pre-fill the buffer with current pot value for smooth transition
   throttleFilterReset(currentPwmVal);
diff --git a/src/sp140/throttle.cpp b/src/sp140/throttle.cpp
index 8166e8c..5c01faf 100644
--- a/src/sp140/throttle.cpp
+++ b/src/sp140/throttle.cpp
@@ -61,6 +61,15 @@ int potRawToPwm(uint16_t raw) {
   return map(raw, POT_MIN_VALUE, POT_MAX_VALUE, ESC_MIN_PWM, ESC_MAX_PWM);
 }
 
+/** Map raw ADC (0..4095) to mode-specific PWM range.
+ *  Chill mode: full physical range -> ESC_MIN_PWM..CHILL_MODE_MAX_PWM
+ *  Sport mode: full physical range -> ESC_MIN_PWM..ESC_MAX_PWM
+ */
+int potRawToModePwm(uint16_t raw, uint8_t performance_mode) {
+  int maxPwm = (performance_mode == 0) ? CHILL_MODE_MAX_PWM : ESC_MAX_PWM;
+  return map(raw, POT_MIN_VALUE, POT_MAX_VALUE, ESC_MIN_PWM, maxPwm);
+}
+
 /**
  * Apply mode-specific ramp limiting (in microseconds per tick) and clamp
  * the result to the current mode's max PWM. Updates prevPwm to the final value.
@@ -110,13 +119,16 @@ int throttleFilterAverage() {
 /**
  * Read throttle input and return smoothed PWM value.
  * This is the core throttle processing pipeline without any state logic.
+ * Uses mode-aware mapping so the full physical range covers the mode's
+ * PWM output range (chill: 1035-1600, sport: 1035-1950).
  *
+ * @param performance_mode 0 = CHILL, 1 = SPORT
  * @return Smoothed PWM value from throttle input
  */
-int getSmoothedThrottlePwm() {
-  // Read and convert raw pot to PWM
+int getSmoothedThrottlePwm(uint8_t performance_mode) {
+  // Read and convert raw pot to mode-specific PWM range
   int potValRaw = readThrottleRaw();
-  int targetPwm = potRawToPwm(potValRaw);
+  int targetPwm = potRawToModePwm(potValRaw, performance_mode);
 
   // Smooth in PWM domain using ring buffer
   throttleFilterPush(targetPwm);
@@ -134,21 +146,15 @@ void resetThrottleState(int& prevPwm) {
 
 /**
  * Calculate the cruise control PWM value from a raw pot reading.
- * Uses the same mapping as normal throttle: map to full range first,
- * then clamp to the mode maximum and cruise maximum.
- *
- * This ensures cruise maintains the actual throttle output level,
- * not a re-mapped value that would cause throttle drop in chill mode.
+ * Uses the same mode-aware mapping as normal throttle so the full
+ * physical range maps to the mode's output range, then applies the
+ * absolute cruise max cap.
  */
 uint16_t calculateCruisePwm(uint16_t potVal, uint8_t performance_mode, float cruiseMaxPct) {
-  // Step 1: Map to full PWM range (same as normal throttle)
-  uint16_t pwm = potRawToPwm(potVal);
-
-  // Step 2: Clamp to mode maximum (chill mode caps at lower PWM)
-  int maxPwmForMode = (performance_mode == 0) ? CHILL_MODE_MAX_PWM : ESC_MAX_PWM;
-  pwm = min(pwm, (uint16_t)maxPwmForMode);
+  // Step 1: Map to mode-specific PWM range (same mapping as normal throttle)
+  uint16_t pwm = potRawToModePwm(potVal, performance_mode);
 
-  // Step 3: Apply absolute cruise max cap
+  // Step 2: Apply absolute cruise max cap
   uint16_t absoluteMaxCruisePwm = ESC_MIN_PWM + (uint16_t)((ESC_MAX_PWM - ESC_MIN_PWM) * cruiseMaxPct);
   pwm = min(pwm, absoluteMaxCruisePwm);
 
diff --git a/test/test_throttle/test_throttle.cpp b/test/test_throttle/test_throttle.cpp
index f182bd5..67603c6 100644
--- a/test/test_throttle/test_throttle.cpp
+++ b/test/test_throttle/test_throttle.cpp
@@ -198,83 +198,97 @@ TEST(ThrottleTest, ResetThrottleState) {
     EXPECT_EQ(throttleFilterAverage(), 0); // Buffer should be empty
 }
 
-// Test calculateCruisePwm function - ensures cruise maintains actual throttle level
-TEST(ThrottleTest, CalculateCruisePwmBasic) {
-    // Test that cruise PWM matches normal throttle mapping
-    // potRawToPwm(2048) = 1492 (50% of full range)
-    // In chill mode, 1492 < 1600 (CHILL_MODE_MAX_PWM), so no clamping
-    // With 60% cruise cap: 1035 + 915*0.6 = 1584
+// Test potRawToModePwm - mode-aware mapping
+TEST(ThrottleTest, PotRawToModePwmMapping) {
+    // SPORT mode (mode 1) should be identical to potRawToPwm (full range)
+    EXPECT_EQ(potRawToModePwm(0, 1), 1035);
+    EXPECT_EQ(potRawToModePwm(4095, 1), 1950);
+    EXPECT_EQ(potRawToModePwm(2047, 1), 1492);
+    EXPECT_EQ(potRawToModePwm(1024, 1), 1263);
+    EXPECT_EQ(potRawToModePwm(3072, 1), 1721);
+
+    // CHILL mode (mode 0) maps full physical range to 1035-1600
+    // Range: 1600 - 1035 = 565
+    EXPECT_EQ(potRawToModePwm(0, 0), 1035);        // Min ADC -> Min PWM
+    EXPECT_EQ(potRawToModePwm(4095, 0), 1600);     // Max ADC -> CHILL max
+    EXPECT_EQ(potRawToModePwm(2048, 0), 1317);     // 50% -> mid chill range
+    EXPECT_EQ(potRawToModePwm(1024, 0), 1176);     // 25% -> 25% of chill range
+    EXPECT_EQ(potRawToModePwm(3072, 0), 1458);     // 75% -> 75% of chill range
+}
 
-    // 50% pot in SPORT mode (mode 1) - should match potRawToPwm exactly
+// Test calculateCruisePwm function - ensures cruise uses same mode-aware mapping
+TEST(ThrottleTest, CalculateCruisePwmBasic) {
+    // 50% pot in SPORT mode (mode 1) - should match potRawToModePwm exactly
+    // potRawToModePwm(2048, 1) = 1492, cruise cap at 60% = 1584
     uint16_t result = calculateCruisePwm(2048, 1, 0.60);
-    EXPECT_EQ(result, 1492);  // Same as potRawToPwm(2048)
+    EXPECT_EQ(result, 1492);  // Below cruise cap
 
-    // 50% pot in CHILL mode (mode 0) - should also be 1492 (below chill max)
+    // 50% pot in CHILL mode (mode 0) - uses chill range mapping
+    // potRawToModePwm(2048, 0) = 1317, cruise cap at 60% = 1584
     result = calculateCruisePwm(2048, 0, 0.60);
-    EXPECT_EQ(result, 1492);  // Not clamped, same as normal throttle
+    EXPECT_EQ(result, 1317);  // Chill mode maps full physical range to 1035-1600
 }
 
-// Test the bug that was fixed: chill mode cruise shouldn't drop throttle
-TEST(ThrottleTest, CalculateCruisePwmChillModeNoDrop) {
-    // THE BUG: Old code did map(pot, 0, 4095, 1035, 1600) in chill mode
-    // which gave 1317 for 50% pot instead of 1492
+// Test that cruise in chill mode uses the same mapping as normal throttle
+TEST(ThrottleTest, CalculateCruisePwmChillModeConsistency) {
+    // Cruise should use the same mode-aware mapping as normal throttle.
+    // In chill mode, full physical range maps to 1035-1600.
+    int normalThrottle = potRawToModePwm(2048, 0);  // 1317
+    EXPECT_EQ(normalThrottle, 1317);
 
-    // Verify normal throttle mapping at 50%
-    int normalThrottle = potRawToPwm(2048);
-    EXPECT_EQ(normalThrottle, 1492);
-
-    // Cruise in chill mode should match normal throttle (no drop!)
+    // Cruise in chill mode should match the normal throttle mapping
     uint16_t cruiseThrottle = calculateCruisePwm(2048, 0, 0.60);
-    EXPECT_EQ(cruiseThrottle, normalThrottle);  // CRITICAL: Must match!
+    EXPECT_EQ(cruiseThrottle, (uint16_t)normalThrottle);  // Both use same mapping
 }
 
-// Test chill mode max clamping
-TEST(ThrottleTest, CalculateCruisePwmChillModeClamping) {
-    // At high throttle in chill mode, should clamp to CHILL_MODE_MAX_PWM (1600)
-    // potRawToPwm(4095) = 1950, but chill mode caps at 1600
-
-    // 100% pot in CHILL mode - should clamp to 1600 (not 1950)
+// Test chill mode at full physical range
+TEST(ThrottleTest, CalculateCruisePwmChillModeFullRange) {
+    // 100% pot in CHILL mode - maps to exactly CHILL_MODE_MAX_PWM (1600)
+    // potRawToModePwm(4095, 0) = 1600, cruise cap at 70% = 1675
     uint16_t result = calculateCruisePwm(4095, 0, 0.70);
-    EXPECT_EQ(result, 1600);  // Clamped to CHILL_MODE_MAX_PWM
+    EXPECT_EQ(result, 1600);  // Full physical range -> CHILL_MODE_MAX_PWM
 
-    // 80% pot in CHILL mode - potRawToPwm(3276) ≈ 1767, clamps to 1600
+    // 80% pot in CHILL mode - potRawToModePwm(3276, 0) = 1486
+    // Cruise cap at 70% = 1675, so 1486 is below cap
     result = calculateCruisePwm(3276, 0, 0.70);
-    EXPECT_EQ(result, 1600);  // Clamped to CHILL_MODE_MAX_PWM
+    EXPECT_EQ(result, 1486);  // Full granular control, no clamping
 }
 
 // Test cruise max percentage capping
 TEST(ThrottleTest, CalculateCruisePwmCruiseMaxCap) {
     // Cruise max cap at 60%: 1035 + (1950-1035)*0.6 = 1035 + 549 = 1584
 
-    // In SPORT mode at high throttle, cruise cap should apply
-    // potRawToPwm(4095) = 1950, but cruise cap at 60% = 1584
+    // In SPORT mode at full throttle, cruise cap should apply
+    // potRawToModePwm(4095, 1) = 1950, cruise cap at 60% = 1584
     uint16_t result = calculateCruisePwm(4095, 1, 0.60);
-    EXPECT_EQ(result, 1584);  // Capped by cruise max, not mode max
+    EXPECT_EQ(result, 1584);  // Capped by cruise max
 
     // At 70% cruise cap: 1035 + 915*0.7 = 1675
     result = calculateCruisePwm(4095, 1, 0.70);
     EXPECT_EQ(result, 1675);  // Higher cruise cap
 
-    // In chill mode, the lower of (chill max, cruise cap) applies
-    // Chill max = 1600, cruise cap at 70% = 1675, so 1600 wins
+    // In chill mode at full throttle, chill max (1600) < cruise cap (1675)
+    // potRawToModePwm(4095, 0) = 1600
     result = calculateCruisePwm(4095, 0, 0.70);
-    EXPECT_EQ(result, 1600);  // Chill mode max is lower than cruise cap
+    EXPECT_EQ(result, 1600);  // Chill mode max is the limiter
 }
 
 // Test edge cases
 TEST(ThrottleTest, CalculateCruisePwmEdgeCases) {
-    // Minimum pot value
+    // Minimum pot value - same in both modes
     uint16_t result = calculateCruisePwm(0, 0, 0.60);
     EXPECT_EQ(result, 1035);  // ESC_MIN_PWM
 
     result = calculateCruisePwm(0, 1, 0.60);
     EXPECT_EQ(result, 1035);  // ESC_MIN_PWM
 
-    // Low throttle (25%) - should not be affected by any caps
-    // potRawToPwm(1024) = 1263
+    // Low throttle (25%) in chill mode - uses chill mapping
+    // potRawToModePwm(1024, 0) = 1176
     result = calculateCruisePwm(1024, 0, 0.60);
-    EXPECT_EQ(result, 1263);
+    EXPECT_EQ(result, 1176);  // Chill mode maps to reduced range
 
+    // Low throttle (25%) in sport mode - uses full mapping
+    // potRawToModePwm(1024, 1) = 1263
     result = calculateCruisePwm(1024, 1, 0.60);
     EXPECT_EQ(result, 1263);
 }

From 270d05677df878e5424a19b0915faf9143825e84 Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Thu, 12 Feb 2026 12:29:16 -0600
Subject: [PATCH 5/9] Update expected PWM value in throttle test

Fix off-by-one expected PWM in CalculateCruisePwmChillModeFullRange test: potRawToModePwm(3276, 0) returns 1487, so update the comment and EXPECT_EQ from 1486 to 1487.
---
 test/test_throttle/test_throttle.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/test/test_throttle/test_throttle.cpp b/test/test_throttle/test_throttle.cpp
index 67603c6..639b3ed 100644
--- a/test/test_throttle/test_throttle.cpp
+++ b/test/test_throttle/test_throttle.cpp
@@ -248,10 +248,10 @@ TEST(ThrottleTest, CalculateCruisePwmChillModeFullRange) {
     uint16_t result = calculateCruisePwm(4095, 0, 0.70);
     EXPECT_EQ(result, 1600);  // Full physical range -> CHILL_MODE_MAX_PWM
 
-    // 80% pot in CHILL mode - potRawToModePwm(3276, 0) = 1486
-    // Cruise cap at 70% = 1675, so 1486 is below cap
+    // 80% pot in CHILL mode - potRawToModePwm(3276, 0) = 1487
+    // Cruise cap at 70% = 1675, so 1487 is below cap
     result = calculateCruisePwm(3276, 0, 0.70);
-    EXPECT_EQ(result, 1486);  // Full granular control, no clamping
+    EXPECT_EQ(result, 1487);  // Full granular control, no clamping
 }
 
 // Test cruise max percentage capping

From b8afdd4d6bd1eaf188ce14c7dfa38afa377d0dfe Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Thu, 12 Feb 2026 12:37:54 -0600
Subject: [PATCH 6/9] Add analogRead test hook and new throttle tests

Expose a test hook for analogRead in native Arduino stubs (g_testAnalogReadValue + setTestAnalogReadValue) so tests can control ADC input. Add unit tests to exercise throttle behavior: deceleration clamping to ESC minimum (ApplyModeRampClampDecelToFloor), throttle filter ring buffer rollover and averages (ThrottleFilterRollover), and mode-aware mapping plus smoothing for getSmoothedThrottlePwm (GetSmoothedThrottlePwmModeAwareAndSmoothing). These additions increase coverage for boundary cases and smoothing logic.
---
 test/native_stubs/Arduino.h          |  4 +-
 test/test_throttle/test_throttle.cpp | 59 ++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+), 1 deletion(-)

diff --git a/test/native_stubs/Arduino.h b/test/native_stubs/Arduino.h
index 089be60..ed8c8e6 100644
--- a/test/native_stubs/Arduino.h
+++ b/test/native_stubs/Arduino.h
@@ -43,7 +43,9 @@ inline void delay(unsigned long ms) {
 #endif
 
 inline void pinMode(int, int) {}
-inline int analogRead(int) { return 0; }
+inline int g_testAnalogReadValue = 0;
+inline void setTestAnalogReadValue(int value) { g_testAnalogReadValue = value; }
+inline int analogRead(int) { return g_testAnalogReadValue; }
 inline void analogReadResolution(int) {}
 
 // Math helpers
diff --git a/test/test_throttle/test_throttle.cpp b/test/test_throttle/test_throttle.cpp
index 639b3ed..924c1c2 100644
--- a/test/test_throttle/test_throttle.cpp
+++ b/test/test_throttle/test_throttle.cpp
@@ -159,6 +159,22 @@ TEST(ThrottleTest, ApplyModeRampClamp) {
     EXPECT_EQ(result, 1867);     // Should ramp by 27, not clamp yet
 }
 
+// Test deceleration clamping at ESC minimum floor
+TEST(ThrottleTest, ApplyModeRampClampDecelToFloor) {
+    int prevPwm = 1040;
+
+    // Large decel in CHILL mode should never go below ESC_MIN_PWM
+    int result = applyModeRampClamp(500, prevPwm, 0);
+    EXPECT_EQ(result, 1035);
+    EXPECT_EQ(prevPwm, 1035);
+
+    // Large decel in SPORT mode should also clamp at ESC_MIN_PWM
+    prevPwm = 1040;
+    result = applyModeRampClamp(500, prevPwm, 1);
+    EXPECT_EQ(result, 1035);
+    EXPECT_EQ(prevPwm, 1035);
+}
+
 // Test throttle filter functions
 TEST(ThrottleTest, ThrottleFiltering) {
     // Clear buffer first
@@ -182,6 +198,29 @@ TEST(ThrottleTest, ThrottleFiltering) {
     EXPECT_EQ(throttleFilterAverage(), 1500);  // Should be filled with 1500
 }
 
+// Test throttle filter ring buffer rollover (capacity = 8)
+TEST(ThrottleTest, ThrottleFilterRollover) {
+    throttleFilterClear();
+
+    // Fill exactly to capacity
+    throttleFilterPush(1000);
+    throttleFilterPush(1100);
+    throttleFilterPush(1200);
+    throttleFilterPush(1300);
+    throttleFilterPush(1400);
+    throttleFilterPush(1500);
+    throttleFilterPush(1600);
+    throttleFilterPush(1700);
+    EXPECT_EQ(throttleFilterAverage(), 1350);  // (1000..1700)/8
+
+    // Push beyond capacity: oldest entries should roll off
+    throttleFilterPush(1800);                  // Buffer now 1100..1800
+    EXPECT_EQ(throttleFilterAverage(), 1450);
+
+    throttleFilterPush(1900);                  // Buffer now 1200..1900
+    EXPECT_EQ(throttleFilterAverage(), 1550);
+}
+
 // Test resetThrottleState function
 TEST(ThrottleTest, ResetThrottleState) {
     int prevPwm = 1500;
@@ -198,6 +237,26 @@ TEST(ThrottleTest, ResetThrottleState) {
     EXPECT_EQ(throttleFilterAverage(), 0); // Buffer should be empty
 }
 
+// Test getSmoothedThrottlePwm mode-aware mapping + smoothing behavior
+TEST(ThrottleTest, GetSmoothedThrottlePwmModeAwareAndSmoothing) {
+    // CHILL full-stick should map to chill max
+    throttleFilterClear();
+    setTestAnalogReadValue(4095);
+    EXPECT_EQ(getSmoothedThrottlePwm(0), 1600);
+
+    // SPORT full-stick should map to full ESC max
+    throttleFilterClear();
+    setTestAnalogReadValue(4095);
+    EXPECT_EQ(getSmoothedThrottlePwm(1), 1950);
+
+    // Verify smoothing in SPORT mode across two samples
+    throttleFilterClear();
+    setTestAnalogReadValue(0);              // 1035
+    EXPECT_EQ(getSmoothedThrottlePwm(1), 1035);
+    setTestAnalogReadValue(4095);           // 1950
+    EXPECT_EQ(getSmoothedThrottlePwm(1), 1492);  // (1035 + 1950) / 2
+}
+
 // Test potRawToModePwm - mode-aware mapping
 TEST(ThrottleTest, PotRawToModePwmMapping) {
     // SPORT mode (mode 1) should be identical to potRawToPwm (full range)

From 0c53a0fa01319bac96cac69ac9196c11605e3b94 Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Thu, 12 Feb 2026 20:39:34 -0600
Subject: [PATCH 7/9] Remove unused getHighestTemp

---
 inc/sp140/esc.h   |  7 ++-----
 src/sp140/esc.cpp | 17 ++++-------------
 2 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h
index 84831ad..10962c0 100644
--- a/inc/sp140/esc.h
+++ b/inc/sp140/esc.h
@@ -20,11 +20,8 @@ void setESCThrottle(int throttlePWM);
 void readESCTelemetry();
 bool setupTWAI();
 
-// Get the highest temperature from all ESC sensors
-float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry);
-
-// ESC Error Decoding Functions
-String decodeRunningError(uint16_t errorCode);
+// ESC Error Decoding Functions
+String decodeRunningError(uint16_t errorCode);
 String decodeSelfCheckError(uint16_t errorCode);
 bool hasRunningError(uint16_t errorCode);
 bool hasSelfCheckError(uint16_t errorCode);
diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp
index 1ac72c1..ed74f25 100644
--- a/src/sp140/esc.cpp
+++ b/src/sp140/esc.cpp
@@ -301,19 +301,10 @@ double mapDouble(double x, double in_min, double in_max, double out_min, double
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-/**
- * Get the highest temperature reading from all ESC sensors
- * @param telemetry ESC telemetry data structure
- * @return The highest temperature value among motor, MOSFET, and capacitor temps
- */
-float getHighestTemp(const STR_ESC_TELEMETRY_140& telemetry) {
-  return max(telemetry.motor_temp, max(telemetry.mos_temp, telemetry.cap_temp));
-}
-
-/**
- * Decode running error bitmask into human-readable string
- * @param errorCode 16-bit running error code from ESC
- * @return String containing decoded error messages
+/**
+ * Decode running error bitmask into human-readable string
+ * @param errorCode 16-bit running error code from ESC
+ * @return String containing decoded error messages
  */
 String decodeRunningError(uint16_t errorCode) {
   if (errorCode == 0) {

From 2e78237848f0fa2db8eda5ffe25bbe0419c7397b Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Thu, 12 Feb 2026 21:02:35 -0600
Subject: [PATCH 8/9] Remove alt_wire param and unused utilities

---
 inc/sp140/altimeter.h   |  4 ++--
 inc/sp140/utilities.h   | 19 +++++++------------
 src/sp140/altimeter.cpp | 12 +++++-------
 src/sp140/sp140.ino     |  2 +-
 src/sp140/utilities.cpp | 31 +++++++------------------------
 5 files changed, 22 insertions(+), 46 deletions(-)

diff --git a/inc/sp140/altimeter.h b/inc/sp140/altimeter.h
index aee7c86..b78ea0d 100644
--- a/inc/sp140/altimeter.h
+++ b/inc/sp140/altimeter.h
@@ -10,8 +10,8 @@
 #define VARIO_BUFFER_SIZE 25  // Number of samples to average for vertical speed
 #define MAX_VERTICAL_SPEED 250.0f  // Maximum vertical speed to display (m/s)
 
-// Set up the barometer
-bool setupAltimeter(bool alt_wire = false);
+// Set up the barometer
+bool setupAltimeter();
 
 // Get the altitude (in meters)
 float getAltitude(const STR_DEVICE_DATA_140_V1& deviceData);
diff --git a/inc/sp140/utilities.h b/inc/sp140/utilities.h
index ef1b1ad..3ab1696 100644
--- a/inc/sp140/utilities.h
+++ b/inc/sp140/utilities.h
@@ -1,15 +1,10 @@
-#ifndef INC_SP140_UTILITIES_H_
-#define INC_SP140_UTILITIES_H_
-
-#include <Arduino.h>
-
-double mapd(double x, double in_min, double in_max, double out_min, double out_max);
-
-// Function for digital time display - adds leading zeros
-String convertToDigits(byte digits);
-
-// Function to get unique chip ID
-String chipId();
+#ifndef INC_SP140_UTILITIES_H_
+#define INC_SP140_UTILITIES_H_
+
+#include <Arduino.h>
+
+// Function to get unique chip ID
+String chipId();
 
 // Definitions for main rainbow colors in WRGB format for NeoPixel.
 // The 32-bit color value is WRGB. W (White) is ignored for RGB pixels.
diff --git a/src/sp140/altimeter.cpp b/src/sp140/altimeter.cpp
index 5fa7f2a..8b70a0c 100644
--- a/src/sp140/altimeter.cpp
+++ b/src/sp140/altimeter.cpp
@@ -75,13 +75,11 @@ float getBaroPressure() {
   return __FLT_MIN__;  // Return a very small number if BMP is not present
 }
 
-// Start the bmp3XX sensor
-bool setupAltimeter(bool altWire) {
-  TwoWire* wire = &Wire;
-
-  // pull down pin 40 to high to set the address
-  pinMode(40, OUTPUT);
-  digitalWrite(40, HIGH);
+// Start the bmp3XX sensor
+bool setupAltimeter() {
+  // pull down pin 40 to high to set the address
+  pinMode(40, OUTPUT);
+  digitalWrite(40, HIGH);
   if (!bmp.begin_I2C(BMP3XX_DEFAULT_ADDRESS, &Wire)) return false;
 
   bmp.setOutputDataRate(BMP3_ODR_25_HZ);
diff --git a/src/sp140/sp140.ino b/src/sp140/sp140.ino
index 381e289..7d44402 100644
--- a/src/sp140/sp140.ino
+++ b/src/sp140/sp140.ino
@@ -799,7 +799,7 @@ void setup140() {
   const int SDA_PIN = 44;
   const int SCL_PIN = 41;
   Wire.setPins(SDA_PIN, SCL_PIN);
-  if (!setupAltimeter(board_config.alt_wire)) {
+  if (!setupAltimeter()) {
     USBSerial.println("Error initializing BMP3xx barometer");
   }
   if (ENABLE_VIBE) {
diff --git a/src/sp140/utilities.cpp b/src/sp140/utilities.cpp
index d5444b7..94920f3 100644
--- a/src/sp140/utilities.cpp
+++ b/src/sp140/utilities.cpp
@@ -1,27 +1,10 @@
-// Copyright 2021 <Zach Whitehead>
-#include "sp140/utilities.h"
-#include "Arduino.h"
-
-double mapd(double x, double in_min, double in_max, double out_min, double out_max) {
-  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-/**
- * For digital time display - prints leading 0
- *
- * @param digits number to be converted to a string.
- * @return string `12`, or 07 if `digits` is less than 10.
- */
-String convertToDigits(byte digits) {
-  String digits_string = "";
-  if (digits < 10) digits_string.concat("0");
-  digits_string.concat(digits);
-  return digits_string;
-}
-
-/**
- * Gets a unique ID string for the ESP32 chip
- *
+// Copyright 2021 <Zach Whitehead>
+#include "sp140/utilities.h"
+#include "Arduino.h"
+
+/**
+ * Gets a unique ID string for the ESP32 chip
+ *
  * @return String representation of the chip ID
  */
 String chipId() {

From 8c9ae75d833b209c82c5826f331987381825f67a Mon Sep 17 00:00:00 2001
From: Zach Whitehead <zjwhitehead@users.noreply.github.com>
Date: Thu, 12 Feb 2026 21:23:43 -0600
Subject: [PATCH 9/9] Clean up old critical alert service code

---
 inc/sp140/vibration_pwm.h   | 31 ------------------
 src/sp140/alert_display.cpp |  3 --
 src/sp140/vibration_pwm.cpp | 65 -------------------------------------
 3 files changed, 99 deletions(-)

diff --git a/inc/sp140/vibration_pwm.h b/inc/sp140/vibration_pwm.h
index 2a93198..89be351 100644
--- a/inc/sp140/vibration_pwm.h
+++ b/inc/sp140/vibration_pwm.h
@@ -74,35 +74,4 @@ void pulseVibration(uint16_t duration_ms, uint8_t intensity);
  */
 void stopVibration();
 
-/**
- * @brief Initializes the critical alert service.
- *
- * This function sets up the necessary resources for handling synchronized critical alerts.
- * It should be called once during system initialization.
- */
-void initCriticalAlertService();
-
-/**
- * @brief Starts the critical alert notifications.
- *
- * Activates the synchronized vibration and border flashing. If alerts are
- * already running, this function has no effect.
- */
-void startCriticalAlerts();
-
-/**
- * @brief Stops the critical alert notifications.
- *
- * Deactivates the vibration and border flashing. If alerts are not currently
- * running, this function has no effect.
- */
-void stopCriticalAlerts();
-
-/**
- * @brief Checks if the critical alert system is currently active.
- *
- * @return true if critical alerts are active, false otherwise.
- */
-bool isCriticalAlertActive();
-
 #endif  // INC_SP140_VIBRATION_PWM_H_
diff --git a/src/sp140/alert_display.cpp b/src/sp140/alert_display.cpp
index 20d7cef..ed0c324 100644
--- a/src/sp140/alert_display.cpp
+++ b/src/sp140/alert_display.cpp
@@ -43,9 +43,6 @@ void initAlertDisplay() {
     return;
   }
 
-  // Init the new alert service
-  initCriticalAlertService();
-
   // Create aggregation task (low priority)
   xTaskCreate(alertAggregationTask, "AlertAgg", 4096, NULL, 1, &alertAggregationTaskHandle);
   USBSerial.println("[AlertDisplay] Init complete");
diff --git a/src/sp140/vibration_pwm.cpp b/src/sp140/vibration_pwm.cpp
index 096385a..23d2ca5 100644
--- a/src/sp140/vibration_pwm.cpp
+++ b/src/sp140/vibration_pwm.cpp
@@ -1,6 +1,5 @@
 #include "sp140/vibration_pwm.h"
 #include "sp140/esp32s3-config.h"
-#include "sp140/lvgl/lvgl_updates.h"
 
 // Pin is configured via s3_config.vibe_pwm
 const int VIBE_PWM_FREQ = 1000;  // Adjust as needed
@@ -12,27 +11,6 @@ const int VIBE_PWM_RESOLUTION = 8;  // 8-bit resolution
 // Channels 2-7: Available for future use
 const int VIBE_PWM_CHANNEL = 0;
 
-static bool criticalVibrationActive = false;
-static TaskHandle_t criticalVibeTaskHandle = NULL;
-
-/**
- * Critical vibration task - provides continuous vibration for critical alerts
- */
-void criticalVibeTask(void* parameter) {
-  for (;;) {
-    if (criticalVibrationActive && ENABLE_VIBE) {
-      // Pulse every 1 second for critical alerts
-      ledcWrite(VIBE_PWM_CHANNEL, 200);  // Medium intensity for continuous
-      vTaskDelay(pdMS_TO_TICKS(300));    // 300ms on
-      ledcWrite(VIBE_PWM_CHANNEL, 0);
-      vTaskDelay(pdMS_TO_TICKS(700));    // 700ms off (total 1 second cycle)
-    } else {
-      // If not active, suspend task to save resources
-      vTaskSuspend(NULL);
-    }
-  }
-}
-
 /**
  * Vibration task - processes vibration requests from queue
  */
@@ -211,46 +189,3 @@ void customVibePattern(const uint8_t intensities[], const uint16_t durations[],
   }
   ledcWrite(VIBE_PWM_CHANNEL, 0);
 }
-
-// Service state for critical alerts
-static bool g_critical_alert_active = false;
-
-/**
- * @brief Initializes the critical alert service.
- */
-void initCriticalAlertService() {
-  // Initialization can be expanded if needed in the future.
-}
-
-/**
- * @brief Starts the critical alert notifications.
- */
-void startCriticalAlerts() {
-  if (g_critical_alert_active) {
-    return;
-  }
-  g_critical_alert_active = true;
-
-  // Start the single master LVGL timer, which will handle both border and vibration
-  startCriticalBorderFlash();
-}
-
-/**
- * @brief Stops the critical alert notifications.
- */
-void stopCriticalAlerts() {
-  if (!g_critical_alert_active) {
-    return;
-  }
-  g_critical_alert_active = false;
-
-  // Stop the master LVGL timer
-  stopCriticalBorderFlash();
-}
-
-/**
- * @brief Checks if the critical alert system is currently active.
- */
-bool isCriticalAlertActive() {
-  return g_critical_alert_active;
-}