publish max44009 light sensor

diff --git a/scripts/max44009_light.py b/scripts/max44009_light.py
deleted file mode 100755 (executable)
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 cd4cc741c631f2017df5df7d1762ba33fa78ebcd..b7fba05d8a9750a67753c54214628958cf953250 100644 (file)
--- 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 <sys/socket.h>
 #include <arpa/inet.h>
 
 using std::placeholders::_1;
+using namespace std::chrono_literals;
 
 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));
+                       sub_light = this->create_subscription<sensor_msgs::msg::Illuminance>("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<sensor_msgs::msg::BatteryState>::SharedPtr sub_bat;
+               rclcpp::Subscription<sensor_msgs::msg::Illuminance>::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"""(
                                <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">%s</text>
                                        <text x="0" y="30" font-family="monospace" xml:space="preserve">%5.2f V   %5.2f A</text>
-                                       <text x="0" y="45" font-family="monospace" xml:space="preserve"></text>
+                                       <text x="0" y="45" font-family="monospace" xml:space="preserve">%.2f lx</text>
                                        <text x="0" y="60" font-family="monospace" xml:space="preserve"></text>
                                </svg>
                        )""";
 
-                       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 ef4b3f15d1e01bff88c1f64e59eb82222886512a..35c5d3cac0d72034c5a9274af72e1cf45aef7a0e 100644 (file)
--- 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 <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -97,6 +98,7 @@ class A4wd3 : public rclcpp::Node {
                A4wd3() : Node("A4WD3") {
                        pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
                        pub_bat = this->create_publisher<sensor_msgs::msg::BatteryState>("battery", 10);
+                       pub_light = this->create_publisher<sensor_msgs::msg::Illuminance>("light", 10);
                        sub_cmd_vel = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1));
                        sub_imu = this->create_subscription<sensor_msgs::msg::Imu>("imu", 10, std::bind(&A4wd3::imu_callback, this, std::placeholders::_1));
                        tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
@@ -117,6 +119,7 @@ class A4wd3 : public rclcpp::Node {
                rclcpp::TimerBase::SharedPtr timer;
                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
                rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_bat;
+               rclcpp::Publisher<sensor_msgs::msg::Illuminance>::SharedPtr pub_light;
                rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
                rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
                std::unique_ptr<tf2_ros::TransformBroadcaster> 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]);