From 6e8197883fcdf33a6f8906ee894b1d6f25547467 Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 19 Nov 2024 09:03:56 +0000 Subject: [PATCH] add pin validation wait time --- .../panther_system/gpio/gpio_driver.hpp | 6 +++++- .../src/panther_system/gpio/gpio_controller.cpp | 2 +- .../src/panther_system/gpio/gpio_driver.cpp | 12 ++++++++++-- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp index d71d81c9..9c45fa28 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system/gpio/gpio_driver.hpp @@ -189,13 +189,17 @@ class GPIODriver * * @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); + bool SetPinValue( + const GPIOPin pin, const bool value, + const std::chrono::milliseconds & pin_validation_wait_time = std::chrono::milliseconds(0)); private: std::unique_ptr CreateLineRequest(gpiod::chip & chip); diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp index fe13fcd8..76212c6a 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp +++ b/panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp @@ -177,7 +177,7 @@ bool GPIOControllerPTH12X::ChargerEnable(const bool enable) bool GPIOControllerPTH12X::LEDControlEnable(const bool enable) { - return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable); + return gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, enable, std::chrono::milliseconds(10)); } std::unordered_map GPIOControllerPTH12X::QueryControlInterfaceIOStates() const diff --git a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp b/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp index 09b8c0fd..48126889 100644 --- a/panther_hardware_interfaces/src/panther_system/gpio/gpio_driver.cpp +++ b/panther_hardware_interfaces/src/panther_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."); }