--- /dev/null
+#!/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()
--- /dev/null
+#!/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()
projects / a4wd3.git / commitdiff