47 lines
1.3 KiB
C++
47 lines
1.3 KiB
C++
#include "rclcpp/rclcpp.hpp"
|
|
#include "mg_msgs/srv/i2c.hpp"
|
|
|
|
extern "C" {
|
|
|
|
#include <fcntl.h>
|
|
#include <linux/i2c-dev.h>
|
|
#include <sys/ioctl.h>
|
|
#include <i2c/smbus.h>
|
|
}
|
|
class MgI2c : public rclcpp::Node {
|
|
using I2cSrv = mg_msgs::srv::I2c;
|
|
|
|
public:
|
|
MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
|
|
auto cb
|
|
= [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
|
|
i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
|
|
}
|
|
|
|
void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
|
|
ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT
|
|
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());
|
|
}
|
|
|
|
private:
|
|
rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
|
|
int i2c_fd_;
|
|
};
|
|
|
|
int main(int argc, const char* const* argv) {
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::spin(std::make_shared<MgI2c>("i2cu"));
|
|
rclcpp::shutdown();
|
|
|
|
return 0;
|
|
} |