Ros2 fix led bug

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<gpiod::line_request> CreateLineRequest(gpiod::chip & chip);

panther_hardware_interfaces/src/panther_system/gpio/gpio_controller.cpp

@@ -177,7 +177,9 @@ bool GPIOControllerPTH12X::ChargerEnable(const bool enable)
 
 bool GPIOControllerPTH12X::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<GPIOPin, bool> GPIOControllerPTH12X::QueryControlInterfaceIOStates() const

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.");
     }