feat(sensors): Enable dual sensor logic for 8/96 channel (#789)

* first pass idea

* move implementation to interface and add tests

* use new methods

* format lint

* lint

include/can/core/ids.hpp

@@ -214,7 +214,8 @@ enum class SensorOutputBinding {
     sync = 0x1,
     report = 0x2,
     max_threshold_sync = 0x4,
-    auto_baseline_report = 0x8,
+    auto_baseline_report = 0x08,
+    multi_sensor_sync = 0x10,
 };
 
 /** How a sensor's threshold should be interpreted. */

include/motor-control/core/motor_messages.hpp

@@ -91,6 +91,7 @@ struct SensorSyncMove {  // NOLINT(cppcoreguidelines-pro-type-member-init)
     uint16_t usage_key;
     can::ids::SensorId sensor_id;
     can::ids::SensorType sensor_type;
+    uint8_t binding_flags;
 
     auto build_ack(uint32_t position, int32_t pulses, uint8_t flags,
                    AckMessageId _id) -> Ack {
@@ -121,6 +122,7 @@ struct GearMotorMove  // NOLINT(cppcoreguidelines-pro-type-member-init)
     can::ids::GearMotorId gear_motor_id;
     can::ids::SensorId sensor_id;
     can::ids::SensorType sensor_type;
+    uint8_t binding_flags;
     auto build_ack(uint32_t position, int32_t pulses, uint8_t flags,
                    AckMessageId _id) -> GearMotorAck {
         return GearMotorAck{message_index, group_id,

include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp

@@ -408,20 +408,17 @@ class MotorInterruptHandler {
                 hardware.get_encoder_pulses();
 #ifdef USE_SENSOR_MOVE
             if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) {
-                auto binding =
-                    static_cast<uint8_t>(can::ids::SensorOutputBinding::sync) |
-                    static_cast<uint8_t>(
-                        can::ids::SensorOutputBinding::report) |
-                    static_cast<uint8_t>(
-                        can::ids::SensorOutputBinding::auto_baseline_report);
                 if (buffered_move.sensor_id == can::ids::SensorId::BOTH) {
                     send_bind_message(buffered_move.sensor_type,
-                                      can::ids::SensorId::S0, binding);
+                                      can::ids::SensorId::S0,
+                                      buffered_move.binding_flags);
                     send_bind_message(buffered_move.sensor_type,
-                                      can::ids::SensorId::S1, binding);
+                                      can::ids::SensorId::S1,
+                                      buffered_move.binding_flags);
                 } else {
                     send_bind_message(buffered_move.sensor_type,
-                                      buffered_move.sensor_id, binding);
+                                      buffered_move.sensor_id,
+                                      buffered_move.binding_flags);
                 }
             }
 #endif

include/sensors/core/sensor_hardware_interface.hpp

@@ -3,6 +3,7 @@
 #include <functional>
 #include <optional>
 
+#include "can/core/ids.hpp"
 #include "common/firmware/gpio.hpp"
 
 namespace sensors {
@@ -15,6 +16,31 @@ struct SensorHardwareConfiguration {
     std::optional<gpio::PinConfig> tip_sense = std::nullopt;
 };
 
+enum class SensorIdBitMask : uint8_t {
+    S0 = 0x01,
+    S1 = 0x02,
+    UNUSED = 0x00,
+    BOTH = 0x03,
+};
+
+static auto get_mask_from_id(can::ids::SensorId sensor) -> uint8_t {
+    auto mask_enum = SensorIdBitMask::UNUSED;
+    switch (sensor) {
+        case can::ids::SensorId::BOTH:
+            mask_enum = SensorIdBitMask::BOTH;
+            break;
+        case can::ids::SensorId::S0:
+            mask_enum = SensorIdBitMask::S0;
+            break;
+        case can::ids::SensorId::S1:
+            mask_enum = SensorIdBitMask::S1;
+            break;
+        case can::ids::SensorId::UNUSED:
+        default:
+            mask_enum = SensorIdBitMask::UNUSED;
+    }
+    return static_cast<uint8_t>(mask_enum);
+}
 /** abstract sensor hardware device for a sync line */
 class SensorHardwareBase {
   public:
@@ -28,6 +54,71 @@ class SensorHardwareBase {
     virtual auto set_sync() -> void = 0;
     virtual auto reset_sync() -> void = 0;
     virtual auto check_tip_presence() -> bool = 0;
+
+    [[nodiscard]] auto mask_satisfied() const -> bool {
+        if (set_sync_required_mask !=
+            static_cast<uint8_t>(SensorIdBitMask::UNUSED)) {
+            // if anything is "required" only sync when they are all triggered
+            return (sync_state_mask & set_sync_required_mask) ==
+                   set_sync_required_mask;
+        }
+        return (sync_state_mask & set_sync_enabled_mask) != 0;
+    }
+
+    auto set_sync(can::ids::SensorId sensor) -> void {
+        // force the bit for this sensor to 1
+        sync_state_mask |= get_mask_from_id(sensor);
+        if (mask_satisfied()) {
+            set_sync();
+        }
+    }
+
+    auto reset_sync(can::ids::SensorId sensor) -> void {
+        // force the bit for this sensor to 0
+        sync_state_mask &= 0xFF ^ get_mask_from_id(sensor);
+        if (!mask_satisfied()) {
+            reset_sync();
+        }
+    }
+
+    auto set_sync_enabled(can::ids::SensorId sensor, bool enabled) -> void {
+        uint8_t applied_mask = get_mask_from_id(sensor);
+        if (!enabled) {
+            // force enabled bit to 0
+            set_sync_enabled_mask &= 0xFF ^ applied_mask;
+        } else {
+            // force enabled bit to 1
+            set_sync_enabled_mask |= applied_mask;
+        }
+        // update sync state now that requirements are different
+        if (mask_satisfied()) {
+            set_sync();
+        } else {
+            reset_sync();
+        }
+    }
+
+    auto set_sync_required(can::ids::SensorId sensor, bool required) -> void {
+        uint8_t applied_mask = get_mask_from_id(sensor);
+        if (!required) {
+            // force required bit to 0
+            set_sync_required_mask &= 0xFF ^ applied_mask;
+        } else {
+            // force required bit to 1
+            set_sync_required_mask |= applied_mask;
+        }
+        // update sync state now that requirements are different
+        if (mask_satisfied()) {
+            set_sync();
+        } else {
+            reset_sync();
+        }
+    }
+
+  private:
+    uint8_t set_sync_required_mask = 0x00;
+    uint8_t set_sync_enabled_mask = 0x00;
+    uint8_t sync_state_mask = 0x00;
 };
 
 struct SensorHardwareContainer {

include/sensors/core/tasks/capacitive_driver.hpp

@@ -91,12 +91,15 @@ class FDC1004 {
 
     void set_bind_sync(bool should_bind) {
         bind_sync = should_bind;
-        hardware.reset_sync();
+        hardware.set_sync_enabled(sensor_id, should_bind);
+    }
+    void set_multi_sensor_sync(bool should_bind) {
+        hardware.set_sync_required(sensor_id, should_bind);
     }
 
     void set_max_bind_sync(bool should_bind) {
         max_capacitance_sync = should_bind;
-        hardware.reset_sync();
+        hardware.reset_sync(sensor_id);
     }
 
     auto set_bind_flags(uint8_t binding) -> void { sensor_binding = binding; }
@@ -278,16 +281,17 @@ class FDC1004 {
 
         if (max_capacitance_sync) {
             if (capacitance > fdc1004::MAX_CAPACITANCE_READING) {
+                // Use the set_sync that always sets the sync line here
                 hardware.set_sync();
             } else {
-                hardware.reset_sync();
+                hardware.reset_sync(sensor_id);
             }
         }
         if (bind_sync) {
             if (capacitance > zero_threshold_pf) {
-                hardware.set_sync();
+                hardware.set_sync(sensor_id);
             } else {
-                hardware.reset_sync();
+                hardware.reset_sync(sensor_id);
             }
         }
 

include/sensors/core/tasks/pressure_driver.hpp

@@ -88,12 +88,16 @@ class MMR920 {
 
     void set_bind_sync(bool should_bind) {
         bind_sync = should_bind;
-        hardware.reset_sync();
+        hardware.set_sync_enabled(sensor_id, should_bind);
+    }
+
+    void set_multi_sensor_sync(bool should_bind) {
+        hardware.set_sync_required(sensor_id, should_bind);
     }
 
     void set_max_bind_sync(bool should_bind) {
         max_pressure_sync = should_bind;
-        hardware.reset_sync();
+        hardware.reset_sync(sensor_id);
     }
 
     auto get_threshold() -> int32_t { return threshold_pascals; }
@@ -368,16 +372,16 @@ class MMR920 {
                 (std::fabs(pressure - current_pressure_baseline_pa -
                            current_moving_pressure_baseline_pa) >
                  threshold_pascals)) {
-                hardware.set_sync();
+                hardware.set_sync(sensor_id);
             } else {
-                hardware.reset_sync();
+                hardware.reset_sync(sensor_id);
             }
         } else {
             if (std::fabs(pressure - current_pressure_baseline_pa) >
                 threshold_pascals) {
-                hardware.set_sync();
+                hardware.set_sync(sensor_id);
             } else {
-                hardware.reset_sync();
+                hardware.reset_sync(sensor_id);
             }
         }
     }
@@ -418,6 +422,7 @@ class MMR920 {
                 max_pressure_consecutive_readings = 0;
             }
             if (over_threshold) {
+                // Use the set_sync that always sets the sync line here
                 hardware.set_sync();
                 can_client.send_can_message(
                     can::ids::NodeId::host,
@@ -431,7 +436,7 @@ class MMR920 {
                 // the sync line on purpose this causes bouncing on the line
                 // that turns off the sync and then immediately turns it back on
                 // and this can cause disrupt the behavior
-                hardware.reset_sync();
+                hardware.reset_sync(sensor_id);
             }
         }
         if (bind_sync) {

include/sensors/core/tasks/pressure_sensor_task.hpp

@@ -130,6 +130,9 @@ class PressureMessageHandler {
         driver.set_echoing(
             m.binding &
             static_cast<uint8_t>(can::ids::SensorOutputBinding::report));
+        driver.set_multi_sensor_sync(
+            m.binding & static_cast<uint8_t>(
+                            can::ids::SensorOutputBinding::multi_sensor_sync));
         driver.set_bind_sync(
             m.binding &
             static_cast<uint8_t>(can::ids::SensorOutputBinding::sync));

include/sensors/firmware/sensor_hardware.hpp

@@ -22,5 +22,6 @@ class SensorHardware : public SensorHardwareBase {
 
     sensors::hardware::SensorHardwareConfiguration hardware;
 };
+
 };  // namespace hardware
 };  // namespace sensors

include/sensors/tests/mock_hardware.hpp

@@ -18,6 +18,21 @@ class MockSensorHardware : public sensors::hardware::SensorHardwareBase {
     auto get_sync_set_calls() const -> uint32_t { return sync_set_calls; }
     auto get_sync_reset_calls() const -> uint32_t { return sync_reset_calls; }
 
+    auto set_sync(can::ids::SensorId sensor) -> void {
+        sensors::hardware::SensorHardwareBase::set_sync(sensor);
+    }
+    auto reset_sync(can::ids::SensorId sensor) -> void {
+        sensors::hardware::SensorHardwareBase::reset_sync(sensor);
+    }
+    auto set_sync_enabled(can::ids::SensorId sensor, bool enabled) -> void {
+        sensors::hardware::SensorHardwareBase::set_sync_enabled(sensor,
+                                                                enabled);
+    }
+    auto set_sync_required(can::ids::SensorId sensor, bool required) -> void {
+        sensors::hardware::SensorHardwareBase::set_sync_required(sensor,
+                                                                 required);
+    }
+
   private:
     bool sync_state = false;
     uint32_t sync_set_calls = 0;

sensors/tests/CMakeLists.txt

@@ -13,6 +13,7 @@ add_executable(
         test_pressure_sensor.cpp
         test_pressure_driver.cpp
         test_capacitive_sensor_utils.cpp
+        test_sensor_hardware.cpp
 )
 
 target_include_directories(sensors PUBLIC ${CMAKE_SOURCE_DIR}/include)