From 7fa5ae3fe340b2cb41cb85ebb99e0ce3fd844079 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sat, 21 Mar 2026 11:41:36 +0100
Subject: [PATCH] Display voltage, current on oled

---
 .screen-startup                              |  1 +
 CMakeLists.txt                               |  5 ++
 launch/a4wd3_launch.py                       |  8 ++-
 scripts/{oled_ssd1780.py => oled_ssd1306.py} |  0
 src/display.cpp                              | 56 ++++++++++++++++++++
 src/hw_node.cpp                              |  3 --
 6 files changed, 69 insertions(+), 4 deletions(-)
 rename scripts/{oled_ssd1780.py => oled_ssd1306.py} (100%)
 create mode 100644 src/display.cpp

diff --git a/.screen-startup b/.screen-startup
index 67f5862..1c93023 100644
--- a/.screen-startup
+++ b/.screen-startup
@@ -5,3 +5,4 @@ source $HOME/.screenrc
 
 #screen 0 zsh -is eval 'ros2 run a4wd3 hw_node --ros-args --log-level info'
 screen 0 zsh -is eval 'ros2 launch a4wd3 a4wd3_launch.py'
+screen 1 zsh -is eval 'docker_python2 /root/ros2_ws/src/a4wd3/scripts/oled_ssd1306.py "udpsrc ! rsvgdec ! videoconvert"'
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 81637a4..a0cb2d1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,8 +26,13 @@ target_include_directories(hw_node PUBLIC
 target_link_libraries(hw_node i2c)
 target_compile_features(hw_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
+add_executable(display src/display.cpp)
+ament_target_dependencies(display rclcpp sensor_msgs)
+
 install(TARGETS hw_node
   DESTINATION lib/${PROJECT_NAME})
+install(TARGETS display
+  DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params
diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py
index de16d6e..ad40653 100644
--- a/launch/a4wd3_launch.py
+++ b/launch/a4wd3_launch.py
@@ -15,6 +15,12 @@ def generate_launch_description():
 	    parameters=[{"enable_odom_tf": False}],
 	    output="screen",
         ),
+        Node(
+            package='a4wd3',
+            executable='display',
+            name='a4wd3_display',
+	    output="screen",
+        ),
         Node(
             package='bno085_uart',
             executable='bno085',
@@ -26,7 +32,7 @@ def generate_launch_description():
             package='tf2_ros',
             executable='static_transform_publisher',
             name='tf_base_imu',
-	    arguments = ['--x', '0', '--y', '0.00', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'],
+	    arguments = ['--x', '-0.02', '--y', '-0.013', '--roll', '3.1416', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'],
 	    output="screen"
         ),
         Node(
diff --git a/scripts/oled_ssd1780.py b/scripts/oled_ssd1306.py
similarity index 100%
rename from scripts/oled_ssd1780.py
rename to scripts/oled_ssd1306.py
diff --git a/src/display.cpp b/src/display.cpp
new file mode 100644
index 0000000..243503a
--- /dev/null
+++ b/src/display.cpp
@@ -0,0 +1,56 @@
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+using std::placeholders::_1;
+
+class A4wd3display : public rclcpp::Node {
+	public:
+		A4wd3display() : Node("A4WD3_display")
+		{
+			sub_bat = this->create_subscription<sensor_msgs::msg::BatteryState>("battery", 10, std::bind(&A4wd3display::battery_callback, this, _1));
+			sock = socket(AF_INET, SOCK_DGRAM, 0);
+			if (sock < 0) {
+				perror("socket");
+				exit(-1);
+			}
+			memset((char *) &addr, 0, sizeof(addr));
+			addr.sin_family = AF_INET;
+			addr.sin_port = htons(5004);
+			inet_aton("127.0.0.1", &addr.sin_addr);
+		}
+
+	private:
+		rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr sub_bat;
+		int sock;
+		struct sockaddr_in addr;
+
+		void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) const {
+			char svg[1024];
+			const char *svg_template = R"""(
+				<svg width="128" height="64" xmlns="http://www.w3.org/2000/svg">"
+					<rect width="128" height="64" x="0" y="0" style="fill:rgb(255,255,255)" />
+					<text x="0" y="15" font-family="monospace" xml:space="preserve">%5.2f V</text>
+					<text x="0" y="30" font-family="monospace" xml:space="preserve">%5.2f A</text>
+					<text x="0" y="45" font-family="monospace" xml:space="preserve"></text>
+					<text x="0" y="60" font-family="monospace" xml:space="preserve"></text>
+				</svg>
+			)""";
+
+			snprintf(svg, 1024, svg_template, msg->voltage, msg->current);
+			RCLCPP_DEBUG(this->get_logger(), "%s", svg);
+			if (sendto(sock, svg, strlen(svg), 0, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) {
+				perror("sendto");
+			}
+		}
+};
+
+int main(int argc, char * argv[]) {
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<A4wd3display>());
+	rclcpp::shutdown();
+	return 0;
+}
diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index fa13cce..081f2d8 100644
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -151,9 +151,6 @@ class A4wd3 : public rclcpp::Node {
 				RCLCPP_ERROR(this->get_logger(), "Failed to read Odometry err=%d", ret);
 				return;
 			}
-			uint8_t count;
-			const int retcount = i2c_read_reg(0x50, 0xa2, 1, &count);
-			RCLCPP_INFO(this->get_logger(), "Count: %d, ret: %d", count, retcount);
 
 			values[0].i = __bswap_32(values[0].i);
 			values[1].i = __bswap_32(values[1].i);
-- 
2.39.5