From 1df349b47890446717aa04b9afbec7486bf2c0a8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:19:42 +0100 Subject: [PATCH] add reading bytes when getting a T --- drone_sensors/src/height_sensor.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index a700de9a..13343dc0 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 50ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); @@ -34,14 +34,17 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Height: " + std::to_string((char)readdata[0]); if (readdata[0] == 'T') { RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); + char *measurement = new char[2]; + serial_port.read(measurement, 2); + message.data = "Height: " + std::to_string(measurement[0]) + std::to_string(measurement[1]); + RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); + publisher_->publish(message); + } - RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); - publisher_->publish(message); } /**