diff --git a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp index a79b29f99..67c8f7905 100644 --- a/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp +++ b/husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/gpio/gpio_driver.hpp @@ -45,7 +45,9 @@ class GPIODriverInterface virtual void ChangePinDirection(const GPIOPin pin, const gpiod::line::direction direction) = 0; virtual bool IsPinAvailable(const GPIOPin pin) const = 0; virtual bool IsPinActive(const GPIOPin pin) = 0; - virtual bool SetPinValue(const GPIOPin pin, const bool value) = 0; + virtual bool SetPinValue( + const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)) = 0; }; /** @@ -168,13 +170,18 @@ class GPIODriver : public GPIODriverInterface * * @param pin GPIOPin to set the value for. * @param value The boolean value to set for the pin. + * @param pin_validation_wait_time The time duration to wait for the pin value to change before + * checking if change was successful. * * @throws std::invalid_argument if trying to set the value for an INPUT pin. * @throws std::runtime_error if changing the GPIO state fails. * * @return true if the pin value is successfully set, false otherwise. */ - bool SetPinValue(const GPIOPin pin, const bool value) override; + bool SetPinValue( + const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time = + std::chrono::milliseconds(0)) override; private: std::unique_ptr CreateLineRequest(gpiod::chip & chip); diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp index bfb8bdc5a..a90ac92f1 100644 --- a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_controller.cpp @@ -189,7 +189,9 @@ bool GPIOController::ChargerEnable(const bool enable) bool GPIOController::LEDControlEnable(const bool enable) { - return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); + // pin_validation_wait_time=10ms used due to slow pin state transition + // on pin loaded by high 100nF capacity in SBC Overlay v1.4 + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable, std::chrono::milliseconds(10)); } std::unordered_map GPIOController::QueryControlInterfaceIOStates() const diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp index d386b68db..db050d979 100644 --- a/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp +++ b/husarion_ugv_hardware_interfaces/src/robot_system/gpio/gpio_driver.cpp @@ -164,7 +164,8 @@ bool GPIODriver::IsPinActive(const GPIOPin pin) return pin_info.value == gpiod::line::value::ACTIVE; } -bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) +bool GPIODriver::SetPinValue( + const GPIOPin pin, const bool value, const std::chrono::milliseconds & pin_validation_wait_time) { GPIOInfo & gpio_info = GetGPIOInfoRef(pin); @@ -174,10 +175,17 @@ bool GPIODriver::SetPinValue(const GPIOPin pin, const bool value) gpiod::line::value gpio_value = value ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE; - std::lock_guard lock(gpio_info_storage_mutex_); + std::unique_lock lock(gpio_info_storage_mutex_); + try { line_request_->set_value(gpio_info.offset, gpio_value); + if (pin_validation_wait_time.count() > 0) { + lock.unlock(); + std::this_thread::sleep_for(pin_validation_wait_time); + lock.lock(); + } + if (line_request_->get_value(gpio_info.offset) != gpio_value) { throw std::runtime_error("Failed to change GPIO state."); } diff --git a/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp index 8be90f2cf..89ce4cc79 100644 --- a/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp +++ b/husarion_ugv_hardware_interfaces/test/robot_system/gpio/test_gpio_controller.cpp @@ -39,7 +39,13 @@ class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterf (override)); MOCK_METHOD(bool, IsPinAvailable, (const GPIOPin pin), (const, override)); MOCK_METHOD(bool, IsPinActive, (const GPIOPin pin), (override)); - MOCK_METHOD(bool, SetPinValue, (const GPIOPin pin, const bool value), (override)); + MOCK_METHOD( + bool, SetPinValue, + (const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time), + (override)); + + using NiceMock = testing::NiceMock; }; class GPIOControllerWrapper : public husarion_ugv_hardware_interfaces::GPIOController @@ -64,18 +70,19 @@ class TestGPIOController : public ::testing::Test protected: float GetRobotVersion(); - std::shared_ptr gpio_driver_; + std::shared_ptr gpio_driver_; std::unique_ptr gpio_controller_wrapper_; static constexpr int watchdog_edges_per_100ms_ = 10; }; TestGPIOController::TestGPIOController() { - gpio_driver_ = std::make_shared(); + gpio_driver_ = std::make_shared(); // Mock methods called during the initialization process - ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true)).WillByDefault(testing::Return(true)); - ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true)) + ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::VMOT_ON, true, testing::_)) + .WillByDefault(testing::Return(true)); + ON_CALL(*gpio_driver_, SetPinValue(GPIOPin::DRIVER_EN, true, testing::_)) .WillByDefault(testing::Return(true)); ON_CALL(*gpio_driver_, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(true)); @@ -106,7 +113,7 @@ TEST(TestGPIOControllerInitialization, WatchdogPinNotAvailable) { auto gpio_driver = std::make_shared(); - EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true)) + EXPECT_CALL(*gpio_driver, SetPinValue(testing::_, true, testing::_)) .Times(2) .WillRepeatedly(testing::Return(true)); ON_CALL(*gpio_driver, IsPinAvailable(GPIOPin::WATCHDOG)).WillByDefault(testing::Return(false)); @@ -142,14 +149,16 @@ TEST_P(ParametrizedTestGPIOController, TestGPIOEnableDisable) auto const disable = !enable; // Enable GPIO - EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable)).WillOnce(testing::Return(true)); + EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, enable, testing::_)) + .WillOnce(testing::Return(true)); EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(enable)); param.enable_method(gpio_controller_wrapper_.get(), true); EXPECT_EQ(enable, gpio_controller_wrapper_->IsPinActive(param.pin)); // Disable GPIO - EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable)).WillOnce(testing::Return(true)); + EXPECT_CALL(*gpio_driver_, SetPinValue(param.pin, disable, testing::_)) + .WillOnce(testing::Return(true)); EXPECT_CALL(*gpio_driver_, IsPinActive(param.pin)).WillOnce(testing::Return(disable)); param.enable_method(gpio_controller_wrapper_.get(), false); diff --git a/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp index 7797002eb..1b93e6081 100644 --- a/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp +++ b/husarion_ugv_hardware_interfaces/test/utils/system_test_utils.hpp @@ -66,7 +66,10 @@ class MockGPIODriver : public husarion_ugv_hardware_interfaces::GPIODriverInterf bool, IsPinAvailable, (const husarion_ugv_hardware_interfaces::GPIOPin), (const, override)); MOCK_METHOD(bool, IsPinActive, (const husarion_ugv_hardware_interfaces::GPIOPin), (override)); MOCK_METHOD( - bool, SetPinValue, (const husarion_ugv_hardware_interfaces::GPIOPin, const bool), (override)); + bool, SetPinValue, + (const husarion_ugv_hardware_interfaces::GPIOPin, const bool, + const std::chrono::milliseconds & pin_validation_wait_time), + (override)); using NiceMock = testing::NiceMock; };