From e8db5a4ce91a09cb57eb5597898ca7c5a3452a8f Mon Sep 17 00:00:00 2001 From: Ryan Howard Date: Mon, 1 Jul 2024 15:13:03 -0400 Subject: [PATCH] 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 | 3 +- include/motor-control/core/motor_messages.hpp | 2 + .../stepper_motor/motor_interrupt_handler.hpp | 15 +- .../core/sensor_hardware_interface.hpp | 91 ++++++++++ .../sensors/core/tasks/capacitive_driver.hpp | 14 +- .../sensors/core/tasks/pressure_driver.hpp | 19 +- .../core/tasks/pressure_sensor_task.hpp | 3 + include/sensors/firmware/sensor_hardware.hpp | 1 + include/sensors/tests/mock_hardware.hpp | 15 ++ sensors/tests/CMakeLists.txt | 1 + sensors/tests/test_sensor_hardware.cpp | 165 ++++++++++++++++++ 11 files changed, 307 insertions(+), 22 deletions(-) create mode 100644 sensors/tests/test_sensor_hardware.cpp diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index e32dd038e..50232ad3f 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -215,7 +215,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. */ diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp index 0f77c5dc7..62035d439 100644 --- a/include/motor-control/core/motor_messages.hpp +++ b/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, diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index f79bd79c5..5196a7f2d 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/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(can::ids::SensorOutputBinding::sync) | - static_cast( - can::ids::SensorOutputBinding::report) | - static_cast( - 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 diff --git a/include/sensors/core/sensor_hardware_interface.hpp b/include/sensors/core/sensor_hardware_interface.hpp index 3a29aefbc..e39db7b76 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 { @@ -15,6 +16,31 @@ struct SensorHardwareConfiguration { std::optional 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(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(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 { diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 3b5c4c7b8..4fd5ed3ef 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/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); } } diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index cde222888..4bdb7f579 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/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) { diff --git a/include/sensors/core/tasks/pressure_sensor_task.hpp b/include/sensors/core/tasks/pressure_sensor_task.hpp index 83f4294a8..7c0d72023 100644 --- a/include/sensors/core/tasks/pressure_sensor_task.hpp +++ b/include/sensors/core/tasks/pressure_sensor_task.hpp @@ -130,6 +130,9 @@ class PressureMessageHandler { driver.set_echoing( m.binding & static_cast(can::ids::SensorOutputBinding::report)); + driver.set_multi_sensor_sync( + m.binding & static_cast( + can::ids::SensorOutputBinding::multi_sensor_sync)); driver.set_bind_sync( m.binding & static_cast(can::ids::SensorOutputBinding::sync)); diff --git a/include/sensors/firmware/sensor_hardware.hpp b/include/sensors/firmware/sensor_hardware.hpp index d439b3c50..b6ccc774b 100644 --- a/include/sensors/firmware/sensor_hardware.hpp +++ b/include/sensors/firmware/sensor_hardware.hpp @@ -22,5 +22,6 @@ class SensorHardware : public SensorHardwareBase { sensors::hardware::SensorHardwareConfiguration hardware; }; + }; // namespace hardware }; // namespace sensors diff --git a/include/sensors/tests/mock_hardware.hpp b/include/sensors/tests/mock_hardware.hpp index fcfbac999..56b7d839f 100644 --- a/include/sensors/tests/mock_hardware.hpp +++ b/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; diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt index b00f035c7..55f9d034b 100644 --- a/sensors/tests/CMakeLists.txt +++ b/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) diff --git a/sensors/tests/test_sensor_hardware.cpp b/sensors/tests/test_sensor_hardware.cpp new file mode 100644 index 000000000..d885eaf5d --- /dev/null +++ b/sensors/tests/test_sensor_hardware.cpp @@ -0,0 +1,165 @@ +#include + +#include "can/core/messages.hpp" +#include "catch2/catch.hpp" +#include "sensors/core/sensor_hardware_interface.hpp" +#include "sensors/tests/mock_hardware.hpp" + +constexpr auto sensor_id_primary = can::ids::SensorId::S0; +constexpr auto sensor_id_secondary = can::ids::SensorId::S1; + +SCENARIO("Multiple Sensors connected") { + test_mocks::MockSensorHardware mock_hw = test_mocks::MockSensorHardware{}; + GIVEN("One Sensor In use") { + WHEN("Sensor not enabled") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + WHEN("Sensor enabled") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync_enabled(sensor_id_primary, true); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + WHEN("Sensor required") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync_required(sensor_id_primary, true); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + WHEN("Sensor enabled and other sensor sets") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync_enabled(sensor_id_primary, true); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + WHEN("Resetting sync for in use sensor") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync_enabled(sensor_id_primary, true); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + mock_hw.reset_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + WHEN("Resetting sync for in other sensor") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync_enabled(sensor_id_primary, true); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + mock_hw.reset_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + } + GIVEN("Two sensors enabled") { + mock_hw.set_sync_enabled(sensor_id_primary, true); + mock_hw.set_sync_enabled(sensor_id_secondary, true); + WHEN("primary sets") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + WHEN("Secondary sets") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + WHEN("both are set") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_primary); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + WHEN("Sensors are disable") { + mock_hw.set_sync(sensor_id_primary); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + mock_hw.set_sync_enabled(sensor_id_primary, false); + REQUIRE(mock_hw.get_sync_state_mock() == true); + mock_hw.set_sync_enabled(sensor_id_secondary, false); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + } + GIVEN("Two sensors required") { + mock_hw.set_sync_required(sensor_id_primary, true); + mock_hw.set_sync_required(sensor_id_secondary, true); + WHEN("primary sets") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + WHEN("Secondary sets") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + WHEN("both are set") { + REQUIRE(mock_hw.get_sync_state_mock() == false); + mock_hw.set_sync(sensor_id_primary); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + } +} + +SCENARIO("Controling multiple sensors at once") { + test_mocks::MockSensorHardware mock_hw{}; + GIVEN("Using the BOTH sensorid") { + WHEN("BOTH sensors are enabled") { + mock_hw.set_sync_enabled(can::ids::SensorId::BOTH, true); + THEN("Primary works") { + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + THEN("Secondary works") { + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + } + WHEN("BOTH sensors are required") { + mock_hw.set_sync_required(can::ids::SensorId::BOTH, true); + THEN("Primary wont solo trigger") { + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + THEN("Secondary wont solo trigger") { + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + THEN("both set triggers") { + mock_hw.set_sync(sensor_id_primary); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == true); + } + } + } + GIVEN("Using the NONE sensorid") { + WHEN("NONE sensors are enabled") { + mock_hw.set_sync_enabled(can::ids::SensorId::UNUSED, true); + THEN("Primary doesn't set") { + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + THEN("Secondary doesn't set") { + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + } + WHEN("NONE sensors are required") { + mock_hw.set_sync_required(can::ids::SensorId::UNUSED, true); + THEN("Primary wont solo trigger") { + mock_hw.set_sync(sensor_id_primary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + THEN("Secondary wont solo trigger") { + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + THEN("both set wont trigger") { + mock_hw.set_sync(sensor_id_primary); + mock_hw.set_sync(sensor_id_secondary); + REQUIRE(mock_hw.get_sync_state_mock() == false); + } + } + } +}