Skip to content

fix uart to be nonblock, sync 4ms on control board and 4ms wall timer #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
iBoot32 wants to merge 2 commits into
base: main
Choose a base branch
from
Open

fix uart to be nonblock, sync 4ms on control board and 4ms wall timer #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
835c2f2
fix uart to be nonblock, sync 4ms on control board and 4ms wall timer
Feb 16, 2025
0866b56
fix print timing and compile
Feb 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 1 addition & 3 deletions src/prm_control/control_communicator/include/ControlCommunicatorNode.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,16 +88,14 @@ class ControlCommunicatorNode : public rclcpp::Node
rclcpp::TimerBase::SharedPtr uart_read_timer;
rclcpp::TimerBase::SharedPtr heart_beat_timer;

void start_uart(const char *port);
bool start_uart(const char *port);

void publish_static_tf(float, float, float, float, float, float, const char *, const char *);

void auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg);
void nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
void heart_beat_handler();

bool read_alignment();

void read_uart();
};

Expand Down
147 changes: 60 additions & 87 deletions src/prm_control/control_communicator/src/ControlCommunicatorNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,6 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr

this->start_uart(port);

if (this->read_alignment())
{
RCLCPP_INFO(this->get_logger(), "Inital Read alignment success.");
}
else
{
RCLCPP_WARN(this->get_logger(), "Inital Read alignment failed.");
}

RCLCPP_INFO(this->get_logger(), "should have printed.");

this->tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
Expand All @@ -60,7 +51,7 @@ ControlCommunicatorNode::ControlCommunicatorNode(const char *port) : Node("contr
this->target_robot_color_publisher = this->create_publisher<std_msgs::msg::String>("color_set", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
this->match_status_publisher = this->create_publisher<std_msgs::msg::Bool>("match_start", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());

// this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));
this->uart_read_timer = this->create_wall_timer(4ms, std::bind(&ControlCommunicatorNode::read_uart, this));

RCLCPP_INFO(this->get_logger(), "Control Communicator Node Started.");
}
Expand Down Expand Up @@ -92,60 +83,69 @@ void ControlCommunicatorNode::publish_static_tf(float x, float y, float z, float
tf_static_broadcaster->sendTransform(t);
}

void ControlCommunicatorNode::start_uart(const char *port)
bool ControlCommunicatorNode::start_uart(const char *port)
{
// Open the serial port in read/write mode, no controlling terminal, and no delay
int baud_rate = B1152000;
this->is_connected = false;
this->port_fd = open(port, O_RDWR);

// Check for errors
if (this->port_fd < 0)
{
RCLCPP_ERROR(this->get_logger(), "Failed to open: %s, %s", port, strerror(errno));
return;
}

struct termios tty;

// Set UART TTY to 8n1
tty.c_cflag &= ~PARENB;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8;

tty.c_cflag &= ~CRTSCTS; // No RTS/CTS flow control
tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines
tty.c_lflag &= ~ICANON; // Disable canonical mode

// Disable echo, erasure and newline echo
tty.c_lflag &= ~ECHO;
tty.c_lflag &= ~ECHOE;
tty.c_lflag &= ~ECHONL;

// Disable interpretation of INTR, QUIT and SUSP
tty.c_lflag &= ~ISIG;

// Disable special handling, interpretation, S/W flow control, \n conv.
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
tty.c_oflag &= ~OPOST;
tty.c_oflag &= ~ONLCR;

tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds)
tty.c_cc[VMIN] = sizeof(PackageOut); // Block for sizeof(PackageOut) bits

// Set the baud rate
cfsetispeed(&tty, B1152000);

// Save tty settings, also checking for error
this->port_fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);

if (this->port_fd == -1)
{
RCLCPP_ERROR(this->get_logger(), "Error %i from open: %s", errno, strerror(errno));
return false;
}
else
{
tcflush(this->port_fd, TCIOFLUSH); // Flush both input and output buffers
}

// Configure serial port settings
struct termios tty;
if (tcgetattr(this->port_fd, &tty) != 0)
{
RCLCPP_ERROR(this->get_logger(), "Error %i from tcgetattr: %s", errno, strerror(errno));
close(this->port_fd);
return false;
}

// Set baud rate
if (cfsetispeed(&tty, baud_rate) != 0 || cfsetospeed(&tty, baud_rate) != 0)
{
RCLCPP_ERROR(this->get_logger(), "Error setting baud rate: %s", strerror(errno));
close(this->port_fd);
return false;
}

