From 5f862b4bffb0f3d692b6f726a1f8bd1c46bca6c5 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 26 Apr 2026 11:54:35 +0200 Subject: [PATCH] publish max44009 light sensor --- scripts/max44009_light.py | 25 ------------------------- src/display.cpp | 24 ++++++++++++++++++++---- src/hw_node.cpp | 27 +++++++++++++++++++++++++++ 3 files changed, 47 insertions(+), 29 deletions(-) delete mode 100755 scripts/max44009_light.py diff --git a/scripts/max44009_light.py b/scripts/max44009_light.py deleted file mode 100755 index f0b83d1..0000000 --- a/scripts/max44009_light.py +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/env tauthon -# -*- coding: iso-8859-15 -*- - -from pyshared.i2c import * -import struct -from time import sleep -from datetime import datetime - -class MAX44009: - def __init__(self): - self.addr = 0x94 - - def get_lux(self): - high, low = struct.unpack("bb", i2c_read_reg(self.addr, 3, 2)) - exponent = high>>4 - mantissa = ((high&0x0f)<<4) | low&0xf - lux = 2**exponent * mantissa * 0.045 - return lux - - -if __name__ == "__main__": - pLight = MAX44009() - while True: - print "%s: %.3f lx" % (datetime.now().strftime("%d.%m.%Y %H:%M:%S"), pLight.get_lux()) - sleep(1) diff --git a/src/display.cpp b/src/display.cpp index cd4cc74..b7fba05 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -4,16 +4,19 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/illuminance.hpp" #include #include using std::placeholders::_1; +using namespace std::chrono_literals; class A4wd3display : public rclcpp::Node { public: A4wd3display() : Node("A4WD3_display") { sub_bat = this->create_subscription("battery", 10, std::bind(&A4wd3display::battery_callback, this, _1)); + sub_light = this->create_subscription("light", 10, std::bind(&A4wd3display::light_callback, this, _1)); sock = socket(AF_INET, SOCK_DGRAM, 0); if (sock < 0) { perror("socket"); @@ -23,28 +26,41 @@ class A4wd3display : public rclcpp::Node { addr.sin_family = AF_INET; addr.sin_port = htons(5004); inet_aton("127.0.0.1", &addr.sin_addr); + timer = this->create_wall_timer(100ms, std::bind(&A4wd3display::timer_callback, this)); } private: rclcpp::Subscription::SharedPtr sub_bat; + rclcpp::Subscription::SharedPtr sub_light; int sock; struct sockaddr_in addr; + double voltage=0, current=0, illuminance=0; + rclcpp::TimerBase::SharedPtr timer; - void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) const { + void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) { + voltage = msg->voltage; + current = msg->current; + } + + void light_callback(const sensor_msgs::msg::Illuminance::SharedPtr msg) { + illuminance = msg->illuminance; + } + + void timer_callback() { char svg[1024]; const char *svg_template = R"""( " %s %5.2f V %5.2f A - + %.2f lx )"""; - const auto now = std::chrono::system_clock::now(); + const auto now = std::chrono::zoned_time("Europe/Berlin", std::chrono::system_clock::now()); std::string timestr = std::format("{:%d.%m.%y %H:%M:%OS}", now); - snprintf(svg, 1024, svg_template, timestr.c_str(), msg->voltage, msg->current); + snprintf(svg, 1024, svg_template, timestr.c_str(), voltage, current, illuminance); RCLCPP_DEBUG(this->get_logger(), "%s", svg); if (sendto(sock, svg, strlen(svg), 0, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) { perror("sendto"); diff --git a/src/hw_node.cpp b/src/hw_node.cpp index ef4b3f1..35c5d3c 100644 --- a/src/hw_node.cpp +++ b/src/hw_node.cpp @@ -9,6 +9,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/illuminance.hpp" #include #include #include @@ -97,6 +98,7 @@ class A4wd3 : public rclcpp::Node { A4wd3() : Node("A4WD3") { pub_odom = this->create_publisher("odom", 10); pub_bat = this->create_publisher("battery", 10); + pub_light = this->create_publisher("light", 10); sub_cmd_vel = this->create_subscription("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1)); sub_imu = this->create_subscription("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1)); tf_broadcaster = std::make_unique(*this); @@ -117,6 +119,7 @@ class A4wd3 : public rclcpp::Node { rclcpp::TimerBase::SharedPtr timer; rclcpp::Publisher::SharedPtr pub_odom; rclcpp::Publisher::SharedPtr pub_bat; + rclcpp::Publisher::SharedPtr pub_light; rclcpp::Subscription::SharedPtr sub_cmd_vel; rclcpp::Subscription::SharedPtr sub_imu; std::unique_ptr tf_broadcaster; @@ -282,12 +285,36 @@ class A4wd3 : public rclcpp::Node { pub_bat->publish(msg); } + void update_light() { + uint8_t val[2]; + int ret = i2c_read_reg(0x94, 0x03, 2, val); + if (ret != 2) { + RCLCPP_ERROR(this->get_logger(), "Failed to read MAX44009 light err=%d", ret); + return; + } + uint8_t high = val[0]; + uint8_t low = val[1]; + + if ((high & 0xf0) == 0xf0) { // overrange + return; + } + uint8_t exponent = high>>4; + uint8_t mantissa = ((high&0x0f)<<4) | (low&0xf); + double lux = pow(2, exponent) * mantissa * 0.045; + + auto msg = sensor_msgs::msg::Illuminance(); + msg.header.stamp = this->get_clock()->now(); + msg.illuminance = lux*2.0183-5.2079; + pub_light->publish(msg); + } + void timer_callback() { if (imu_received) { // Wait until IMU node is ready before we publish odometry so robot_localization will get correct orientation first update_odom(); } update_pwr(); + update_light(); if (!cmd_vel.empty()) { set_speed(cmd_vel[0], cmd_vel[1]); -- 2.39.5