From: Erik Andresen Date: Sat, 21 Mar 2026 10:41:36 +0000 (+0100) Subject: Display voltage, current on oled X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=7fa5ae3fe340b2cb41cb85ebb99e0ce3fd844079;p=a4wd3.git Display voltage, current on oled --- 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_ssd1306.py b/scripts/oled_ssd1306.py new file mode 100755 index 0000000..f0bf6f1 --- /dev/null +++ b/scripts/oled_ssd1306.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import sys +import struct +import gi +gi.require_version('Gst', '1.0') +from gi.repository import GObject, Gst +from time import sleep +from pyshared.i2c import * + +SET_DISPLAY_ON=0xaf +SET_DISPLAY_OFF=0xa4 +SET_MULTIPLEX_RATIO=0xa8 +SET_DISPLAY_OFFSET=0xd3 +SET_COM_PINS_HARDWARE_CONFIGURATION=0xda +SET_NORMAL_DISPLAY=0xa6 +SET_INVERSE_DISPLAY=0xa7 +SET_DISPLAY_CLOCK_DIVIDE_RATIO=0xd5 +SET_MEMORY_ADDRESSING_MODE=0x20 +SET_COLUMN_ADDRESS=0x21 +SET_PAGE_ADDRESS=0x22 +SET_CONTRAST=0x81 + +def binarize(px, thres=127): + return px > thres + +class ssd1306: + def __init__(self): + Gst.init(None) + self.addr=0x7a + self.width=128 + self.height=64 + self.initialize() + + def play(self, src): + self.pipeline = Gst.parse_launch('%s ! decodebin ! videoconvert ! videoscale add-borders=true ! videorate average-period=1000000000 max-rate=4 ! video/x-raw,format=GRAY8,width=128,height=64 ! appsink' % (src)) + appsink = self.pipeline.get_by_name('appsink0') + bus = self.pipeline.get_bus() + bus.add_signal_watch() + bus.connect('message', self.on_gst_message) + appsink.set_property('emit-signals', True) + appsink.connect('new-sample', self.new_appsink_buffer) + self.pipeline.set_state(Gst.State.PLAYING) + loop = GObject.MainLoop() + loop.run() + + def new_appsink_buffer(self, appsink): + sample = appsink.emit("pull-sample") + buf = sample.get_buffer() + imgbuf = buf.extract_dup(0, buf.get_size()) + pOled.ssd1306_data(list(imgbuf)) + return Gst.FlowReturn.OK + + def on_gst_message(self, bus, message): + if message.type == Gst.MessageType.EOS: + sys.exit(0) + elif message.type == Gst.MessageType.ERROR: + err, debug = message.parse_error() + print err, debug + sys.exit(1) + elif message.type == Gst.MessageType.WARNING: + err, debug = message.parse_warning() + print err, debug + sys.exit(1) + + def ssd1306_cmd(self, ctrlbyte, databytes=[]): + data = chr(ctrlbyte) + "".join([chr(i) for i in databytes]) + i2c_write_reg(self.addr, 0b00000000, data) + + def initialize(self): + # Set MUX Ratio + self.ssd1306_cmd(SET_MULTIPLEX_RATIO, [63]) + + # Set Display Offset + self.ssd1306_cmd(SET_DISPLAY_OFFSET, [0]) + + # Set Display Start Line + self.ssd1306_cmd(0x40) + + # Set Segment re-map 0xA0/0xA1 + self.ssd1306_cmd(0xa1) + + # Set COM Output Scan Direction 0xC0/0xC8 + self.ssd1306_cmd(0xc8) + + # Set COM Pins hardware configuration + self.ssd1306_cmd(SET_COM_PINS_HARDWARE_CONFIGURATION, [0x12]) + + # Disable Entire Display On + self.ssd1306_cmd(SET_DISPLAY_OFF) + + # Set Normal Display + self.ssd1306_cmd(SET_NORMAL_DISPLAY) + + # Set Osc Frequency + self.ssd1306_cmd(SET_DISPLAY_CLOCK_DIVIDE_RATIO, [0x80]) + + # Enable charge pump regulator + self.ssd1306_cmd(0x8d, [0x14]) + self.ssd1306_cmd(0xd9, [0xf1]) + self.ssd1306_cmd(0xdb, [0x40]) + + # Misc setup + self.ssd1306_cmd(SET_MEMORY_ADDRESSING_MODE, [0x00]) + self.ssd1306_cmd(SET_CONTRAST, [0x7f]) + + # Display On + self.ssd1306_cmd(SET_DISPLAY_ON) + + self.clear() + + def ssd1306_data(self, img): + img = [ord(c) if type(c) == str else c for c in img] + thres = sum(img)/len(img) + print "Threshold=%d" % (thres) + if thres <= 31: thres=31 + elif thres > 223: thres=223 + data = [] + for y in range(0, self.height, 8): # step with page increment + for x in range(0, self.width): + px = binarize(img[(y+0)*self.width + x], thres) << 0 + px |= binarize(img[(y+1)*self.width + x], thres) << 1 + px |= binarize(img[(y+2)*self.width + x], thres) << 2 + px |= binarize(img[(y+3)*self.width + x], thres) << 3 + px |= binarize(img[(y+4)*self.width + x], thres) << 4 + px |= binarize(img[(y+5)*self.width + x], thres) << 5 + px |= binarize(img[(y+6)*self.width + x], thres) << 6 + px |= binarize(img[(y+7)*self.width + x], thres) << 7 + data.append(chr(px)) + + self.ssd1306_cmd(SET_COLUMN_ADDRESS, [0x00, 0x7f]) + self.ssd1306_cmd(SET_PAGE_ADDRESS, [0x00, 0x07]) + i2c_write_reg(self.addr, 0b01000000, "".join(data)) + + def clear(self): + img = [0x00] * (128*64) + self.ssd1306_data(img) + +if __name__ == "__main__": + pOled = ssd1306() + pOled.play(sys.argv[1]) diff --git a/scripts/oled_ssd1780.py b/scripts/oled_ssd1780.py deleted file mode 100755 index f0bf6f1..0000000 --- a/scripts/oled_ssd1780.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python -# -*- coding: iso-8859-15 -*- - -import sys -import struct -import gi -gi.require_version('Gst', '1.0') -from gi.repository import GObject, Gst -from time import sleep -from pyshared.i2c import * - -SET_DISPLAY_ON=0xaf -SET_DISPLAY_OFF=0xa4 -SET_MULTIPLEX_RATIO=0xa8 -SET_DISPLAY_OFFSET=0xd3 -SET_COM_PINS_HARDWARE_CONFIGURATION=0xda -SET_NORMAL_DISPLAY=0xa6 -SET_INVERSE_DISPLAY=0xa7 -SET_DISPLAY_CLOCK_DIVIDE_RATIO=0xd5 -SET_MEMORY_ADDRESSING_MODE=0x20 -SET_COLUMN_ADDRESS=0x21 -SET_PAGE_ADDRESS=0x22 -SET_CONTRAST=0x81 - -def binarize(px, thres=127): - return px > thres - -class ssd1306: - def __init__(self): - Gst.init(None) - self.addr=0x7a - self.width=128 - self.height=64 - self.initialize() - - def play(self, src): - self.pipeline = Gst.parse_launch('%s ! decodebin ! videoconvert ! videoscale add-borders=true ! videorate average-period=1000000000 max-rate=4 ! video/x-raw,format=GRAY8,width=128,height=64 ! appsink' % (src)) - appsink = self.pipeline.get_by_name('appsink0') - bus = self.pipeline.get_bus() - bus.add_signal_watch() - bus.connect('message', self.on_gst_message) - appsink.set_property('emit-signals', True) - appsink.connect('new-sample', self.new_appsink_buffer) - self.pipeline.set_state(Gst.State.PLAYING) - loop = GObject.MainLoop() - loop.run() - - def new_appsink_buffer(self, appsink): - sample = appsink.emit("pull-sample") - buf = sample.get_buffer() - imgbuf = buf.extract_dup(0, buf.get_size()) - pOled.ssd1306_data(list(imgbuf)) - return Gst.FlowReturn.OK - - def on_gst_message(self, bus, message): - if message.type == Gst.MessageType.EOS: - sys.exit(0) - elif message.type == Gst.MessageType.ERROR: - err, debug = message.parse_error() - print err, debug - sys.exit(1) - elif message.type == Gst.MessageType.WARNING: - err, debug = message.parse_warning() - print err, debug - sys.exit(1) - - def ssd1306_cmd(self, ctrlbyte, databytes=[]): - data = chr(ctrlbyte) + "".join([chr(i) for i in databytes]) - i2c_write_reg(self.addr, 0b00000000, data) - - def initialize(self): - # Set MUX Ratio - self.ssd1306_cmd(SET_MULTIPLEX_RATIO, [63]) - - # Set Display Offset - self.ssd1306_cmd(SET_DISPLAY_OFFSET, [0]) - - # Set Display Start Line - self.ssd1306_cmd(0x40) - - # Set Segment re-map 0xA0/0xA1 - self.ssd1306_cmd(0xa1) - - # Set COM Output Scan Direction 0xC0/0xC8 - self.ssd1306_cmd(0xc8) - - # Set COM Pins hardware configuration - self.ssd1306_cmd(SET_COM_PINS_HARDWARE_CONFIGURATION, [0x12]) - - # Disable Entire Display On - self.ssd1306_cmd(SET_DISPLAY_OFF) - - # Set Normal Display - self.ssd1306_cmd(SET_NORMAL_DISPLAY) - - # Set Osc Frequency - self.ssd1306_cmd(SET_DISPLAY_CLOCK_DIVIDE_RATIO, [0x80]) - - # Enable charge pump regulator - self.ssd1306_cmd(0x8d, [0x14]) - self.ssd1306_cmd(0xd9, [0xf1]) - self.ssd1306_cmd(0xdb, [0x40]) - - # Misc setup - self.ssd1306_cmd(SET_MEMORY_ADDRESSING_MODE, [0x00]) - self.ssd1306_cmd(SET_CONTRAST, [0x7f]) - - # Display On - self.ssd1306_cmd(SET_DISPLAY_ON) - - self.clear() - - def ssd1306_data(self, img): - img = [ord(c) if type(c) == str else c for c in img] - thres = sum(img)/len(img) - print "Threshold=%d" % (thres) - if thres <= 31: thres=31 - elif thres > 223: thres=223 - data = [] - for y in range(0, self.height, 8): # step with page increment - for x in range(0, self.width): - px = binarize(img[(y+0)*self.width + x], thres) << 0 - px |= binarize(img[(y+1)*self.width + x], thres) << 1 - px |= binarize(img[(y+2)*self.width + x], thres) << 2 - px |= binarize(img[(y+3)*self.width + x], thres) << 3 - px |= binarize(img[(y+4)*self.width + x], thres) << 4 - px |= binarize(img[(y+5)*self.width + x], thres) << 5 - px |= binarize(img[(y+6)*self.width + x], thres) << 6 - px |= binarize(img[(y+7)*self.width + x], thres) << 7 - data.append(chr(px)) - - self.ssd1306_cmd(SET_COLUMN_ADDRESS, [0x00, 0x7f]) - self.ssd1306_cmd(SET_PAGE_ADDRESS, [0x00, 0x07]) - i2c_write_reg(self.addr, 0b01000000, "".join(data)) - - def clear(self): - img = [0x00] * (128*64) - self.ssd1306_data(img) - -if __name__ == "__main__": - pOled = ssd1306() - pOled.play(sys.argv[1]) 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 + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include +#include + +using std::placeholders::_1; + +class A4wd3display : public rclcpp::Node { + public: + A4wd3display() : Node("A4WD3_display") + { + sub_bat = this->create_subscription("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::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"""( + " + + %5.2f V + %5.2f A + + + + )"""; + + 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()); + 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);