abse/isv57t090-motion-controller

examples/ISV57T090MotionController/firmware/thermal_guard.py

"""Fail-closed thermal policy for the iSV57T-090 RP2040 controller.

The remote 10 kOhm B3950 NTC must be bonded to the motor heatsink and wired
between J_IO pin 5 and pin 6 (GND). The onboard TMP117 monitors PCB temperature.
The motor manual limits heatsink temperature to 70 C; the remote trip is 65 C.
"""

import math


NTC_PULLUP_OHMS = 10000.0
NTC_NOMINAL_OHMS = 10000.0
NTC_BETA_K = 3950.0
NTC_NOMINAL_K = 298.15
REMOTE_TRIP_C = 65.0
BOARD_TRIP_C = 75.0
MIN_PLAUSIBLE_C = -20.0
MAX_PLAUSIBLE_C = 125.0
ADC_MIN_VALID = 1000
ADC_MAX_VALID = 64535


def tmp117_temperature_c(msb, lsb):
    """Decode TMP117 temperature register 0x00 (signed, 1/128 C per bit)."""
    raw = (msb << 8) | lsb
    if raw & 0x8000:
        raw -= 0x10000
    return raw / 128.0


def remote_ntc_temperature_c(adc_u16):
    """Return degrees C, or None for an open/shorted/unreadable sensor."""
    if adc_u16 is None or not ADC_MIN_VALID < adc_u16 < ADC_MAX_VALID:
        return None
    resistance = NTC_PULLUP_OHMS * adc_u16 / (65535.0 - adc_u16)
    inverse_k = (1.0 / NTC_NOMINAL_K) + math.log(
        resistance / NTC_NOMINAL_OHMS
    ) / NTC_BETA_K
    if inverse_k <= 0:
        return None
    return (1.0 / inverse_k) - 273.15


class ThermalGuard:
    """Never re-arm after a fault until the RP2040 is intentionally reset."""

    def __init__(self, startup_samples=10, alarm_startup_samples=40):
        self.startup_samples = startup_samples
        self.alarm_startup_samples = alarm_startup_samples
        self.healthy_samples = 0
        self.alarm_wait_samples = 0
        self.ready = False
        self.fault = None

    def trip(self, reason):
        if self.fault is None:
            self.fault = reason
        return False

    def update(self, board_c, remote_c, estop_closed, alarm_ok):
        if self.fault is not None:
            return False
        if not estop_closed:
            return self.trip("estop_open")
        if board_c is None or remote_c is None:
            return self.trip("temperature_sensor_missing")
        if not (MIN_PLAUSIBLE_C <= board_c <= MAX_PLAUSIBLE_C):
            return self.trip("board_temperature_invalid")
        if not (MIN_PLAUSIBLE_C <= remote_c <= MAX_PLAUSIBLE_C):
            return self.trip("remote_temperature_invalid")
        if board_c >= BOARD_TRIP_C:
            return self.trip("board_overheat")
        if remote_c >= REMOTE_TRIP_C:
            return self.trip("motor_overheat")

        # The motor's normally-low alarm is unpowered while the external
        # contactor is open. Energize the coil only after temperatures and
        # E-stop pass, then allow its alarm output time to become valid.
        if self.healthy_samples < self.startup_samples:
            self.healthy_samples += 1
            if self.healthy_samples < self.startup_samples:
                return False
        if not self.ready:
            if alarm_ok:
                self.ready = True
            else:
                self.alarm_wait_samples += 1
                if self.alarm_wait_samples > self.alarm_startup_samples:
                    return self.trip("motor_alarm_startup_timeout")
            return True
        if not alarm_ok:
            return self.trip("motor_alarm")
        return True