diff --git a/include/sensor_trigger/jetson_gpio.hpp b/include/sensor_trigger/jetson_gpio.hpp index 51bd52a..f223a42 100644 --- a/include/sensor_trigger/jetson_gpio.hpp +++ b/include/sensor_trigger/jetson_gpio.hpp @@ -25,8 +25,8 @@ #define SYSFS_GPIO_DIR "/sys/class/gpio" #define BUFFER_SIZE 64 -#define GPIO_OUTPUT 1 -#define GPIO_INPUT 0 +#define GPIO_OUTPUT gpiod::line_request::DIRECTION_OUTPUT +#define GPIO_INPUT gpiod::line_request::DIRECTION_INPUT #define GPIO_HIGH 1 #define GPIO_LOW 0 @@ -35,29 +35,22 @@ typedef int gpio_state; namespace jetson_gpio { -// Mapping of GPIO number to pin number for ROSCubeX -// Note: pin 5->216 is pin 5 on the DB50 connector, run by GPIO chip 216 (starting at GPIO number -// 216) -static std::map pin_gpio_mapping{{5, 216}, {51, 408}, {52, 350}, {53, 446}, {54, 445}}; - class JetsonGpio { public: - JetsonGpio() : state_file_descriptor_(-1) {} + JetsonGpio() = default; ~JetsonGpio(); bool init_gpio_pin(unsigned int gpio_chip, unsigned int gpio_line, gpio_direction direction); bool set_gpio_pin_state(gpio_state state); protected: - bool export_gpio(); - bool unexport_gpio(); + bool close_gpio(); bool set_gpio_direction(gpio_direction direction); - int state_file_descriptor_; int gpio_; gpiod::chip gpio_chip_; - gpiod::line_bulk gpio_lines_; + gpiod::line gpio_line_; gpiod::line_request gpio_request_; }; } // namespace jetson_gpio diff --git a/src/jetson_gpio.cpp b/src/jetson_gpio.cpp index a291a1e..be97cf5 100644 --- a/src/jetson_gpio.cpp +++ b/src/jetson_gpio.cpp @@ -22,15 +22,11 @@ namespace jetson_gpio { JetsonGpio::~JetsonGpio() { - if (state_file_descriptor_ > -1) { - close(state_file_descriptor_); - } - // Regardless of the existence of other processes that uses the same GPIO pin // (incl. zombie GPIO port opening because of failure exit), - // this unexport closes the target GPIO pin anyway. + // this function closes the target GPIO pin anyway. // This behavior intends to make next try to use this GPIO pin success. - unexport_gpio(); + close_gpio(); } bool JetsonGpio::init_gpio_pin( @@ -39,49 +35,29 @@ bool JetsonGpio::init_gpio_pin( std::string gpio_character_device = "/dev/gpiochip" + std::to_string(gpio_chip); gpio_chip_ = gpiod::chip(gpio_character_device); - gpio_lines_ = gpio_chip_.get_lines( - std::vector({gpio_line})); // XXX: 143 = Anvil misc.I/O GP_Out_1, 108 = PWM_Out_0 + gpio_line_ = gpio_chip_.get_line(gpio_line); gpio_request_ = { - "sensor_trigger", // consumer name. XXX: fixed name may conflict for multiple instances - gpiod::line_request::DIRECTION_OUTPUT, // request_type - 0 // flag + "sensor_trigger", // consumer name + direction, // request_type + 0 // flag }; - if (!set_gpio_direction(direction)) { - return false; - } - gpio_lines_.request(gpio_request_, std::vector({GPIO_LOW})); + gpio_line_.request(gpio_request_, GPIO_LOW); return true; } -bool JetsonGpio::export_gpio() { return true; } - -bool JetsonGpio::unexport_gpio() +bool JetsonGpio::close_gpio() { gpio_chip_.~chip(); return true; } -bool JetsonGpio::set_gpio_direction(gpio_direction direction) -{ - switch (direction) { - case GPIO_INPUT: - gpio_request_.request_type = gpiod::line_request::DIRECTION_INPUT; - break; - case GPIO_OUTPUT: - gpio_request_.request_type = gpiod::line_request::DIRECTION_OUTPUT; - break; - } - - return true; -} - bool JetsonGpio::set_gpio_pin_state(gpio_state state) { - gpio_lines_.set_values(std::vector({state})); + gpio_line_.set_value(state); return true; } diff --git a/src/sensor_trigger.cpp b/src/sensor_trigger.cpp index b6bfbbf..1364d4a 100644 --- a/src/sensor_trigger.cpp +++ b/src/sensor_trigger.cpp @@ -136,14 +136,13 @@ void SensorTrigger::run() target_nsec = start_nsec; wait_nsec = 1e9 - now_nsec + start_nsec - 1e7; } - // Keep waiting for half the remaining time until the last millisecond. + // Keep waiting for half the remaining time until the last 10 milliseconds. // This is required as sleep_for tends to oversleep significantly if (wait_nsec > 1e7) { rclcpp::sleep_for(std::chrono::nanoseconds(wait_nsec / 2)); } } while (wait_nsec > 1e7); - // std::lock_guard guard(iomutex_); - // Block the last millisecond + // Block the last 10 milliseconds now_nsec = rclcpp::Clock{RCL_SYSTEM_TIME}.now().nanoseconds() % (uint64_t)1e9; if (start_nsec == end_nsec) { while (now_nsec > 1e7) {