[Issue #8] - adding MQTT reconnect if the connection drops

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2026 gballan. 
+Copyright (c) 2026 Bit Goblin. 
 
 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 

README.md

@@ -1,18 +1,43 @@
 # Pico Weather
 
 ![Woodpecker CI badge](https://builds.metaunix.net/api/badges/92/status.svg)
+![shields.io version badge](https://img.shields.io/gitea/v/release/BitGoblin/pico-weather?gitea_url=https://git.metaunix.net)
 
 Raspberry Pi Pico app to read BME280 sensor data and send it to Home Assistant.
 
 Made in Go with [TinyGo](https://tinygo.org/).
 
-## Requirements
+## Hardware
+
+The following hardware is what I used for a deployment:
+
+* Raspberry Pi Pico 2 W: https://www.amazon.com/dp/B0DRJXPPWL
+  * The **W** is VERY important, as without networking it can't reach Home Assistant!
+  * Also a Raspberry Pi Pico W *should* work too, but I haven't tested it yet
+* Waveshare Bosch BME280 sensor: https://www.amazon.com/dp/B07Q47DFXS
+* Elegoo SSD1306 LCD: https://www.amazon.com/dp/B0FSRQG23K
+
+Most (or all?) of these components can be swapped for equivalent components. You just need to make sure the code is compatible with whatever board you choose (not likely), is a genuine BME280 sensor, and an SSD1306 display.
+
+Support for other hardware types may come in the future, but if there's a specific sensor or board you're in submit an issue or pull request!
+
+### Prototyping
+
+To help with debugging/testing deployments, I also used these components:
+
+* Elegoo 400-pin Breadboard: https://www.amazon.com/dp/B01EV640I6
+* Freenove Raspberry Pi Pico Breakout Board: https://www.amazon.com/dp/B0BFB53Y2N
+* Elegoo 20cm Jumper Cables 120 count: https://www.amazon.com/dp/B01EV70C78
+
+## Software Deployment
+
+### Requirements
 
 * [TinyGo](https://tinygo.org/getting-started/install/) - TinyGo installation guide
 
-## Deployment
+### Steps
 
-1. Clone this git repository `git clone https://git.metaunix.net/gballan/pico-weather`.
+1. Clone this git repository `git clone https://git.metaunix.net/BitGoblin/pico-weather`.
 2. Copy `config.go.example` to `config.go` and modify the values to match your environment.
 3. Hold down the **BOOTSEL** button on your Raspberry Pi Pico 2W.
 4. Plug in your Pico to a USB port on your PC.

config.go.example

@@ -1,9 +1,20 @@
 package main
 
+import (
+	"time"
+)
+
 const (
 	WifiSSID = "My Wifi Name"
 	WifiPass = "SecretWifiPassword"
 
+	// Networking settings
+	Hostname = "pico2w-bme280"
+	EnableICMP = true
+
+	// Sensor configuration
+	SensorType = "bme280"
+
 	MQTTHost = "192.168.1.50" // Must be an IP address; DNS resolution is not supported currently
 	MQTTPort = "1883"
 	MQTTUser = "" // Leave empty if not required
@@ -12,4 +23,6 @@ const (
 	NodeName = "Studio Weather"
 	NodeID   = "studio_env"
 	ClientID = "pico2w_studio_env"
+
+	UpdateInterval = 30 * time.Second
 )

go.mod

@@ -1,13 +1,16 @@
-module git.metaunix.net/gballan/pico-weather
+module git.metaunix.net/BitGoblin/pico-weather
 
 go 1.25.2
 
 require (
 	github.com/soypat/cyw43439 v0.1.1
 	github.com/soypat/lneto v0.3.2
+	tinygo.org/x/drivers v0.36.0
+	tinygo.org/x/tinyfont v0.7.0
 )
 
 require (
+	github.com/google/shlex v0.0.0-20191202100458-e7afc7fbc510 // indirect
 	github.com/soypat/seqs v0.0.0-20250124201400-0d65bc7c1710 // indirect
 	github.com/tinygo-org/pio v0.3.0 // indirect
 	golang.org/x/exp v0.0.0-20241204233417-43b7b7cde48d // indirect

go.sum

@@ -1,3 +1,5 @@
+github.com/google/shlex v0.0.0-20191202100458-e7afc7fbc510 h1:El6M4kTTCOh6aBiKaUGG7oYTSPP8MxqL4YI3kZKwcP4=
+github.com/google/shlex v0.0.0-20191202100458-e7afc7fbc510/go.mod h1:pupxD2MaaD3pAXIBCelhxNneeOaAeabZDe5s4K6zSpQ=
 github.com/soypat/cyw43439 v0.1.1 h1:vcaTiVzfuz3keK7lJpVxStZ6tV8HCw7Ugzsh1k4mneE=
 github.com/soypat/cyw43439 v0.1.1/go.mod h1:R2uSILRwSPmcmmKy5Z0FtK4ypgiPf5YqK+F+IKmXqxc=
 github.com/soypat/lneto v0.3.2 h1:iUFeRSq2czT7Db6MMOsAnMCBlKCqvIr941zsNf9dcu0=
@@ -8,3 +10,7 @@ github.com/tinygo-org/pio v0.3.0 h1:opEnOtw58KGB4RJD3/n/Rd0/djYGX3DeJiXLI6y/yDI=
 github.com/tinygo-org/pio v0.3.0/go.mod h1:wf6c6lKZp+pQOzKKcpzchmRuhiMc27ABRuo7KVnaMFU=
 golang.org/x/exp v0.0.0-20241204233417-43b7b7cde48d h1:0olWaB5pg3+oychR51GUVCEsGkeCU/2JxjBgIo4f3M0=
 golang.org/x/exp v0.0.0-20241204233417-43b7b7cde48d/go.mod h1:qj5a5QZpwLU2NLQudwIN5koi3beDhSAlJwa67PuM98c=
+tinygo.org/x/drivers v0.36.0 h1:F0x342A6GWqh6abtCa57uAxCyz/b9MbGzvIVvIf+gpE=
+tinygo.org/x/drivers v0.36.0/go.mod h1:DQgKyHkB4G6IEOKVTAjApbKnWGwESN91EVJO+nMOE9Y=
+tinygo.org/x/tinyfont v0.7.0 h1:Ju901eGRThlHLoti9K7myMYyv4phd3I0DrJwc14NTTU=
+tinygo.org/x/tinyfont v0.7.0/go.mod h1:onflMSkpWl7r7j4MIqhPEVV39pn7yL4N3MOePl3G+G8=

main.go

@@ -1,14 +1,17 @@
 package main
 
 import (
-	"bytes"
 	"context"
-	"encoding/binary"
 	"fmt"
+	"image/color"
 	"io"
 	"machine"
 	"strconv"
 	"time"
+
+	"tinygo.org/x/drivers/ssd1306"
+	"tinygo.org/x/tinyfont"
+	"tinygo.org/x/tinyfont/proggy"
 )
 
 var StateTopic = fmt.Sprintf("homeassistant/sensor/%s/state", NodeID)
@@ -33,8 +36,8 @@ func (m *SimpleMQTT) Connect(clientID, username, password string) error {
 		flags |= 0x40
 	}
 
-	// Variable header: MQTT (proto name), Level 4 (MQTT 3.1.1), flags, KeepAlive 60s
-	varHeader := []byte{0x00, 0x04, 'M', 'Q', 'T', 'T', 0x04, flags, 0x00, 0x3C}
+	// Variable header: MQTT (proto name), Level 4 (MQTT 3.1.1), flags, KeepAlive 0s (disabled)
+	varHeader := []byte{0x00, 0x04, 'M', 'Q', 'T', 'T', 0x04, flags, 0x00, 0x00}
 
 	payload := encodeString(clientID)
 	if username != "" {
@@ -98,151 +101,6 @@ func encodeLength(length int) []byte {
 	return encoded
 }
 
-// -------------------------------------------------------------
-// BME280 Driver
-// -------------------------------------------------------------
-const (
-	BME280Addr  = 0x77
-	regCalib00  = 0x88
-	regCalib26  = 0xE1
-	regReset    = 0xE0
-	regCtrlHum  = 0xF2
-	regCtrlMeas = 0xF4
-	regConfig   = 0xF5
-	regData     = 0xF7
-)
-
-type BME280Calib struct {
-	digT1 uint16
-	digT2 int16
-	digT3 int16
-	digP1 uint16
-	digP2 int16
-	digP3 int16
-	digP4 int16
-	digP5 int16
-	digP6 int16
-	digP7 int16
-	digP8 int16
-	digP9 int16
-	digH1 uint8
-	digH2 int16
-	digH3 uint8
-	digH4 int16
-	digH5 int16
-	digH6 int8
-}
-
-type BME280 struct {
-	bus   *machine.I2C
-	addr  uint8
-	calib BME280Calib
-}
-
-func NewBME280(bus *machine.I2C, addr uint8) (*BME280, error) {
-	b := &BME280{bus: bus, addr: addr}
-	if err := bus.WriteRegister(addr, regReset, []byte{0xB6}); err != nil {
-		return nil, err
-	}
-	time.Sleep(100 * time.Millisecond)
-
-	if err := b.readCalibration(); err != nil {
-		return nil, err
-	}
-	if err := bus.WriteRegister(addr, regCtrlHum, []byte{0x01}); err != nil {
-		return nil, err
-	}
-	if err := bus.WriteRegister(addr, regCtrlMeas, []byte{0x27}); err != nil {
-		return nil, err
-	}
-	if err := bus.WriteRegister(addr, regConfig, []byte{0xA0}); err != nil {
-		return nil, err
-	}
-	return b, nil
-}
-
-func (b *BME280) readCalibration() error {
-	var buf24 [24]byte
-	if err := b.bus.ReadRegister(b.addr, regCalib00, buf24[:]); err != nil {
-		return err
-	}
-	r := bytes.NewReader(buf24[:])
-	binary.Read(r, binary.LittleEndian, &b.calib.digT1)
-	binary.Read(r, binary.LittleEndian, &b.calib.digT2)
-	binary.Read(r, binary.LittleEndian, &b.calib.digT3)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP1)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP2)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP3)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP4)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP5)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP6)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP7)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP8)
-	binary.Read(r, binary.LittleEndian, &b.calib.digP9)
-
-	var h1 [1]byte
-	if err := b.bus.ReadRegister(b.addr, 0xA1, h1[:]); err != nil {
-		return err
-	}
-	b.calib.digH1 = h1[0]
-
-	var hBuf [7]byte
-	if err := b.bus.ReadRegister(b.addr, regCalib26, hBuf[:]); err != nil {
-		return err
-	}
-	b.calib.digH2 = int16(binary.LittleEndian.Uint16(hBuf[0:2]))
-	b.calib.digH3 = hBuf[2]
-	b.calib.digH4 = (int16(hBuf[3]) << 4) | (int16(hBuf[4]) & 0x0F)
-	b.calib.digH5 = (int16(hBuf[5]) << 4) | (int16(hBuf[4]) >> 4)
-	b.calib.digH6 = int8(hBuf[6])
-	return nil
-}
-
-func (b *BME280) ReadValues() (float64, float64, float64, error) {
-	var raw [8]byte
-	if err := b.bus.ReadRegister(b.addr, regData, raw[:]); err != nil {
-		return 0, 0, 0, err
-	}
-
-	rawP := (int32(raw[0]) << 12) | (int32(raw[1]) << 4) | (int32(raw[2]) >> 4)
-	rawT := (int32(raw[3]) << 12) | (int32(raw[4]) << 4) | (int32(raw[5]) >> 4)
-	rawH := (int32(raw[6]) << 8) | int32(raw[7])
-
-	var1 := (((rawT >> 3) - (int32(b.calib.digT1) << 1)) * int32(b.calib.digT2)) >> 11
-	var2 := (((((rawT >> 4) - int32(b.calib.digT1)) * ((rawT >> 4) - int32(b.calib.digT1))) >> 12) * int32(b.calib.digT3)) >> 14
-	tFine := var1 + var2
-	temp := float64((tFine*5+128)>>8) / 100.0
-
-	pVar1 := int64(tFine) - 128000
-	pVar2 := pVar1*pVar1*int64(b.calib.digP6) + ((pVar1 * int64(b.calib.digP5)) << 17) + (int64(b.calib.digP4) << 35)
-	pVar1 = ((pVar1 * pVar1 * int64(b.calib.digP3)) >> 8) + ((pVar1 * int64(b.calib.digP2)) << 12)
-	pVar1 = (((int64(1) << 47) + pVar1) * int64(b.calib.digP1)) >> 33
-
-	var pres float64
-	if pVar1 != 0 {
-		p := int64(1048576 - rawP)
-		p = (((p << 31) - pVar2) * 3125) / pVar1
-		pVar1 = (int64(b.calib.digP9) * (p >> 13) * (p >> 13)) >> 25
-		pVar2 = (int64(b.calib.digP8) * p) >> 19
-		p = ((p + pVar1 + pVar2) >> 8) + (int64(b.calib.digP7) << 4)
-		pres = (float64(p) / 256.0) / 100.0
-	}
-
-	hVar := tFine - 76800
-	hVar = (((((rawH << 14) - (int32(b.calib.digH4) << 20) - (int32(b.calib.digH5) * hVar)) + 16384) >> 15) *
-		(((((((hVar * int32(b.calib.digH6)) >> 10) * (((hVar * int32(b.calib.digH3)) >> 11) + 32768)) >> 10) + 2097152)*
-			int32(b.calib.digH2) + 8192) >> 14))
-	hVar = hVar - (((((hVar >> 15) * (hVar >> 15)) >> 7) * int32(b.calib.digH1)) >> 4)
-	if hVar < 0 {
-		hVar = 0
-	} else if hVar > 419430400 {
-		hVar = 419430400
-	}
-	hum := float64(hVar>>12) / 1024.0
-
-	return temp, pres, hum, nil
-}
-
 // -------------------------------------------------------------
 // Home Assistant Auto-Discovery
 // -------------------------------------------------------------
