3d_audio/lib_sw_pll/python/sw_pll/controller_model.py

# Copyright 2023 XMOS LIMITED.
# This Software is subject to the terms of the XMOS Public Licence: Version 1.

from sw_pll.dco_model import lut_dco, sigma_delta_dco, lock_count_threshold
import numpy as np


class pi_ctrl():
    """
    Parent PI(I) controller class
    """
    def __init__(self,  Kp, Ki, Kii=None, i_windup_limit=None, ii_windup_limit=None, verbose=False):
        self.Kp     = Kp
        self.Ki     = Ki
        self.Kii    = 0.0 if Kii is None else Kii
        self.i_windup_limit = i_windup_limit
        self.ii_windup_limit = ii_windup_limit

        self.error_accum = 0.0          # Integral of error
        self.error_accum_accum = 0.0    # Double integral of error (optional)
        self.total_error = 0.0          # Calculated total error

        self.verbose = verbose

        if verbose:
            print(f"Init sw_pll_pi_ctrl, Kp: {Kp} Ki: {Ki} Kii: {Kii}")

    def _reset_controller(self):
        """
        Reset any accumulated state
        """
        self.error_accum = 0.0
        self.error_accum_accum = 0.0
        self.total_error = 0.0

    def do_control_from_error(self, error):
        """
        Calculate the LUT setting from the input error
        """

        # clamp integral terms to stop them irrecoverably drifting off.
        if self.i_windup_limit is None:
            self.error_accum = self.error_accum + error
        else:
            self.error_accum = np.clip(self.error_accum + error, -self.i_windup_limit, self.i_windup_limit) 
        
        if self.ii_windup_limit is None:
            self.error_accum_accum = self.error_accum_accum + self.error_accum
        else:
            self.error_accum_accum = np.clip(self.error_accum_accum + self.error_accum, -self.ii_windup_limit, self.ii_windup_limit) 
    
        error_p  = self.Kp * error;
        error_i  = self.Ki * self.error_accum
        error_ii = self.Kii * self.error_accum_accum

        self.total_error = error_p + error_i + error_ii

        if self.verbose:
            print(f"error: {error} error_p: {error_p} error_i: {error_i} error_ii: {error_ii} total error: {self.total_error}")

        return self.total_error



##############################
# LOOK UP TABLE IMPLEMENTATION
##############################

class lut_pi_ctrl(pi_ctrl):
    """
        This class instantiates a control loop instance. It takes a lookup table function which can be generated 
        from the error_from_h class which allows it use the actual pre-calculated transfer function.
        Once instantiated, the do_control method runs the control loop.

        This class forms the core of the simulator and allows the constants (K..) to be tuned to acheive the 
        desired response. The function run_sim allows for a plot of a step resopnse input which allows this
        to be done visually.
    """
    def __init__(self, Kp, Ki, Kii=None, base_lut_index=None, verbose=False):
        """
        Create instance absed on specific control constants
        """        
        self.dco = lut_dco()
        self.lut_lookup_function = self.dco.get_lut()
        lut_size = self.dco.get_lut_size()
        self.diff = 0.0                 # Most recent diff between expected and actual. Used by tests


        # By default set the nominal LUT index to half way
        if base_lut_index is None:
            base_lut_index = lut_size // 2
        self.base_lut_index = base_lut_index

        # Set windup limit to the lut_size, which by default is double of the deflection from nominal
        i_windup_limit     = lut_size / Ki if Ki != 0.0 else 0.0
        ii_windup_limit    = 0.0 if Kii is None else lut_size / Kii if Kii != 0.0 else 0.0

        pi_ctrl.__init__(self, Kp, Ki, Kii=Kii, i_windup_limit=i_windup_limit, ii_windup_limit=ii_windup_limit, verbose=verbose)

        self.verbose = verbose

        if verbose:
            print(f"Init lut_pi_ctrl, Kp: {Kp} Ki: {Ki} Kii: {Kii}")

    def get_dco_control_from_error(self, error, first_loop=False):
        """
        Calculate the LUT setting from the input error
        """
        self.diff = error # Used by tests

        if first_loop:
            pi_ctrl._reset_controller(self)
            error = 0.0

        dco_ctrl = self.base_lut_index - pi_ctrl.do_control_from_error(self, error)

        return None if first_loop else dco_ctrl 


######################################
# SIGMA DELTA MODULATOR IMPLEMENTATION
######################################

class sdm_pi_ctrl(pi_ctrl):
    def __init__(self, mod_init, sdm_in_max, sdm_in_min, Kp, Ki, Kii=None, verbose=False):
        """
        Create instance absed on specific control constants
        """        
        pi_ctrl.__init__(self, Kp, Ki, Kii=Kii, verbose=verbose)

        # Low pass filter state
        self.alpha = 0.125
        self.iir_y = 0

        # Nominal setting for SDM
        self.initial_setting = mod_init

        # Limits for SDM output
        self.sdm_in_max = sdm_in_max
        self.sdm_in_min = sdm_in_min

        # Lock status state
        self.lock_status = -1
        self.lock_count = lock_count_threshold

    def do_control_from_error(self, error):
        """
        Run the control loop. Also contains an additional
        low passs filtering stage.
        """
        x = pi_ctrl.do_control_from_error(self, -error)
        x = int(x)

        # Filter noise into DCO to reduce jitter
        # First order IIR, make A=0.125
        # y = y + A(x-y)

        # self.iir_y = int(self.iir_y + (x - self.iir_y) * self.alpha)
        self.iir_y += (x - self.iir_y) >> 3 # This matches the firmware

        sdm_in = self.initial_setting + self.iir_y


        if sdm_in > self.sdm_in_max:
            print(f"SDM Pos clip: {sdm_in}, {self.sdm_in_max}")
            sdm_in = self. sdm_in_max
            self.lock_status = 1
            self.lock_count = lock_count_threshold

        elif sdm_in < self.sdm_in_min:
            print(f"SDM Neg clip: {sdm_in}, {self.sdm_in_min}")
            sdm_in = self.sdm_in_min
            self.lock_status = -1
            self.lock_count = lock_count_threshold

        else:
            if self.lock_count > 0:
                self.lock_count -= 1
            else:
                self.lock_status = 0

        return sdm_in, self.lock_status 


if __name__ == '__main__':
    """
    This module is not intended to be run directly. This is here for internal testing only.
    """
    Kp = 1.0
    Ki = 0.1
    Kii = 0.0
    
    sw_pll = lut_pi_ctrl(Kp, Ki, Kii=Kii, verbose=True)
    for error_input in range(-10, 20):
        dco_ctrl = sw_pll.do_control_from_error(error_input)

    mod_init = (sigma_delta_dco.sdm_in_max + sigma_delta_dco.sdm_in_min) / 2
    Kp = 0.0
    Ki = 0.1
    Kii = 0.1

    sw_pll = sdm_pi_ctrl(mod_init, sigma_delta_dco.sdm_in_max, sigma_delta_dco.sdm_in_min, Kp, Ki, Kii=Kii, verbose=True)
    for error_input in range(-10, 20):
        dco_ctrl, lock_status = sw_pll.do_control_from_error(error_input)