// Set 8N1 (8 data bits, no parity, 1 stop bit)
tty.c_cflag &= ~PARENB; // Disable parity
tty.c_cflag &= ~CSTOPB; // Use one stop bit
tty.c_cflag &= ~CSIZE; // Clear current data size setting
tty.c_cflag |= CS8; // 8 data bits

// Disable hardware flow control
tty.c_cflag &= ~CRTSCTS;

// Enable reading and ignore modem control lines
tty.c_cflag |= CREAD | CLOCAL;

// Set raw mode (disable canonical mode, echo, signals, etc.)
cfmakeraw(&tty);

// Set blocking mode with a 1-second read timeout
tty.c_cc[VMIN] = 1; // Minimum 1 character to read
tty.c_cc[VTIME] = 10; // Timeout in deciseconds (1 second)

// Apply the configuration to the serial port
if (tcsetattr(this->port_fd, TCSANOW, &tty) != 0)
{
RCLCPP_ERROR(this->get_logger(), "Error %i from tcsetattr: %s", errno, strerror(errno));
return;
return false;
}
this->is_connected = true;
RCLCPP_INFO(this->get_logger(), "UART Connected");

return;
return true;
}

void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs::msg::PredictedArmor> msg)
Expand Down Expand Up @@ -198,11 +198,10 @@ void ControlCommunicatorNode::auto_aim_handler(const std::shared_ptr<vision_msgs
package.autoAimPackage.fire = 1;
write(this->port_fd, &package, sizeof(PackageOut));
fsync(this->port_fd);
if (auto_aim_frame_id % 1000 == 0)
if (this->auto_aim_frame_id % 1000 == 0)
{
RCLCPP_INFO(this->get_logger(), "Yaw, Pitch: %.3f, %.3f, FIRE=%i", package.autoAimPackage.yaw, package.autoAimPackage.pitch, package.autoAimPackage.fire);
}
// RCLCPP_INFO_ONCE(this->get_logger(), "First auto aim pkg sent, pkg is %i bytes", sizeof(PackageOut));
}

void ControlCommunicatorNode::nav_handler(const std::shared_ptr<geometry_msgs::msg::Twist> msg)
Expand Down Expand Up @@ -259,38 +258,20 @@ void ControlCommunicatorNode::heart_beat_handler()
}
}

bool ControlCommunicatorNode::read_alignment()
{
RCLCPP_INFO(this->get_logger(), "Attemp to alignment.");
uint8_t i = 0;
uint8_t buffer[33];
do
{
int success = read(this->port_fd, &(buffer[0]), sizeof(buffer[0]));
if (success)
{
i++;
}
} while (buffer[0] != 0xAA || i > sizeof(PackageIn) * 2);
read(this->port_fd, &buffer, sizeof(PackageIn) - 1);

return i <= sizeof(PackageIn) * 2;
}

void ControlCommunicatorNode::read_uart()
{
RCLCPP_INFO(this->get_logger(), "reading uart");
PackageIn package;
int success = read(this->port_fd, &package, sizeof(PackageIn));

rclcpp::Time curr_time = this->now();
if (success == -1)
{
RCLCPP_ERROR(this->get_logger(), "Error %i from read: %s", errno, strerror(errno));
return;
RCLCPP_INFO(this->get_logger(), "DEBUG: Nothing in buffer, err %i from read: %s", errno, strerror(errno));
return; // No data to read, try again later
}

// // print each field of package
// print each field of package
// RCLCPP_INFO(this->get_logger(), "\n");
// RCLCPP_INFO(this->get_logger(), "head: %x", package.head);
// RCLCPP_INFO(this->get_logger(), "pitch: %f", package.pitch);
Expand All @@ -306,14 +287,6 @@ void ControlCommunicatorNode::read_uart()
if (package.head != 0xAA) // Package validation
{
RCLCPP_WARN(this->get_logger(), "Packet miss aligned.");
if (this->read_alignment())
{
RCLCPP_INFO(this->get_logger(), "Read alignment success.");
}
else
{
RCLCPP_WARN(this->get_logger(), "Read alignment failed.");
}
return;
}

Expand Down Expand Up @@ -414,7 +387,7 @@ void ControlCommunicatorNode::read_uart()
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS0");
auto node = std::make_shared<ControlCommunicatorNode>("/dev/ttyTHS1");

rclcpp::spin(node);
rclcpp::shutdown();
Expand Down