From f36a9bdff3400a4347c7d560b0cf21cf1b0dd500 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 20 Sep 2026 19:39:06 +0200 Subject: [PATCH] Add temperature scripts --- CMakeLists.txt | 1 + launch/a4wd3_launch.py | 12 +++ scripts/bme_ambient_pub | 176 ++++++++++++++++++++++++++++++++++++++++ scripts/cpu_temp_pub.py | 80 ++++++++++++++++++ src/hw_node.cpp | 8 +- 5 files changed, 275 insertions(+), 2 deletions(-) create mode 100755 scripts/bme_ambient_pub create mode 100755 scripts/cpu_temp_pub.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5084c1a..bd8f72a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,6 +56,7 @@ install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS scripts/cpu_temp_pub.py DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py index 1d5c369..72293ac 100644 --- a/launch/a4wd3_launch.py +++ b/launch/a4wd3_launch.py @@ -149,4 +149,16 @@ def generate_launch_description(): parameters=[{"i2c_bus": 7}], output="screen", ), + Node( + package='multichannel_gas_sensor', + executable='sensor_node', + name='multichannel_gas_sensor', + output="screen", + ), + Node( + package='a4wd3', + executable='cpu_temp_pub.py', + name='cpu_temp_pub', + output="screen", + ), ]) diff --git a/scripts/bme_ambient_pub b/scripts/bme_ambient_pub new file mode 100755 index 0000000..a32b98d --- /dev/null +++ b/scripts/bme_ambient_pub @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +"""bme_ambient_pub.py ? run ON THE ROBOT. Publish the BME680 chassis reading +corrected to AMBIENT / room temperature using the 2026-09-05 fitted model. + +Inputs: + * BME680 (chassis temp): ROS topic, because the sensor is an external AVR + over I2C and only appears on the bus as /bme680/temperature. + * SoC (CPU) temp: read DIRECTLY from sysfs on the robot (no rosbridge / + cpu_temp_pub dependency): + /sys/devices/virtual/thermal/thermal_zone0/hwmon0/temp1_input + (millidegrees; a value >1000 is treated as milli and divided by 1000) + Fallback: /sys/class/thermal/thermal_zone0/temp + +Why: the BME680 sits in the closed base and reads chassis heat (CPU+ambient), +not room air. Invert the thermal model to recover the ambient temperature: + + Model (steady state): T_bme = T_room + k*(T_cpu - T_room) + Inverted for ambient: T_room = (T_bme - k*T_cpu) / (1 - k) + +k is NOT constant. Fitted at two steady states (room = 25.0 C): + idle : T_cpu 56.4 C -> k 0.216 + hot : T_cpu 84.9 C -> k 0.189 +k is linearly interpolated against the current CPU temp, clamped at the ends. + +CAVEAT: the BME is a slow sensor (thermal time constant ~51 min). The +correction is only valid NEAR STEADY STATE; during a CPU load ramp the BME +lags by ~30-60 min so the value is temporarily wrong while heating/cooling. +Fine as a room-temperature monitor; not a fast sensor. + +Jazzy, single file, no deps beyond rclpy + sensor_msgs. +Run: source /opt/ros/jazzy/setup.bash && python3 bme_ambient_pub.py +Params (ros params, defaults shown): + bme_topic default /bme680/temperature + pub_topic default /bme680/temperature_ambient + cpu_sysfs_paths default "..." (comma-separated, first readable wins) + k_idle_cpu, k_idle, k_hot_cpu, k_hot (calibration) + min_stale_s ignore BME if not updated in this many s (default 10) + publish_hz default 1.0 +""" +import math +import os + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from sensor_msgs.msg import Temperature + +# ---- pure math (no rclpy; unit-testable) ----------------------------------- +def k_of_cpu(t_cpu, idle_cpu, idle_k, hot_cpu, hot_k): + """Linearly interpolate k between the two fitted steady states, clamped.""" + if t_cpu <= idle_cpu: + return idle_k + if t_cpu >= hot_cpu: + return hot_k + f = (t_cpu - idle_cpu) / (hot_cpu - idle_cpu) + return idle_k + f * (hot_k - idle_k) + + +def ambient_from(t_bme, t_cpu, k): + """Invert T_bme = T_room + k*(T_cpu - T_room) for T_room.""" + if k >= 1.0: # degenerate; never happens with our k ~0.2 + return None + return (t_bme - k * t_cpu) / (1.0 - k) + + +def read_cpu_from_sysfs(paths): + """Read the SoC temperature (C) from the first readable sysfs path. + Returns float or None. Handles both millidegrees and degrees raw files. + """ + for p in paths: + try: + with open(p) as f: + raw = f.read().strip() + val = float(raw) + # Allwinner D1 reports millidegrees (e.g. 56450) -> /1000. + if val > 1000.0: + val = val / 1000.0 + return val + except (FileNotFoundError, ValueError, OSError): + continue + return None + + +# ---- node ------------------------------------------------------------------ +class BmeAmbientPub(Node): + def __init__(self): + super().__init__('bme_ambient_pub') + + self.declare_parameter('bme_topic', '/bme680/temperature') + self.declare_parameter('pub_topic', '/bme680/temperature_ambient') + self.declare_parameter( + 'cpu_sysfs_paths', + '/sys/devices/virtual/thermal/thermal_zone0/hwmon0/temp1_input,' + '/sys/class/thermal/thermal_zone0/temp') + self.declare_parameter('k_idle_cpu', 56.4) + self.declare_parameter('k_idle', 0.216) + self.declare_parameter('k_hot_cpu', 84.9) + self.declare_parameter('k_hot', 0.189) + self.declare_parameter('min_stale_s', 10.0) + self.declare_parameter('publish_hz', 1.0) + + bme_topic = self.get_parameter('bme_topic').value + pub_topic = self.get_parameter('pub_topic').value + self.min_stale_s = self.get_parameter('min_stale_s').value + publish_hz = self.get_parameter('publish_hz').value + self.idle_cpu = self.get_parameter('k_idle_cpu').value + self.idle_k = self.get_parameter('k_idle').value + self.hot_cpu = self.get_parameter('k_hot_cpu').value + self.hot_k = self.get_parameter('k_hot').value + + self._cpu_paths = [p for p in + self.get_parameter('cpu_sysfs_paths').value.split(',') + if p.strip()] + + self._bme = None + self._bme_t = None + self._log_count = 0 + + qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST) + self._pub = self.create_publisher(Temperature, pub_topic, qos) + self.create_subscription(Temperature, bme_topic, self._bme_cb, qos) + self.create_timer(1.0 / max(publish_hz, 0.05), self._tick) + + self.get_logger().info( + f"bme_ambient_pub: {bme_topic} + sysfs -> {pub_topic} " + f"[k {self.idle_k}@{self.idle_cpu}C .. {self.hot_k}@{self.hot_cpu}C]") + + def _bme_cb(self, msg): + self._bme = msg.temperature + self._bme_t = self.get_clock().now() + + def _seconds_since(self, t): + if t is None: + return math.inf + return (self.get_clock().now() - t).nanoseconds / 1e9 + + def _tick(self): + if self._bme is None or self._seconds_since(self._bme_t) > self.min_stale_s: + return # no BME, or stale (bridge gap / node off) -> hold last + t_cpu = read_cpu_from_sysfs(self._cpu_paths) + if t_cpu is None: + return # can't read SoC right now + + k = k_of_cpu(t_cpu, self.idle_cpu, self.idle_k, self.hot_cpu, self.hot_k) + t_room = ambient_from(self._bme, t_cpu, k) + if t_room is None: + return + + m = Temperature() + m.header.stamp = self.get_clock().now().to_msg() + m.header.frame_id = 'bme680_ambient' + m.temperature = t_room + self._pub.publish(m) + + # reduced-rate log for verification (~every 60s at 1 Hz) + self._log_count += 1 + if self._log_count % 60 == 1: + self.get_logger().info( + f"ambient {t_room:.2f} C (bme {self._bme:.2f}, cpu {t_cpu:.2f}, k {k:.3f})") + + +def main(): + rclpy.init() + node = BmeAmbientPub() + try: + rclpy.spin(node) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/scripts/cpu_temp_pub.py b/scripts/cpu_temp_pub.py new file mode 100755 index 0000000..c8bb3e8 --- /dev/null +++ b/scripts/cpu_temp_pub.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +"""cpu_temp_pub.py ? publish the Banana Pi M7 SoC temperature as a ROS 2 topic. + +Copy to the robot (192.168.36.21) and run with the ROS 2 Jazzy python env: + + source /opt/ros/jazzy/setup.bash + python3 cpu_temp_pub.py + +Publishes /cpu/temperature (sensor_msgs/Temperature) at 1 Hz with the +sensor_data QoS profile, matching the BME680 driver. +""" +import sys +import time + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +from sensor_msgs.msg import Temperature + +# Primary path (Allwinner D1 / Banana Pi M7); kernel thermal zone as fallback. +PATHS = [ + "/sys/devices/virtual/thermal/thermal_zone0/hwmon0/temp1_input", + "/sys/class/thermal/thermal_zone0/temp", +] + + +def read_cpu_c() -> float | None: + for p in PATHS: + try: + with open(p) as f: + raw = float(f.read().strip()) + except (OSError, ValueError): + continue + # Allwinner thermal zones report millidegrees; scale defensively. + if raw > 1000: + raw /= 1000.0 + return raw + return None + + +class CpuTempNode(Node): + def __init__(self): + super().__init__("cpu_temp_pub") + self.pub = self.create_publisher( + Temperature, + "/cpu/temperature", + QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + depth=10, + ), + ) + self.last_path = None + self.create_timer(1.0, self.tick) + + def tick(self): + t = read_cpu_c() + if t is None: + return + msg = Temperature() + msg.header.stamp = self.get_clock().now().to_msg() + msg.temperature = t + self.pub.publish(msg) + + +def main(): + rclpy.init() + node = CpuTempNode() + t = read_cpu_c() + node.get_logger().info(f"/cpu/temperature publishing; SoC temp now {t} C") + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/hw_node.cpp b/src/hw_node.cpp index 77343a1..e0cc222 100644 --- a/src/hw_node.cpp +++ b/src/hw_node.cpp @@ -64,9 +64,11 @@ int i2c_write_reg(const uint8_t addr, const uint8_t command, const std::vector 0) { + fprintf(stderr, "addr=0x%x\n", addr); close(file); } - throw -1; + //throw -1; + return ret; } int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length, uint8_t *values) { @@ -91,9 +93,11 @@ int i2c_read_reg(const uint8_t addr, const uint8_t command, const uint8_t length error: if (file > 0) { + fprintf(stderr, "addr=0x%x\n", addr); close(file); } - throw -1; + //throw -1; + return ret; } class A4wd3 : public rclcpp::Node { -- 2.39.5