UART & Sensors
This commit is contained in:
+12
-1
@@ -1,5 +1,16 @@
|
||||
|
||||
# LoRa
|
||||
|
||||
`mpremote connect /dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0 repl`
|
||||
`mpremote connect /dev/serial/by-path/pci-0000:00:14.0-usb-0:6.1:1.0-port0 repl`
|
||||
|
||||
# MQTT
|
||||
|
||||
`mpremote connect /dev/serial/by-path/pci-0000:00:14.0-usb-0:6.2:1.0-port0 repl`
|
||||
|
||||
# UART
|
||||
|
||||
| LoRa | MQTT |
|
||||
| --- | --- |
|
||||
| 45 | P17 |
|
||||
| 46 | P16 |
|
||||
| GND | GND |
|
||||
|
||||
@@ -61,7 +61,7 @@ while True:
|
||||
command = uart_device.read()
|
||||
print(f"[Main] Received command from WiFi Board: {command}")
|
||||
|
||||
uart_device.send(f"Hello from esp-32 lora ID {DEVICE_ID}")
|
||||
# uart_device.send(f"Hello from esp-32 lora ID {DEVICE_ID}")
|
||||
|
||||
# 2. Send local metrics over the wire to the WiFi board every few seconds
|
||||
# uart_device.send("Data Pack: LoRa Link RSSI -72dBm")
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# This file is executed on every boot (including wake-boot from deepsleep)
|
||||
#import esp
|
||||
#esp.osdebug(None)
|
||||
import esp
|
||||
esp.osdebug(True)
|
||||
#import webrepl
|
||||
#webrepl.start()
|
||||
|
||||
@@ -16,4 +16,4 @@ def do_connect(ssid, pwd):
|
||||
print('network config:', sta_if.ifconfig())
|
||||
|
||||
# Attempt to connect to WiFi network
|
||||
do_connect("Smartwave-1", 'Smartwave-prot-1')
|
||||
# do_connect("Smartwave-1", 'Smartwave-prot-1')
|
||||
|
||||
@@ -1,8 +1,11 @@
|
||||
import _thread
|
||||
import select
|
||||
from machine import Pin
|
||||
from shared import get_mqtt_client, get_uart, config
|
||||
from sensors import temperature_gun
|
||||
from shared import get_mqtt_client, get_uart, config, payloads
|
||||
import time
|
||||
import ujson as json
|
||||
import sys
|
||||
|
||||
# Simple thread-safe queue list
|
||||
msg_queue = []
|
||||
@@ -13,17 +16,22 @@ def queue_publish(topic, payload):
|
||||
with queue_lock:
|
||||
msg_queue.append((topic, payload))
|
||||
|
||||
# --- Hardware & Client Setup ---
|
||||
vext = Pin(19, Pin.OUT)
|
||||
vext.value(0)
|
||||
time.sleep_ms(100)
|
||||
# --- INITIALIZE CAMERA ---
|
||||
try:
|
||||
# Pass your confirmed working SCL and SDA pins here
|
||||
temperature_gun.init_camera(scl_pin=21, sda_pin=22, freq=100000)
|
||||
except Exception as e:
|
||||
print("[Main] Critical: Camera setup failed!")
|
||||
sys.print_exception(e)
|
||||
|
||||
# --- READ DEVICE ID ---
|
||||
try:
|
||||
with open("device_id.txt", "r") as f:
|
||||
DEVICE_ID = f.read().strip()
|
||||
except Exception:
|
||||
DEVICE_ID = "ESP32_Inconnu"
|
||||
|
||||
# --- MQTT SETUP ---
|
||||
MQTT_CA_FILE = "/certs/ca.crt"
|
||||
|
||||
mqtt_client = get_mqtt_client(
|
||||
@@ -34,8 +42,28 @@ mqtt_client = get_mqtt_client(
|
||||
keepalive=config.MQTT_KEEPALIVE,
|
||||
)
|
||||
|
||||
global orchestrator_id
|
||||
orchestrator_id = None
|
||||
def on_mqtt_message(message):
|
||||
print("[MQTT Thread] Received message:", message)
|
||||
|
||||
# Try and parse the payload as json, but if it fails, just print the raw payload
|
||||
payload_data=None
|
||||
try:
|
||||
payload_data = json.loads(message['payload'])
|
||||
except Exception as e:
|
||||
print("[MQTT Thread] Error parsing JSON:", e)
|
||||
sys.print_exception(e)
|
||||
pass # Maybe it's not JSON
|
||||
|
||||
if message['topic'] == config.MQTT_TOPIC_HELLO and payload_data and "id_orchestrator" in payload_data and payload_data["id_microwave"] == DEVICE_ID:
|
||||
print("[MQTT Thread] Hello response received from orchestrator:", payload_data["id_orchestrator"])
|
||||
global orchestrator_id
|
||||
orchestrator_id = payload_data["id_orchestrator"]
|
||||
# Unsubscribe from the hello topic since we got a response
|
||||
mqtt_client.unsubscribe(config.MQTT_TOPIC_HELLO)
|
||||
print("[MQTT Thread] Unsubscribed from topic:", config.MQTT_TOPIC_HELLO)
|
||||
print("[MQTT Thread] Message processing complete.")
|
||||
|
||||
mqtt_client.set_callback(on_mqtt_message)
|
||||
|
||||
@@ -73,7 +101,7 @@ def mqtt_background_thread():
|
||||
|
||||
# 3. Handle Keepalive tracking manually
|
||||
if time.time() - last_check >= 15:
|
||||
print("[Thread] Sending keepalive ping...")
|
||||
# print("[Thread] Sending keepalive ping...")
|
||||
mqtt_client._client.ping()
|
||||
last_check = time.time()
|
||||
|
||||
@@ -82,6 +110,7 @@ def mqtt_background_thread():
|
||||
|
||||
except Exception as e:
|
||||
print("[Thread] Connection dropped or error encountered:", e)
|
||||
sys.print_exception(e)
|
||||
print("[Thread] Cleaning up socket context. Retrying in 5 seconds...")
|
||||
|
||||
# --- FIX FOR ERROR 23 (SOCKET LEAK) ---
|
||||
@@ -99,29 +128,54 @@ def mqtt_background_thread():
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(5)
|
||||
|
||||
# --- UART BACKGROUND THREAD ---
|
||||
def uart_background_thread():
|
||||
"""Background UART worker handling all serial operations safely."""
|
||||
print("[Thread] Background UART worker started.")
|
||||
|
||||
uart_device = get_uart(uart_id=2, tx_pin=17, rx_pin=16)
|
||||
|
||||
while True:
|
||||
try:
|
||||
# 1. Check for incoming messages from the Heltec board
|
||||
while uart_device.any():
|
||||
incoming_msg = uart_device.read()
|
||||
print(f"[Thread] Received from esp-lora over UART: {incoming_msg}")
|
||||
|
||||
# 2. Example: Send data to the Heltec board every 5 seconds
|
||||
# uart_device.send("Status Check: WiFi Active")
|
||||
|
||||
# UART
|
||||
uart_device = get_uart(uart_id=2, tx_pin=17, rx_pin=16)
|
||||
time.sleep(5) # Fast responsive polling loop for local UART
|
||||
|
||||
except Exception as e:
|
||||
print("[Thread] UART error encountered:", e)
|
||||
time.sleep(5)
|
||||
|
||||
# --- Launch background worker ---
|
||||
_thread.start_new_thread(mqtt_background_thread, ())
|
||||
# _thread.start_new_thread(mqtt_background_thread, ())
|
||||
# _thread.start_new_thread(uart_background_thread, ())
|
||||
|
||||
|
||||
# --- MAIN APPLICATION THREAD (Core 0) ---
|
||||
print("[Main] Main execution path active.")
|
||||
time.sleep(2) # Give the thread a moment to initial connect
|
||||
mqtt_hello_sent_timestamp = -config.MQTT_HELLO_INTERVAL
|
||||
mqtt_client.subscribe(config.MQTT_TOPIC_HELLO, qos=config.MQTT_QOS)
|
||||
|
||||
while True:
|
||||
print("[Main] Queueing a test message for MQTT...")
|
||||
# Instead of direct publishing, push it to the queue safely
|
||||
queue_publish(config.MQTT_TOPIC_SENSOR, "Hello from ESP32!")
|
||||
|
||||
# 1. Check if the Heltec V3 sent us something over the wire
|
||||
while uart_device.any():
|
||||
incoming_msg = uart_device.read()
|
||||
print(f"[Main] Received from esp-lora over UART: {incoming_msg}")
|
||||
# MQTT HELLO sent every x seconds until we get a response from the orchestrator
|
||||
if (orchestrator_id == None and -(mqtt_hello_sent_timestamp - time.time()) > config.MQTT_HELLO_INTERVAL):
|
||||
print("[Main] Attempting to send initial hello to orchestrator...")
|
||||
queue_publish(config.MQTT_TOPIC_HELLO, payloads.mqtt_hello(DEVICE_ID))
|
||||
mqtt_hello_sent_timestamp = time.time()
|
||||
pass
|
||||
|
||||
# Sensors
|
||||
print(f"[Main] Reading temperature from the gun sensor...")
|
||||
temp = temperature_gun.read_temperature()
|
||||
print(f"[Main] Temperature read: {temp}°C")
|
||||
|
||||
# 2. Example: Send data to the Heltec board every 5 seconds
|
||||
# uart_device.send("Status Check: WiFi Active")
|
||||
|
||||
time.sleep_ms(200) # Fast responsive polling loop for local UART
|
||||
time.sleep(1)
|
||||
@@ -0,0 +1 @@
|
||||
import sensors.temperature_gun as temperature_gun
|
||||
@@ -0,0 +1,969 @@
|
||||
"""
|
||||
Temperatue gun sensor module
|
||||
using the MLX90640-D55/D110 sensor. This module provides a function to read the temperature from the gun sensor.
|
||||
Resolution of 32x24 pixels,
|
||||
I2C interface
|
||||
Noise Equivalent Temperature difference (NETD) is 0.1K RMS @ 1Hz refresh rate
|
||||
"""
|
||||
|
||||
import machine # type: ignore
|
||||
import math
|
||||
import struct
|
||||
import time
|
||||
from micropython import const# Some libraries that we will use
|
||||
import time
|
||||
|
||||
|
||||
class RefreshRate: # pylint: disable=too-few-public-methods
|
||||
""" Enum-like class for MLX90640's refresh rate """
|
||||
REFRESH_0_5_HZ = const(0b000) # 0.5Hz
|
||||
REFRESH_1_HZ = const(0b001) # 1Hz
|
||||
REFRESH_2_HZ = const(0b010) # 2Hz
|
||||
REFRESH_4_HZ = const(0b011) # 4Hz
|
||||
REFRESH_8_HZ = const(0b100) # 8Hz
|
||||
REFRESH_16_HZ = const(0b101) # 16Hz
|
||||
REFRESH_32_HZ = const(0b110) # 32Hz
|
||||
REFRESH_64_HZ = const(0b111) # 64Hz
|
||||
|
||||
class ContextManaged:
|
||||
"""An object that automatically deinitializes hardware with a context manager."""
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
self.deinit()
|
||||
|
||||
# pylint: disable=no-self-use
|
||||
def deinit(self):
|
||||
"""Free any hardware used by the object."""
|
||||
return
|
||||
|
||||
class Lockable(ContextManaged):
|
||||
"""An object that must be locked to prevent collisions on a microcontroller resource."""
|
||||
|
||||
_locked = False
|
||||
|
||||
def try_lock(self):
|
||||
"""Attempt to grab the lock. Return True on success, False if the lock is already taken."""
|
||||
if self._locked:
|
||||
return False
|
||||
self._locked = True
|
||||
return True
|
||||
|
||||
def unlock(self):
|
||||
"""Release the lock so others may use the resource."""
|
||||
if self._locked:
|
||||
self._locked = False
|
||||
else:
|
||||
raise ValueError("Not locked")
|
||||
|
||||
class I2C(Lockable):
|
||||
def __init__(self, pins=(21, 22), frequency=100000):
|
||||
self.init(pins, frequency)
|
||||
|
||||
def init(self, pins, frequency):
|
||||
self.deinit()
|
||||
|
||||
# 1. Force the ESP32 to activate its internal pull-up resistors on these pins
|
||||
self._pins = (
|
||||
machine.Pin(int(pins[0]), machine.Pin.IN, machine.Pin.PULL_UP),
|
||||
machine.Pin(int(pins[1]), machine.Pin.IN, machine.Pin.PULL_UP)
|
||||
)
|
||||
|
||||
try:
|
||||
# 2. Bypasses the glitchy ESP32 hardware block using SoftI2C
|
||||
# (Note: SoftI2C does not take a bus ID number like '0')
|
||||
self._i2c = machine.SoftI2C(scl=self._pins[0], sda=self._pins[1], freq=frequency)
|
||||
except RuntimeError:
|
||||
raise
|
||||
print(f"Created resilient SoftI2C: {self._i2c}")
|
||||
|
||||
def deinit(self):
|
||||
try:
|
||||
del self._i2c
|
||||
except AttributeError:
|
||||
pass
|
||||
|
||||
def scan(self):
|
||||
return self._i2c.scan()
|
||||
|
||||
def readfrom_into(self, address, buffer, *, start=0, end=None):
|
||||
if start is not 0 or end is not None:
|
||||
if end is None:
|
||||
end = len(buffer)
|
||||
buffer = memoryview(buffer)[start:end]
|
||||
stop = True # remove for efficiency later
|
||||
return self._i2c.readfrom_into(address, buffer)
|
||||
|
||||
def writeto(self, address, buffer, *, start=0, end=None, stop=True):
|
||||
if isinstance(buffer, str):
|
||||
buffer = bytes([ord(x) for x in buffer])
|
||||
if start is not 0 or end is not None:
|
||||
if end is None:
|
||||
return self._i2c.writeto(address, memoryview(buffer)[start:], stop)
|
||||
else:
|
||||
return self._i2c.writeto(address, memoryview(buffer)[start:end], stop)
|
||||
return self._i2c.writeto(address, buffer, stop)
|
||||
|
||||
class I2CDevice:
|
||||
def __init__(self, i2c, device_address, probe=True):
|
||||
self.i2c = i2c
|
||||
self._has_write_read = False # hasattr(self.i2c, "writeto_then_readfrom") --> has been turned to False
|
||||
self.device_address = device_address
|
||||
|
||||
if probe:
|
||||
self.__probe_for_device()
|
||||
|
||||
def readinto(self, buf, *, start=0, end=None):
|
||||
if end is None:
|
||||
end = len(buf)
|
||||
self.i2c.readfrom_into(self.device_address, buf, start=start, end=end)
|
||||
|
||||
def write(self, buf, *, start=0, end=None, stop=True):
|
||||
if end is None:
|
||||
end = len(buf)
|
||||
self.i2c.writeto(self.device_address, buf, start=start, end=end, stop=stop)
|
||||
|
||||
# pylint: disable-msg=too-many-arguments
|
||||
def write_then_readinto(
|
||||
self,
|
||||
out_buffer,
|
||||
in_buffer,
|
||||
*,
|
||||
out_start=0,
|
||||
out_end=None,
|
||||
in_start=0,
|
||||
in_end=None,
|
||||
stop=False
|
||||
):
|
||||
if out_end is None:
|
||||
out_end = len(out_buffer)
|
||||
if in_end is None:
|
||||
in_end = len(in_buffer)
|
||||
if stop:
|
||||
raise ValueError("Stop must be False. Use writeto instead.")
|
||||
if self._has_write_read:
|
||||
#print("c",dir(self.i2c))
|
||||
# In linux, at least, this is a special kernel function call
|
||||
self.i2c.writeto_then_readfrom(
|
||||
self.device_address,
|
||||
out_buffer,
|
||||
in_buffer,
|
||||
out_start=out_start,
|
||||
out_end=out_end,
|
||||
in_start=in_start,
|
||||
in_end=in_end,
|
||||
)
|
||||
|
||||
else:
|
||||
# If we don't have a special implementation, we can fake it with two calls
|
||||
self.i2c.writeto(self.device_address, out_buffer, stop=False) # These lines have been changed to make it work with wipy micropython I2C module
|
||||
#self.write(out_buffer, start=out_start, end=out_end, stop=False)
|
||||
#self.readinto(in_buffer, start=in_start, end=in_end)
|
||||
self.i2c.readfrom_into(self.device_address, in_buffer) # These lines have been changed to make it work with wipy micropython I2C module
|
||||
|
||||
|
||||
# pylint: enable-msg=too-many-arguments
|
||||
|
||||
def __enter__(self):
|
||||
while not self.i2c.try_lock():
|
||||
pass
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
self.i2c.unlock()
|
||||
return False
|
||||
|
||||
def __probe_for_device(self):
|
||||
"""
|
||||
Try to read a byte from an address,
|
||||
if you get an OSError it means the device is not there
|
||||
or that the device does not support these means of probing
|
||||
"""
|
||||
while not self.i2c.try_lock():
|
||||
pass
|
||||
try:
|
||||
self.i2c.writeto(self.device_address, b"")
|
||||
except OSError:
|
||||
# some OS's dont like writing an empty bytesting...
|
||||
# Retry by reading a byte
|
||||
try:
|
||||
result = bytearray(1)
|
||||
self.i2c.readfrom_into(self.device_address, result)
|
||||
except OSError:
|
||||
raise ValueError("No I2C device at address: %x" % self.device_address)
|
||||
finally:
|
||||
self.i2c.unlock()
|
||||
|
||||
eeData = [0] * const(832)
|
||||
I2C_READ_LEN = const(2048)
|
||||
SCALEALPHA = const(0.000001)
|
||||
MLX90640_DEVICEID1 = const(0x2407)
|
||||
OPENAIR_TA_SHIFT = const(8)
|
||||
|
||||
class MLX90640: # pylint: disable=too-many-instance-attributes
|
||||
"""Interface to the MLX90640 temperature sensor."""
|
||||
|
||||
kVdd = 0
|
||||
vdd25 = 0
|
||||
KvPTAT = 0
|
||||
KtPTAT = 0
|
||||
vPTAT25 = 0
|
||||
alphaPTAT = 0
|
||||
gainEE = 0
|
||||
tgc = 0
|
||||
KsTa = 0
|
||||
resolutionEE = 0
|
||||
calibrationModeEE = 0
|
||||
ksTo = [0] * 5
|
||||
ct = [0] * 5
|
||||
alpha = [0] * 768
|
||||
alphaScale = 0
|
||||
offset = [0] * 768
|
||||
kta = [0] * 768
|
||||
ktaScale = 0
|
||||
kv = [0] * 768
|
||||
kvScale = 0
|
||||
cpAlpha = [0] * 2
|
||||
cpOffset = [0] * 2
|
||||
ilChessC = [0] * 3
|
||||
brokenPixels = [0xFFFF] * 5
|
||||
outlierPixels = [0xFFFF] * 5
|
||||
cpKta = 0
|
||||
cpKv = 0
|
||||
|
||||
def __init__(self, i2c_bus, address=0x33):
|
||||
self.i2c_device = I2CDevice(i2c_bus, address)
|
||||
self._I2CReadWords(0x2400, eeData)
|
||||
# print(eeData)
|
||||
self._ExtractParameters()
|
||||
|
||||
@property
|
||||
def serial_number(self):
|
||||
""" 3-item tuple of hex values that are unique to each MLX90640 """
|
||||
serialWords = [0, 0, 0]
|
||||
self._I2CReadWords(MLX90640_DEVICEID1, serialWords)
|
||||
return serialWords
|
||||
|
||||
@property
|
||||
def refresh_rate(self):
|
||||
""" How fast the MLX90640 will spit out data. Start at lowest speed in
|
||||
RefreshRate and then slowly increase I2C clock rate and rate until you
|
||||
max out. The sensor does not like it if the I2C host cannot 'keep up'!"""
|
||||
controlRegister = [0]
|
||||
self._I2CReadWords(0x800D, controlRegister)
|
||||
return (controlRegister[0] >> 7) & 0x07
|
||||
|
||||
@refresh_rate.setter
|
||||
def refresh_rate(self, rate):
|
||||
controlRegister = [0]
|
||||
value = (rate & 0x7) << 7
|
||||
self._I2CReadWords(0x800D, controlRegister)
|
||||
value |= controlRegister[0] & 0xFC7F
|
||||
self._I2CWriteWord(0x800D, value)
|
||||
|
||||
def getFrame(self, framebuf):
|
||||
""" Request both 'halves' of a frame from the sensor, merge them
|
||||
and calculate the temperature in C for each of 32x24 pixels. Placed
|
||||
into the 768-element array passed in! """
|
||||
emissivity = 0.95
|
||||
tr = 23.15
|
||||
mlx90640Frame = [0] * 834
|
||||
|
||||
for _ in range(2):
|
||||
status = self._GetFrameData(mlx90640Frame)
|
||||
if status < 0:
|
||||
raise RuntimeError("Frame data error")
|
||||
# For a MLX90640 in the open air the shift is -8 degC.
|
||||
tr = self._GetTa(mlx90640Frame) - OPENAIR_TA_SHIFT
|
||||
self._CalculateTo(mlx90640Frame, emissivity, tr, framebuf)
|
||||
|
||||
def _GetFrameData(self, frameData):
|
||||
dataReady = 0
|
||||
cnt = 0
|
||||
statusRegister = [0]
|
||||
controlRegister = [0]
|
||||
|
||||
while dataReady == 0:
|
||||
self._I2CReadWords(0x8000, statusRegister)
|
||||
dataReady = statusRegister[0] & 0x0008
|
||||
# print("ready status: 0x%x" % dataReady)
|
||||
|
||||
while (dataReady != 0) and (cnt < 5):
|
||||
self._I2CWriteWord(0x8000, 0x0030)
|
||||
# print("Read frame", cnt)
|
||||
self._I2CReadWords(0x0400, frameData, end=832)
|
||||
|
||||
self._I2CReadWords(0x8000, statusRegister)
|
||||
dataReady = statusRegister[0] & 0x0008
|
||||
# print("frame ready: 0x%x" % dataReady)
|
||||
cnt += 1
|
||||
|
||||
if cnt > 4:
|
||||
raise RuntimeError("Too many retries")
|
||||
|
||||
self._I2CReadWords(0x800D, controlRegister)
|
||||
frameData[832] = controlRegister[0]
|
||||
frameData[833] = statusRegister[0] & 0x0001
|
||||
return frameData[833]
|
||||
|
||||
def _GetTa(self, frameData):
|
||||
vdd = self._GetVdd(frameData)
|
||||
|
||||
ptat = frameData[800]
|
||||
if ptat > 32767:
|
||||
ptat -= 65536
|
||||
|
||||
ptatArt = frameData[768]
|
||||
if ptatArt > 32767:
|
||||
ptatArt -= 65536
|
||||
ptatArt = (ptat / (ptat * self.alphaPTAT + ptatArt)) * math.pow(2, 18)
|
||||
|
||||
ta = ptatArt / (1 + self.KvPTAT * (vdd - 3.3)) - self.vPTAT25
|
||||
ta = ta / self.KtPTAT + 25
|
||||
return ta
|
||||
|
||||
def _GetVdd(self, frameData):
|
||||
vdd = frameData[810]
|
||||
if vdd > 32767:
|
||||
vdd -= 65536
|
||||
|
||||
resolutionRAM = (frameData[832] & 0x0C00) >> 10
|
||||
resolutionCorrection = math.pow(2, self.resolutionEE) / math.pow(
|
||||
2, resolutionRAM
|
||||
)
|
||||
vdd = (resolutionCorrection * vdd - self.vdd25) / self.kVdd + 3.3
|
||||
|
||||
return vdd
|
||||
|
||||
def _CalculateTo(self, frameData, emissivity, tr, result):
|
||||
# pylint: disable=too-many-locals, too-many-branches, too-many-statements
|
||||
subPage = frameData[833]
|
||||
alphaCorrR = [0] * 4
|
||||
irDataCP = [0, 0]
|
||||
|
||||
vdd = self._GetVdd(frameData)
|
||||
ta = self._GetTa(frameData)
|
||||
|
||||
ta4 = ta + 273.15
|
||||
ta4 = ta4 * ta4
|
||||
ta4 = ta4 * ta4
|
||||
tr4 = tr + 273.15
|
||||
tr4 = tr4 * tr4
|
||||
tr4 = tr4 * tr4
|
||||
taTr = tr4 - (tr4 - ta4) / emissivity
|
||||
|
||||
ktaScale = math.pow(2, self.ktaScale)
|
||||
kvScale = math.pow(2, self.kvScale)
|
||||
alphaScale = math.pow(2, self.alphaScale)
|
||||
|
||||
alphaCorrR[0] = 1 / (1 + self.ksTo[0] * 40)
|
||||
alphaCorrR[1] = 1
|
||||
alphaCorrR[2] = 1 + self.ksTo[1] * self.ct[2]
|
||||
alphaCorrR[3] = alphaCorrR[2] * (1 + self.ksTo[2] * (self.ct[3] - self.ct[2]))
|
||||
|
||||
# --------- Gain calculation -----------------------------------
|
||||
gain = frameData[778]
|
||||
if gain > 32767:
|
||||
gain -= 65536
|
||||
gain = self.gainEE / gain
|
||||
|
||||
# --------- To calculation -------------------------------------
|
||||
mode = (frameData[832] & 0x1000) >> 5
|
||||
|
||||
irDataCP[0] = frameData[776]
|
||||
irDataCP[1] = frameData[808]
|
||||
for i in range(2):
|
||||
if irDataCP[i] > 32767:
|
||||
irDataCP[i] -= 65536
|
||||
irDataCP[i] *= gain
|
||||
|
||||
irDataCP[0] -= (
|
||||
self.cpOffset[0]
|
||||
* (1 + self.cpKta * (ta - 25))
|
||||
* (1 + self.cpKv * (vdd - 3.3))
|
||||
)
|
||||
if mode == self.calibrationModeEE:
|
||||
irDataCP[1] -= (
|
||||
self.cpOffset[1]
|
||||
* (1 + self.cpKta * (ta - 25))
|
||||
* (1 + self.cpKv * (vdd - 3.3))
|
||||
)
|
||||
else:
|
||||
irDataCP[1] -= (
|
||||
(self.cpOffset[1] + self.ilChessC[0])
|
||||
* (1 + self.cpKta * (ta - 25))
|
||||
* (1 + self.cpKv * (vdd - 3.3))
|
||||
)
|
||||
|
||||
for pixelNumber in range(768):
|
||||
ilPattern = pixelNumber // 32 - (pixelNumber // 64) * 2
|
||||
chessPattern = ilPattern ^ (pixelNumber - (pixelNumber // 2) * 2)
|
||||
conversionPattern = (
|
||||
(pixelNumber + 2) // 4
|
||||
- (pixelNumber + 3) // 4
|
||||
+ (pixelNumber + 1) // 4
|
||||
- pixelNumber // 4
|
||||
) * (1 - 2 * ilPattern)
|
||||
|
||||
if mode == 0:
|
||||
pattern = ilPattern
|
||||
else:
|
||||
pattern = chessPattern
|
||||
|
||||
if pattern == frameData[833]:
|
||||
irData = frameData[pixelNumber]
|
||||
if irData > 32767:
|
||||
irData -= 65536
|
||||
irData *= gain
|
||||
|
||||
kta = self.kta[pixelNumber] / ktaScale
|
||||
kv = self.kv[pixelNumber] / kvScale
|
||||
irData -= (
|
||||
self.offset[pixelNumber]
|
||||
* (1 + kta * (ta - 25))
|
||||
* (1 + kv * (vdd - 3.3))
|
||||
)
|
||||
|
||||
if mode != self.calibrationModeEE:
|
||||
irData += (
|
||||
self.ilChessC[2] * (2 * ilPattern - 1)
|
||||
- self.ilChessC[1] * conversionPattern
|
||||
)
|
||||
|
||||
irData = irData - self.tgc * irDataCP[subPage]
|
||||
irData /= emissivity
|
||||
|
||||
alphaCompensated = SCALEALPHA * alphaScale / self.alpha[pixelNumber]
|
||||
alphaCompensated *= 1 + self.KsTa * (ta - 25)
|
||||
|
||||
Sx = (
|
||||
alphaCompensated
|
||||
* alphaCompensated
|
||||
* alphaCompensated
|
||||
* (irData + alphaCompensated * taTr)
|
||||
)
|
||||
Sx = math.sqrt(math.sqrt(Sx)) * self.ksTo[1]
|
||||
|
||||
To = (
|
||||
math.sqrt(
|
||||
math.sqrt(
|
||||
irData
|
||||
/ (alphaCompensated * (1 - self.ksTo[1] * 273.15) + Sx)
|
||||
+ taTr
|
||||
)
|
||||
)
|
||||
- 273.15
|
||||
)
|
||||
|
||||
if To < self.ct[1]:
|
||||
torange = 0
|
||||
elif To < self.ct[2]:
|
||||
torange = 1
|
||||
elif To < self.ct[3]:
|
||||
torange = 2
|
||||
else:
|
||||
torange = 3
|
||||
|
||||
To = (
|
||||
math.sqrt(
|
||||
math.sqrt(
|
||||
irData
|
||||
/ (
|
||||
alphaCompensated
|
||||
* alphaCorrR[torange]
|
||||
* (1 + self.ksTo[torange] * (To - self.ct[torange]))
|
||||
)
|
||||
+ taTr
|
||||
)
|
||||
)
|
||||
- 273.15
|
||||
)
|
||||
|
||||
result[pixelNumber] = To
|
||||
|
||||
# pylint: enable=too-many-locals, too-many-branches, too-many-statements
|
||||
|
||||
def _ExtractParameters(self):
|
||||
self._ExtractVDDParameters()
|
||||
self._ExtractPTATParameters()
|
||||
self._ExtractGainParameters()
|
||||
self._ExtractTgcParameters()
|
||||
self._ExtractResolutionParameters()
|
||||
self._ExtractKsTaParameters()
|
||||
self._ExtractKsToParameters()
|
||||
self._ExtractCPParameters()
|
||||
self._ExtractAlphaParameters()
|
||||
self._ExtractOffsetParameters()
|
||||
self._ExtractKtaPixelParameters()
|
||||
self._ExtractKvPixelParameters()
|
||||
self._ExtractCILCParameters()
|
||||
self._ExtractDeviatingPixels()
|
||||
|
||||
def _ExtractVDDParameters(self):
|
||||
# extract VDD
|
||||
self.kVdd = (eeData[51] & 0xFF00) >> 8
|
||||
if self.kVdd > 127:
|
||||
self.kVdd -= 256 # convert to signed
|
||||
self.kVdd *= 32
|
||||
self.vdd25 = eeData[51] & 0x00FF
|
||||
self.vdd25 = ((self.vdd25 - 256) << 5) - 8192
|
||||
|
||||
def _ExtractPTATParameters(self):
|
||||
# extract PTAT
|
||||
self.KvPTAT = (eeData[50] & 0xFC00) >> 10
|
||||
if self.KvPTAT > 31:
|
||||
self.KvPTAT -= 64
|
||||
self.KvPTAT /= 4096
|
||||
self.KtPTAT = eeData[50] & 0x03FF
|
||||
if self.KtPTAT > 511:
|
||||
self.KtPTAT -= 1024
|
||||
self.KtPTAT /= 8
|
||||
self.vPTAT25 = eeData[49]
|
||||
self.alphaPTAT = (eeData[16] & 0xF000) / math.pow(2, 14) + 8
|
||||
|
||||
def _ExtractGainParameters(self):
|
||||
# extract Gain
|
||||
self.gainEE = eeData[48]
|
||||
if self.gainEE > 32767:
|
||||
self.gainEE -= 65536
|
||||
|
||||
def _ExtractTgcParameters(self):
|
||||
# extract Tgc
|
||||
#print(eeData[60])
|
||||
self.tgc = eeData[60] & 0x00FF
|
||||
#print(self.tgc)
|
||||
if self.tgc > 127:
|
||||
self.tgc -= 256
|
||||
self.tgc /= 32
|
||||
#print(self.tgc)
|
||||
|
||||
def _ExtractResolutionParameters(self):
|
||||
# extract resolution
|
||||
self.resolutionEE = (eeData[56] & 0x3000) >> 12
|
||||
|
||||
def _ExtractKsTaParameters(self):
|
||||
# extract KsTa
|
||||
self.KsTa = (eeData[60] & 0xFF00) >> 8
|
||||
if self.KsTa > 127:
|
||||
self.KsTa -= 256
|
||||
self.KsTa /= 8192
|
||||
|
||||
def _ExtractKsToParameters(self):
|
||||
# extract ksTo
|
||||
step = ((eeData[63] & 0x3000) >> 12) * 10
|
||||
self.ct[0] = -40
|
||||
self.ct[1] = 0
|
||||
self.ct[2] = (eeData[63] & 0x00F0) >> 4
|
||||
self.ct[3] = (eeData[63] & 0x0F00) >> 8
|
||||
self.ct[2] *= step
|
||||
self.ct[3] = self.ct[2] + self.ct[3] * step
|
||||
|
||||
KsToScale = (eeData[63] & 0x000F) + 8
|
||||
KsToScale = 1 << KsToScale
|
||||
|
||||
self.ksTo[0] = eeData[61] & 0x00FF
|
||||
self.ksTo[1] = (eeData[61] & 0xFF00) >> 8
|
||||
self.ksTo[2] = eeData[62] & 0x00FF
|
||||
self.ksTo[3] = (eeData[62] & 0xFF00) >> 8
|
||||
|
||||
for i in range(4):
|
||||
if self.ksTo[i] > 127:
|
||||
self.ksTo[i] -= 256
|
||||
self.ksTo[i] /= KsToScale
|
||||
self.ksTo[4] = -0.0002
|
||||
|
||||
def _ExtractCPParameters(self):
|
||||
# extract CP
|
||||
offsetSP = [0] * 2
|
||||
alphaSP = [0] * 2
|
||||
|
||||
alphaScale = ((eeData[32] & 0xF000) >> 12) + 27
|
||||
|
||||
offsetSP[0] = eeData[58] & 0x03FF
|
||||
if offsetSP[0] > 511:
|
||||
offsetSP[0] -= 1024
|
||||
|
||||
offsetSP[1] = (eeData[58] & 0xFC00) >> 10
|
||||
if offsetSP[1] > 31:
|
||||
offsetSP[1] -= 64
|
||||
offsetSP[1] += offsetSP[0]
|
||||
|
||||
alphaSP[0] = eeData[57] & 0x03FF
|
||||
if alphaSP[0] > 511:
|
||||
alphaSP[0] -= 1024
|
||||
alphaSP[0] /= math.pow(2, alphaScale)
|
||||
|
||||
alphaSP[1] = (eeData[57] & 0xFC00) >> 10
|
||||
if alphaSP[1] > 31:
|
||||
alphaSP[1] -= 64
|
||||
alphaSP[1] = (1 + alphaSP[1] / 128) * alphaSP[0]
|
||||
|
||||
cpKta = eeData[59] & 0x00FF
|
||||
if cpKta > 127:
|
||||
cpKta -= 256
|
||||
ktaScale1 = ((eeData[56] & 0x00F0) >> 4) + 8
|
||||
self.cpKta = cpKta / math.pow(2, ktaScale1)
|
||||
|
||||
cpKv = (eeData[59] & 0xFF00) >> 8
|
||||
if cpKv > 127:
|
||||
cpKv -= 256
|
||||
kvScale = (eeData[56] & 0x0F00) >> 8
|
||||
self.cpKv = cpKv / math.pow(2, kvScale)
|
||||
|
||||
self.cpAlpha[0] = alphaSP[0]
|
||||
self.cpAlpha[1] = alphaSP[1]
|
||||
self.cpOffset[0] = offsetSP[0]
|
||||
self.cpOffset[1] = offsetSP[1]
|
||||
#print(self.cpAlpha[0])
|
||||
#print(self.cpAlpha[1])
|
||||
|
||||
def _ExtractAlphaParameters(self):
|
||||
# extract alpha
|
||||
accRemScale = eeData[32] & 0x000F
|
||||
accColumnScale = (eeData[32] & 0x00F0) >> 4
|
||||
accRowScale = (eeData[32] & 0x0F00) >> 8
|
||||
alphaScale = ((eeData[32] & 0xF000) >> 12) + 30
|
||||
alphaRef = eeData[33]
|
||||
accRow = [0] * 24
|
||||
accColumn = [0] * 32
|
||||
alphaTemp = [0] * 768
|
||||
|
||||
for i in range(6):
|
||||
p = i * 4
|
||||
accRow[p + 0] = eeData[34 + i] & 0x000F
|
||||
accRow[p + 1] = (eeData[34 + i] & 0x00F0) >> 4
|
||||
accRow[p + 2] = (eeData[34 + i] & 0x0F00) >> 8
|
||||
accRow[p + 3] = (eeData[34 + i] & 0xF000) >> 12
|
||||
|
||||
for i in range(24):
|
||||
if accRow[i] > 7:
|
||||
accRow[i] -= 16
|
||||
|
||||
for i in range(8):
|
||||
p = i * 4
|
||||
accColumn[p + 0] = eeData[40 + i] & 0x000F
|
||||
accColumn[p + 1] = (eeData[40 + i] & 0x00F0) >> 4
|
||||
accColumn[p + 2] = (eeData[40 + i] & 0x0F00) >> 8
|
||||
accColumn[p + 3] = (eeData[40 + i] & 0xF000) >> 12
|
||||
|
||||
for i in range(32):
|
||||
if accColumn[i] > 7:
|
||||
accColumn[i] -= 16
|
||||
for i in range(24):
|
||||
for j in range(32):
|
||||
p = 32 * i + j
|
||||
alphaTemp[p] = (eeData[64 + p] & 0x03F0) >> 4
|
||||
if alphaTemp[p] > 31:
|
||||
alphaTemp[p] -= 64
|
||||
alphaTemp[p] *= 1 << accRemScale
|
||||
alphaTemp[p] += (
|
||||
alphaRef
|
||||
+ (accRow[i] << accRowScale)
|
||||
+ (accColumn[j] << accColumnScale)
|
||||
)
|
||||
alphaTemp[p] /= math.pow(2, alphaScale)
|
||||
alphaTemp[p] -= self.tgc * (self.cpAlpha[0] + self.cpAlpha[1]) / 2
|
||||
alphaTemp[p] = SCALEALPHA / alphaTemp[p]
|
||||
# print("alphaTemp: ", alphaTemp)
|
||||
|
||||
temp = max(alphaTemp)
|
||||
#print("temp", temp)
|
||||
|
||||
alphaScale = 0
|
||||
while temp < 32768:
|
||||
temp *= 2
|
||||
alphaScale += 1
|
||||
|
||||
for i in range(768):
|
||||
temp = alphaTemp[i] * math.pow(2, alphaScale)
|
||||
self.alpha[i] = int(temp + 0.5)
|
||||
|
||||
self.alphaScale = alphaScale
|
||||
|
||||
def _ExtractOffsetParameters(self):
|
||||
# extract offset
|
||||
occRow = [0] * 24
|
||||
occColumn = [0] * 32
|
||||
|
||||
occRemScale = eeData[16] & 0x000F
|
||||
occColumnScale = (eeData[16] & 0x00F0) >> 4
|
||||
occRowScale = (eeData[16] & 0x0F00) >> 8
|
||||
offsetRef = eeData[17]
|
||||
if offsetRef > 32767:
|
||||
offsetRef -= 65536
|
||||
|
||||
for i in range(6):
|
||||
p = i * 4
|
||||
occRow[p + 0] = eeData[18 + i] & 0x000F
|
||||
occRow[p + 1] = (eeData[18 + i] & 0x00F0) >> 4
|
||||
occRow[p + 2] = (eeData[18 + i] & 0x0F00) >> 8
|
||||
occRow[p + 3] = (eeData[18 + i] & 0xF000) >> 12
|
||||
|
||||
for i in range(24):
|
||||
if occRow[i] > 7:
|
||||
occRow[i] -= 16
|
||||
|
||||
for i in range(8):
|
||||
p = i * 4
|
||||
occColumn[p + 0] = eeData[24 + i] & 0x000F
|
||||
occColumn[p + 1] = (eeData[24 + i] & 0x00F0) >> 4
|
||||
occColumn[p + 2] = (eeData[24 + i] & 0x0F00) >> 8
|
||||
occColumn[p + 3] = (eeData[24 + i] & 0xF000) >> 12
|
||||
|
||||
for i in range(32):
|
||||
if occColumn[i] > 7:
|
||||
occColumn[i] -= 16
|
||||
|
||||
for i in range(24):
|
||||
for j in range(32):
|
||||
p = 32 * i + j
|
||||
self.offset[p] = (eeData[64 + p] & 0xFC00) >> 10
|
||||
if self.offset[p] > 31:
|
||||
self.offset[p] -= 64
|
||||
self.offset[p] *= 1 << occRemScale
|
||||
self.offset[p] += (
|
||||
offsetRef
|
||||
+ (occRow[i] << occRowScale)
|
||||
+ (occColumn[j] << occColumnScale)
|
||||
)
|
||||
|
||||
def _ExtractKtaPixelParameters(self): # pylint: disable=too-many-locals
|
||||
# extract KtaPixel
|
||||
KtaRC = [0] * 4
|
||||
ktaTemp = [0] * 768
|
||||
|
||||
KtaRoCo = (eeData[54] & 0xFF00) >> 8
|
||||
if KtaRoCo > 127:
|
||||
KtaRoCo -= 256
|
||||
KtaRC[0] = KtaRoCo
|
||||
|
||||
KtaReCo = eeData[54] & 0x00FF
|
||||
if KtaReCo > 127:
|
||||
KtaReCo -= 256
|
||||
KtaRC[2] = KtaReCo
|
||||
|
||||
KtaRoCe = (eeData[55] & 0xFF00) >> 8
|
||||
if KtaRoCe > 127:
|
||||
KtaRoCe -= 256
|
||||
KtaRC[1] = KtaRoCe
|
||||
|
||||
KtaReCe = eeData[55] & 0x00FF
|
||||
if KtaReCe > 127:
|
||||
KtaReCe -= 256
|
||||
KtaRC[3] = KtaReCe
|
||||
|
||||
ktaScale1 = ((eeData[56] & 0x00F0) >> 4) + 8
|
||||
ktaScale2 = eeData[56] & 0x000F
|
||||
|
||||
for i in range(24):
|
||||
for j in range(32):
|
||||
p = 32 * i + j
|
||||
split = 2 * (p // 32 - (p // 64) * 2) + p % 2
|
||||
ktaTemp[p] = (eeData[64 + p] & 0x000E) >> 1
|
||||
if ktaTemp[p] > 3:
|
||||
ktaTemp[p] -= 8
|
||||
ktaTemp[p] *= 1 << ktaScale2
|
||||
ktaTemp[p] += KtaRC[split]
|
||||
ktaTemp[p] /= math.pow(2, ktaScale1)
|
||||
# ktaTemp[p] = ktaTemp[p] * mlx90640->offset[p];
|
||||
|
||||
temp = abs(ktaTemp[0])
|
||||
for kta in ktaTemp:
|
||||
temp = max(temp, abs(kta))
|
||||
|
||||
ktaScale1 = 0
|
||||
while temp < 64:
|
||||
temp *= 2
|
||||
ktaScale1 += 1
|
||||
|
||||
for i in range(768):
|
||||
temp = ktaTemp[i] * math.pow(2, ktaScale1)
|
||||
if temp < 0:
|
||||
self.kta[i] = int(temp - 0.5)
|
||||
else:
|
||||
self.kta[i] = int(temp + 0.5)
|
||||
self.ktaScale = ktaScale1
|
||||
|
||||
def _ExtractKvPixelParameters(self):
|
||||
KvT = [0] * 4
|
||||
kvTemp = [0] * 768
|
||||
|
||||
KvRoCo = (eeData[52] & 0xF000) >> 12
|
||||
if KvRoCo > 7:
|
||||
KvRoCo -= 16
|
||||
KvT[0] = KvRoCo
|
||||
|
||||
KvReCo = (eeData[52] & 0x0F00) >> 8
|
||||
if KvReCo > 7:
|
||||
KvReCo -= 16
|
||||
KvT[2] = KvReCo
|
||||
|
||||
KvRoCe = (eeData[52] & 0x00F0) >> 4
|
||||
if KvRoCe > 7:
|
||||
KvRoCe -= 16
|
||||
KvT[1] = KvRoCe
|
||||
|
||||
KvReCe = eeData[52] & 0x000F
|
||||
if KvReCe > 7:
|
||||
KvReCe -= 16
|
||||
KvT[3] = KvReCe
|
||||
|
||||
kvScale = (eeData[56] & 0x0F00) >> 8
|
||||
|
||||
for i in range(24):
|
||||
for j in range(32):
|
||||
p = 32 * i + j
|
||||
split = 2 * (p // 32 - (p // 64) * 2) + p % 2
|
||||
kvTemp[p] = KvT[split]
|
||||
kvTemp[p] /= math.pow(2, kvScale)
|
||||
# kvTemp[p] = kvTemp[p] * mlx90640->offset[p];
|
||||
|
||||
temp = abs(kvTemp[0])
|
||||
for kv in kvTemp:
|
||||
temp = max(temp, abs(kv))
|
||||
|
||||
kvScale = 0
|
||||
while temp < 64:
|
||||
temp *= 2
|
||||
kvScale += 1
|
||||
|
||||
for i in range(768):
|
||||
temp = kvTemp[i] * math.pow(2, kvScale)
|
||||
if temp < 0:
|
||||
self.kv[i] = int(temp - 0.5)
|
||||
else:
|
||||
self.kv[i] = int(temp + 0.5)
|
||||
self.kvScale = kvScale
|
||||
|
||||
def _ExtractCILCParameters(self):
|
||||
ilChessC = [0] * 3
|
||||
|
||||
self.calibrationModeEE = (eeData[10] & 0x0800) >> 4
|
||||
self.calibrationModeEE = self.calibrationModeEE ^ 0x80
|
||||
|
||||
ilChessC[0] = eeData[53] & 0x003F
|
||||
if ilChessC[0] > 31:
|
||||
ilChessC[0] -= 64
|
||||
ilChessC[0] /= 16.0
|
||||
|
||||
ilChessC[1] = (eeData[53] & 0x07C0) >> 6
|
||||
if ilChessC[1] > 15:
|
||||
ilChessC[1] -= 32
|
||||
ilChessC[1] /= 2.0
|
||||
|
||||
ilChessC[2] = (eeData[53] & 0xF800) >> 11
|
||||
if ilChessC[2] > 15:
|
||||
ilChessC[2] -= 32
|
||||
ilChessC[2] /= 8.0
|
||||
|
||||
self.ilChessC = ilChessC
|
||||
|
||||
def _ExtractDeviatingPixels(self):
|
||||
self.brokenPixels = [0xFFFF] * 5
|
||||
self.outlierPixels = [0xFFFF] * 5
|
||||
|
||||
pixCnt = 0
|
||||
brokenPixCnt = 0
|
||||
outlierPixCnt = 0
|
||||
|
||||
while (pixCnt < 768) and (brokenPixCnt < 5) and (outlierPixCnt < 5):
|
||||
if eeData[pixCnt + 64] == 0:
|
||||
self.brokenPixels[brokenPixCnt] = pixCnt
|
||||
brokenPixCnt += 1
|
||||
elif (eeData[pixCnt + 64] & 0x0001) != 0:
|
||||
self.outlierPixels[outlierPixCnt] = pixCnt
|
||||
outlierPixCnt += 1
|
||||
pixCnt += 1
|
||||
|
||||
if brokenPixCnt > 4:
|
||||
raise RuntimeError("More than 4 broken pixels")
|
||||
if outlierPixCnt > 4:
|
||||
raise RuntimeError("More than 4 outlier pixels")
|
||||
if (brokenPixCnt + outlierPixCnt) > 4:
|
||||
raise RuntimeError("More than 4 faulty pixels")
|
||||
# print("Found %d broken pixels, %d outliers" % (brokenPixCnt, outlierPixCnt))
|
||||
# TODO INCOMPLETE
|
||||
|
||||
def _I2CWriteWord(self, writeAddress, data):
|
||||
cmd = bytearray(4)
|
||||
cmd[0] = writeAddress >> 8
|
||||
cmd[1] = writeAddress & 0x00FF
|
||||
cmd[2] = data >> 8
|
||||
cmd[3] = data & 0x00FF
|
||||
dataCheck = [0]
|
||||
|
||||
with self.i2c_device as i2c:
|
||||
i2c.write(cmd)
|
||||
# print("Wrote:", [hex(i) for i in cmd])
|
||||
time.sleep(0.001)
|
||||
self._I2CReadWords(writeAddress, dataCheck)
|
||||
# print("dataCheck: 0x%x" % dataCheck[0])
|
||||
# if (dataCheck != data):
|
||||
# return -2
|
||||
|
||||
_inbuf = bytearray(2 * I2C_READ_LEN)
|
||||
|
||||
def _I2CReadWords(self, addr, buffer, *, end=None):
|
||||
# stamp = time.monotonic()
|
||||
if end is None:
|
||||
remainingWords = len(buffer)
|
||||
else:
|
||||
remainingWords = end
|
||||
offset = 0
|
||||
addrbuf = bytearray(2)
|
||||
# inbuf = bytearray(2 * I2C_READ_LEN)
|
||||
inbuf = self._inbuf
|
||||
|
||||
with self.i2c_device as i2c:
|
||||
while remainingWords:
|
||||
addrbuf[0] = addr >> 8 # MSB
|
||||
addrbuf[1] = addr & 0xFF # LSB
|
||||
read_words = min(remainingWords, I2C_READ_LEN)
|
||||
i2c.write_then_readinto(
|
||||
addrbuf, inbuf, in_end=read_words * 2
|
||||
) # in bytes
|
||||
# print("-> ", [hex(i) for i in addrbuf])
|
||||
|
||||
outwords = struct.unpack(
|
||||
">" + "H" * read_words, inbuf[0 : read_words * 2]
|
||||
)
|
||||
# print("<- (", read_words, ")", [hex(i) for i in outwords])
|
||||
for i, w in enumerate(outwords):
|
||||
buffer[offset + i] = w
|
||||
offset += read_words
|
||||
remainingWords -= read_words
|
||||
addr += read_words
|
||||
|
||||
ixc = None
|
||||
mlx = None
|
||||
frame = None
|
||||
|
||||
def init_camera(scl_pin=22, sda_pin=21, freq=100000):
|
||||
"""Explicitly initializes the I2C bus and camera after power is stable."""
|
||||
global ixc, mlx, frame
|
||||
|
||||
print(f"[Camera] Initializing I2C on SCL:{scl_pin}, SDA:{sda_pin} at {freq}Hz...")
|
||||
ixc = I2C(pins=(scl_pin, sda_pin), frequency=freq)
|
||||
|
||||
print("[Camera] Probing for MLX90640...")
|
||||
mlx = MLX90640(ixc)
|
||||
|
||||
# Bonus: Your wiki snapshot recommends 16Hz for smooth images!
|
||||
mlx.refresh_rate = RefreshRate.REFRESH_16_HZ
|
||||
|
||||
frame = [0] * 768
|
||||
print("[Camera] Setup successful!")
|
||||
|
||||
def read_temperature():
|
||||
if mlx is None:
|
||||
print("[Camera] Error: Camera not initialized. Call init_camera() first.")
|
||||
return None
|
||||
try:
|
||||
print("Querying camera...")
|
||||
mlx.getFrame(frame)
|
||||
return frame
|
||||
except Exception as e:
|
||||
print(f"[Camera] Read error: {e}")
|
||||
return None
|
||||
Reference in New Issue
Block a user