From 4f91a0090664c06570c7b5c94b9c243b365906dc Mon Sep 17 00:00:00 2001
From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com>
Date: Tue, 21 May 2024 14:25:19 -0400
Subject: [PATCH] make sure we are setting sensor id

---
 .../sensors/core/tasks/capacitive_driver.hpp  | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp
index 58ab83fbf..408d45342 100644
--- a/include/sensors/core/tasks/capacitive_driver.hpp
+++ b/include/sensors/core/tasks/capacitive_driver.hpp
@@ -79,14 +79,19 @@ class FDC1004 {
     auto get_sensor_id() -> can::ids::SensorId { return sensor_id; }
 
     auto set_sensor_id(can::ids::SensorId _id) -> void {
-        if (shared_sensor && sensor_id != _id) {
-            if (_id == can::ids::SensorId::S1) {
-                measure_mode = fdc1004::MeasureConfigMode::TWO;
-            } else {
-                measure_mode = fdc1004::MeasureConfigMode::ONE;
-            }
+        if (sensor_id != _id) {
+            // we should always update the sensor id
             sensor_id = _id;
-            update_capacitance_configuration();
+            if (shared_sensor) {
+                // if we're sharing the sensor, then we need to update
+                // the measure mode
+                if (_id == can::ids::SensorId::S1) {
+                    measure_mode = fdc1004::MeasureConfigMode::TWO;
+                } else {
+                    measure_mode = fdc1004::MeasureConfigMode::ONE;
+                }
+                update_capacitance_configuration();
+            }
         }
     }