MustafaMulla29/rp2040-bldc-motor-controller-new

firmware/power_policy.py

"""Configurable-PD prototype policy. Units: mV/mA; no hardware side effects.

Input-current limits below are conservative bring-up ceilings, not motor phase
ratings or a validated thermal specification. Calibrate ADCs before loading.
"""
VOLTAGES_MV = (5000, 9000, 15000, 20000)
DEFAULT_MV = 9000
CONTRACT_MA = 2000
# Measured at the board, after cable loss. Motor UV margin is checked separately.
VBUS_WINDOWS = {5000: (4500, 5250), 9000: (8550, 9450),
                15000: (14250, 15750), 20000: (19000, 21000)}
MOTOR_MIN_MV = {5000: 4650, 9000: 8000, 15000: 13500, 20000: 18000}
INPUT_LIMIT_MA = {5000: 800, 9000: 1500, 15000: 1500, 20000: 1500}


def fixed_sink_pdo(millivolts, milliamps, usb_data=False):
    if millivolts not in VOLTAGES_MV or not 100 <= milliamps <= CONTRACT_MA:
        raise ValueError("Supported PD profiles: 5, 9, 15 and 20 V; at most 2 A")
    if millivolts % 50 or milliamps % 10:
        raise ValueError("PDO values must use 50 mV and 10 mA steps")
    return ((1 << 26) if usb_data else 0) | ((millivolts // 50) << 10) | (milliamps // 10)


def adc_millivolts(raw_u16, reference_mv=3300):
    """R11/R30=220k, R12/R31=15k. Nominal transfer; calibrate on hardware."""
    if not 0 <= raw_u16 <= 65535:
        raise ValueError("ADC sample out of range")
    return raw_u16 * reference_mv * 235 // (65535 * 15)


def valid_motor_contract(vbus_mv, rdo, pe_state, pd_ok2_low, selected_mv=DEFAULT_MV):
    """Require explicit selected PDO2, including PDO2=5 V (not fallback PDO1).

    STUSB4500 sets operating current to our request and maximum current to the
    SOURCE capability. A 3 A source is valid for our 2 A request. RDO object
    position indexes source capabilities, not our sink-PDO index.
    """
    if selected_mv not in VOLTAGES_MV:
        return False
    lo, hi = VBUS_WINDOWS[selected_mv]
    return (pd_ok2_low and pe_state == 0x18 and 1 <= ((rdo >> 28) & 7) <= 7
            and (rdo & (1 << 26)) == 0 and (rdo & (1 << 27)) == 0
            and ((rdo >> 10) & 1023) == CONTRACT_MA // 10
            and CONTRACT_MA // 10 <= (rdo & 1023) <= 500
            and lo <= vbus_mv <= hi)


def motor_permitted(vbus_mv, vmotor_mv, input_ma, rdo, pe_state, pd_ok2_low,
                    armed, fault_n, selected_mv=DEFAULT_MV):
    return (valid_motor_contract(vbus_mv, rdo, pe_state, pd_ok2_low, selected_mv)
            and MOTOR_MIN_MV[selected_mv] <= vmotor_mv <= VBUS_WINDOWS[selected_mv][1]
            and 0 <= input_ma < INPUT_LIMIT_MA[selected_mv]
            and armed and fault_n)