@@ -254,14 +112,23 @@ type haSensorDef struct {
 	valTpl string
 }
 
-func registerHADiscovery(mqtt *SimpleMQTT) error {
-	deviceJSON := fmt.Sprintf(`"device":{"identifiers":["%s"],"name":"%s","model":"Pico 2 W","manufacturer":"Raspberry Pi"}`,
-		NodeID, NodeName)
+func registerHADiscovery(mqtt *SimpleMQTT, sensor ClimateSensor) error {
+	deviceJSON := fmt.Sprintf(`"device":{"identifiers":["%s"],"name":"%s","model":"Pico 2 W (%s)","manufacturer":"Raspberry Pi"}`,
+		NodeID, NodeName, sensor.Name())
 
 	sensors := []haSensorDef{
 		{"temperature", "Temperature", "°C", "temperature", "{{ value_json.temperature }}"},
 		{"humidity", "Humidity", "%", "humidity", "{{ value_json.humidity }}"},
-		{"pressure", "Pressure", "hPa", "atmospheric_pressure", "{{ value_json.pressure }}"},
+	}
+
+	if sensor.Name() == "BME280" {
+		sensors = append(sensors, haSensorDef{
+			id:     "pressure",
+			name:   "Pressure",
+			unit:   "hPa",
+			class:  "atmospheric_pressure",
+			valTpl: "{{ value_json.pressure }}",
+		})
 	}
 
 	for _, s := range sensors {
@@ -278,7 +145,7 @@ func registerHADiscovery(mqtt *SimpleMQTT) error {
 }
 
 // -------------------------------------------------------------
-// Panic Handler & Entry Point
+// Panic Handler & Helper Functions
 // -------------------------------------------------------------
 func panicErr(msg string, err error) {
 	if err != nil {
@@ -289,63 +156,125 @@ func panicErr(msg string, err error) {
 	}
 }
 
+func connectMQTT(netStack *NetworkStack, port uint16) (*SimpleMQTT, error) {
+	ctx, cancel := context.WithTimeout(context.Background(), 10*time.Second)
+	defer cancel()
+
+	conn, err := netStack.DialTCP(ctx, MQTTHost, port)
+	if err != nil {
+		return nil, err
+	}
+
+	mqtt := NewSimpleMQTT(conn)
+	if err := mqtt.Connect(ClientID, MQTTUser, MQTTPass); err != nil {
+		return nil, err
+	}
+	return mqtt, nil
+}
+
 func main() {
 	time.Sleep(2 * time.Second)
 
-	// Configure I2C (Pin 16 SDA, Pin 17 SCL -> I2C0)
+	// Configure I2C0 bus
 	i2c := machine.I2C0
 	err := i2c.Configure(machine.I2CConfig{
-		Frequency: 100 * machine.KHz,
-		SDA:       machine.GPIO16,
-		SCL:       machine.GPIO17,
+		Frequency: 400 * machine.KHz,
+		SDA:       machine.GPIO4,
+		SCL:       machine.GPIO5,
 	})
 	if err != nil {
 		panicErr("I2C configuration", err)
 	}
 
-	bme, err := NewBME280(i2c, BME280Addr)
-	if err != nil {
-		panicErr("BME280 init", err)
+	// Instantiate nil oled variable
+	var oled *ssd1306.Device
+	white := color.RGBA{R: 255, G: 255, B: 255, A: 255}
+
+	var dummy [1]byte
+	if err := i2c.ReadRegister(0x3C, 0x00, dummy[:]); err == nil {
+		println("SSD1306 detected at 0x3C")
+		dev := ssd1306.NewI2C(i2c)
+		dev.Configure(ssd1306.Config{
+			Address: 0x3C,
+			Width:   128,
+			Height:  64,
+		})
+		oled = dev
+
+		oled.ClearBuffer()
+		tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 16, "Booting...", white)
+		oled.Display()
+	} else {
+		println("SSD1306 not found — running headless")
 	}
 
+	// Initialize the configured sensor
+	sensor, err := InitSensor(i2c, SensorType)
+	if err != nil {
+		panicErr("Sensor init", err)
+	}
+	fmt.Printf("Started climate monitor using: %s\n", sensor.Name())
+
 	netStack, err := InitNetwork(WifiSSID, WifiPass)
 	if err != nil {
 		panicErr("network init", err)
 	}
 
 	portNum, _ := strconv.Atoi(MQTTPort)
-	ctx, cancel := context.WithTimeout(context.Background(), 10*time.Second)
-	defer cancel()
 
-	conn, err := netStack.DialTCP(ctx, MQTTHost, uint16(portNum))
-	if err != nil {
-		panicErr("TCP dial", err)
-	}
-
-	mqtt := NewSimpleMQTT(conn)
 	println("Connecting MQTT session...")
-	if err := mqtt.Connect(ClientID, MQTTUser, MQTTPass); err != nil {
+	mqtt, err := connectMQTT(netStack, uint16(portNum))
+	if err != nil {
 		panicErr("MQTT connect", err)
 	}
 	println("MQTT connected.")
 
-	if err := registerHADiscovery(mqtt); err != nil {
+	if err := registerHADiscovery(mqtt, sensor); err != nil {
 		println("Warning: HA discovery failed:", err.Error())
 	}
 
 	for {
-		t, p, h, err := bme.ReadValues()
+		t, p, h, hasPres, err := sensor.ReadValues()
 		if err != nil {
-			println("BME280 read error:", err.Error())
+			println(fmt.Sprintf("%s read error: %s", sensor.Name(), err.Error()))
 		} else {
-			stateJSON := fmt.Sprintf(`{"temperature":%.2f,"humidity":%.2f,"pressure":%.2f}`, t, h, p)
+			var stateJSON string
+			if hasPres {
+				stateJSON = fmt.Sprintf(`{"temperature":%.2f,"humidity":%.2f,"pressure":%.2f}`, t, h, p)
+			} else {
+				stateJSON = fmt.Sprintf(`{"temperature":%.2f,"humidity":%.2f}`, t, h)
+			}
+
 			if err := mqtt.Publish(StateTopic, []byte(stateJSON), false); err != nil {
-				println("MQTT publish error:", err.Error())
+				println("MQTT publish error:", err.Error(), "- attempting reconnect...")
+				newMqtt, recErr := connectMQTT(netStack, uint16(portNum))
+				if recErr != nil {
+					println("MQTT reconnect failed:", recErr.Error())
+				} else {
+					println("MQTT reconnected successfully.")
+					mqtt = newMqtt
+					_ = registerHADiscovery(mqtt, sensor)
+					_ = mqtt.Publish(StateTopic, []byte(stateJSON), false)
+				}
 			} else {
 				println(fmt.Sprintf("[%s] Published: %s", NodeName, stateJSON))
 			}
+
+			if oled != nil {
+				oled.ClearBuffer()
+				tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 14, fmt.Sprintf("Node: %s", NodeName), white)
+				tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 30, fmt.Sprintf("Temp: %.1f C", t), white)
+				tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 46, fmt.Sprintf("Hum:  %.1f %%", h), white)
+
+				if hasPres {
+					tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 60, fmt.Sprintf("Pres: %.1f hPa", p), white)
+				} else {
+					tinyfont.WriteLine(oled, &proggy.TinySZ8pt7b, 0, 60, fmt.Sprintf("Sens: %s", sensor.Name()), white)
+				}
+				oled.Display()
+			}
 		}
 
-		time.Sleep(30 * time.Second)
+		time.Sleep(UpdateInterval)
 	}
 }

network.go

@@ -115,16 +115,20 @@ func InitNetwork(ssid, pass string) (*NetworkStack, error) {
 
 	stack := &xnet.StackAsync{}
 	err = stack.Reset(xnet.StackConfig{
-		Hostname:          "pico2w-bme280",
+		Hostname:          Hostname,
 		RandSeed:          time.Now().UnixNano(),
 		MaxActiveTCPPorts: 2,
 		MTU:               uint16(framelen - ethernet.MaxOverheadSize),
 		HardwareAddress:   hwaddr,
+		ICMPQueueLimit:    4,
 	})
 	if err != nil {
 		return nil, fmt.Errorf("stack config reset: %w", err)
 	}
 
+	// Enable network pings if configured
+  _ = stack.EnableICMP(EnableICMP)
+
 	// Start background frame pump
 	ctx := context.Background()
 	go stackLoop(ctx, stack, adapter)

sensor.go

@@ -0,0 +1,106 @@
+package main
+
+import (
+	"errors"
+	"machine"
+  "time"
+
+	"tinygo.org/x/drivers/bme280"
+	"tinygo.org/x/drivers/sht4x"
+)
+
+type ClimateSensor interface {
+	ReadValues() (temp float64, pres float64, hum float64, hasPres bool, err error)
+	Name() string
+}
+
+// -------------------------------------------------------------
+// BME280 Adapter
+// -------------------------------------------------------------
+type BME280Adapter struct {
+	dev bme280.Device
+}
+
+func (b *BME280Adapter) ReadValues() (float64, float64, float64, bool, error) {
+	// ReadTemperature returns milli-°C (e.g. 25000 = 25.00°C)
+	tMilli, err := b.dev.ReadTemperature()
+	if err != nil {
+		return 0, 0, 0, false, err
+	}
+
+	// ReadPressure returns milli-Pa (divide by 100,000 for hPa)
+	pMilli, err := b.dev.ReadPressure()
+	if err != nil {
+		return 0, 0, 0, false, err
+	}
+
+	// ReadHumidity returns 100 * % (e.g. 4500 = 45.00%)
+	hPct, err := b.dev.ReadHumidity()
+	if err != nil {
+		return 0, 0, 0, false, err
+	}
+
+	temp := float64(tMilli) / 1000.0
+	pres := float64(pMilli) / 100000.0
+	hum := float64(hPct) / 100.0
+
+	return temp, pres, hum, true, nil
+}
+
+func (b *BME280Adapter) Name() string { return "BME280" }
+
+// -------------------------------------------------------------
+// SHT41 Adapter
+// -------------------------------------------------------------
+type SHT41Adapter struct {
+	dev sht4x.Device
+}
+
+func (s *SHT41Adapter) ReadValues() (float64, float64, float64, bool, error) {
+	// ReadTemperatureHumidity takes no parameters.
+	// Returns temp in milli-°C and humidity in milli-% (divide by 1000.0)
+	tMilli, hMilli, err := s.dev.ReadTemperatureHumidity()
+	if err != nil {
+		return 0, 0, 0, false, err
+	}
+
+	temp := float64(tMilli) / 1000.0
+	hum := float64(hMilli) / 1000.0
+
+	return temp, 0, hum, false, nil
+}
+
+func (s *SHT41Adapter) Name() string { return "SHT41" }
+
+// -------------------------------------------------------------
+// Factory
+// -------------------------------------------------------------
+func InitSensor(i2c *machine.I2C, sensorType string) (ClimateSensor, error) {
+  // Give the sensors a split-second to wake up before trying to register them
+  time.Sleep(100 * time.Millisecond)
+
+	switch sensorType {
+	case "bme280":
+		dev := bme280.New(i2c)
+		dev.Configure()
+		if !dev.Connected() {
+			return nil, errors.New("BME280 not found on I2C bus")
+		}
+		return &BME280Adapter{dev: dev}, nil
+
+	case "sht41":
+    dev := sht4x.New(i2c)
+    dev.Address = 0x45 // Default is 0x44 (sht4x.Address)
+    if _, _, err := dev.ReadTemperatureHumidity(); err != nil {
+        // Try fallback back to 0x44
+        dev.Address = 0x44
+        if _, _, err := dev.ReadTemperatureHumidity(); err != nil {
+            return nil, errors.New("SHT41 not found at 0x44 or 0x45")
+        }
+    }
+    return &SHT41Adapter{dev: dev}, nil
+
+	default:
+		return nil, errors.New("unsupported sensor configured")
+	}
+}