diff --git a/include/sensors/core/sensor_hardware_interface.hpp b/include/sensors/core/sensor_hardware_interface.hpp index 3a29aefbc..5a1a2f3af 100644 --- a/include/sensors/core/sensor_hardware_interface.hpp +++ b/include/sensors/core/sensor_hardware_interface.hpp @@ -3,6 +3,7 @@ #include #include +#include "can/core/ids.hpp" #include "common/firmware/gpio.hpp" namespace sensors { @@ -28,6 +29,10 @@ class SensorHardwareBase { virtual auto set_sync() -> void = 0; virtual auto reset_sync() -> void = 0; virtual auto check_tip_presence() -> bool = 0; + virtual auto set_sync(can::ids::SensorId sensor) -> void = 0; + virtual auto reset_sync(can::ids::SensorId sensor) -> void = 0; + virtual auto set_sync_required(can::ids::SensorId sensor, bool required) -> void = 0; + virtual auto reset_sync_required(can::ids::SensorId sensor, bool required) -> void = 0; }; struct SensorHardwareContainer { diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 3b5c4c7b8..3339a825b 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -91,12 +91,13 @@ class FDC1004 { void set_bind_sync(bool should_bind) { bind_sync = should_bind; - hardware.reset_sync(); + hardware.set_sync_required(sensor_id); + hardware.reset_sync(sensor_id); } 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; } @@ -280,14 +281,14 @@ class FDC1004 { if (capacitance > fdc1004::MAX_CAPACITANCE_READING) { 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); } } diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index cde222888..04f3bd359 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -88,12 +88,13 @@ class MMR920 { void set_bind_sync(bool should_bind) { bind_sync = should_bind; - hardware.reset_sync(); + hardware.set_sync_required(sensor_id, should_bind); + hardware.reset_sync(sensor_id); } 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 +369,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 +419,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 +433,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) { diff --git a/include/sensors/firmware/sensor_hardware.hpp b/include/sensors/firmware/sensor_hardware.hpp index d439b3c50..110d8a910 100644 --- a/include/sensors/firmware/sensor_hardware.hpp +++ b/include/sensors/firmware/sensor_hardware.hpp @@ -7,6 +7,27 @@ namespace sensors { namespace hardware { +enum class SensorIdBitMask : uint8_t { + S0 = 0x01, + S1 = 0x02, + UNUSED = 0x00, + BOTH = 0x03, +}; + +static auto get_mask_from_id(can::ids::SensorId sensor) -> SensorIdBitMask{ + switch(sensor) { + case can::ids::SensorId::BOTH: + return SensorIdBitMask::BOTH; + case can::ids::SensorId::S0 : + return SensorIdBitMask::S0; + case can::ids::SensorId::S1 : + return SensorIdBitMask::S1; + case can::ids::SensorId::UNUSED : + default : + return SensorIdBitMask::UNUSED; + } +} + class SensorHardware : public SensorHardwareBase { public: SensorHardware(sensors::hardware::SensorHardwareConfiguration hardware) @@ -19,8 +40,32 @@ class SensorHardware : public SensorHardwareBase { } return false; } + auto set_sync(can::ids::SensorId sensor) -> void { + sync_state_mask |= get_mask_from_id(sensor); + if (sync_state_mask & set_sync_required_mask == set_sync_required_mask) { + set_sync(); + } + } + + auto reset_sync(can::ids::SensorId sensor) -> void { + sync_state_mask &= 0xFF ^ get_mask_from_id(sensor); + if (sync_state_mask & set_sync_required_mask != set_sync_required_mask) { + reset_sync(); + } + } + + auto set_sync_required(can::ids::SensorId sensor, bool required) -> void { + uint8_t applied_mask = get_mask_from_id(sensor); + if (!required) { + applied_mask ^= 0xFF; + } + set_sync_required_mask &= applied_mask; + } + + uint8_t set_sync_required_mask = 0x00; + uint8_t sync_state_mask = 0x00; - sensors::hardware::SensorHardwareConfiguration hardware; }; + }; // namespace hardware }; // namespace sensors