From abf571728649e81d95c403bbf6de3cf84491b038 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Tue, 6 May 2025 12:49:21 +0200 Subject: [PATCH] Changed service to wait for proper response from i2c --- mg_bt/i2cmodule/i2cnode.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/mg_bt/i2cmodule/i2cnode.cpp b/mg_bt/i2cmodule/i2cnode.cpp index 1c9f65c..6d3e333 100644 --- a/mg_bt/i2cmodule/i2cnode.cpp +++ b/mg_bt/i2cmodule/i2cnode.cpp @@ -8,7 +8,6 @@ extern "C" { #include #include } - class MgI2c : public rclcpp::Node { using I2cSrv = mg_msgs::srv::I2c; @@ -21,7 +20,15 @@ class MgI2c : public rclcpp::Node { void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const { ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT - int ch = i2c_smbus_read_byte_data(i2c_fd_, req->data); + i2c_smbus_write_byte(i2c_fd_, req->data); + int ch = 0; + + rclcpp::Rate rate(100); + + while (ch == 0 || (ch < 256 && ch > 0)) { + ch = i2c_smbus_read_byte(i2c_fd_); + rate.sleep(); + } resp->resp.push_back(ch); RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front()); }