Merge pull request #1084 from lnls-sirius/acc_filter

Add FOFB accumulator filter

siriuspy/siriuspy/VERSION

@@ -1 +1 @@
-2.87.1
+2.88.0

siriuspy/siriuspy/devices/fofb.py

@@ -39,6 +39,7 @@ class FOFBCtrlRef(_Device, FOFBCtrlBase):
         'LoopIntlk-Mon', 'LoopIntlkReset-Cmd',
         'SYSIDPRBSFOFBAccEn-Sel', 'SYSIDPRBSFOFBAccEn-Sts',
         'SYSIDPRBSBPMPosEn-Sel', 'SYSIDPRBSBPMPosEn-Sts',
+        'FOFBAccFilterNumBiquads-Cte',
         )
 
     def __init__(self, devname, props2init='all'):
@@ -141,6 +142,10 @@ def cmd_reset(self):
         self['LoopIntlkReset-Cmd'] = 1
         return True
 
+    @property
+    def fofbacc_filter_num_biquads(self):
+        """FOFB accumulator filter number of biquads."""
+        return self['FOFBAccFilterNumBiquads-Cte']
 
 # ---------------- DCC devices ----------------
 
@@ -690,6 +695,13 @@ def cmd_dsbl_sysid_exc(self, timeout=DEF_TIMEOUT):
         self._evt_fofb.cmd_external_trigger()
         return True
 
+    @property
+    def fofbacc_filter_num_biquads(self):
+        """FOFB accumulator filter number of biquads."""
+        ctl = self._ctl_refs[FOFBCtrlBase.DEVICES.SI01]
+        return ctl.fofbacc_filter_num_biquads
+
+
 
 class FamFastCorrs(_DeviceSet):
     """Family of FOFB fast correctors."""
@@ -702,6 +714,8 @@ class FamFastCorrs(_DeviceSet):
     DEF_ATOL_FOFBACCSAT = 2e-2
     DEF_ATOL_CURRENT_RB = 1e-6
     DEF_ATOL_CURRENT_MON = 2e-2
+    DEF_ATOL_ACCFILTER = 2**-17
+    DEF_ATOL_ACCFILTERGAIN = 2**-12
 
     def __init__(self, psnames=None):
         """Init."""
@@ -1071,6 +1085,55 @@ def cmd_fofbacc_clear(self, psnames=None, psindices=None):
             dev.cmd_fofbacc_clear()
         return True
 
+    def set_fofbacc_filter(self, value, psnames=None, psindices=None):
+        """Command to set power supply filter coefficients values."""
+        if not isinstance(value, (list, tuple, _np.ndarray)):
+            raise ValueError('Value must be iterable.')
+        devs = self._get_devices(psnames, psindices)
+        for dev in devs:
+            dev.fofbacc_filter = value
+        return True
+
+    def check_fofbacc_filter(
+            self, value, psnames=None, psindices=None,
+            atol=DEF_ATOL_ACCFILTER):
+        """Check power supplies filter coefficients."""
+        if not self.connected:
+            return False
+        if not isinstance(value, (list, tuple, _np.ndarray)):
+            raise ValueError('Value must be iterable.')
+        devs = self._get_devices(psnames, psindices)
+        for dev in devs:    
+            if len(value) != len(dev.fofbacc_filter):
+                return False
+            if not _np.allclose(value, dev.fofbacc_filter, atol=atol):
+                return False
+        return True
+
+    def set_fofbacc_filter_gain(self, value, psnames=None, psindices=None):
+        """Command to set accumulator filter gain."""
+        if not isinstance(value, (int, float)):
+            raise ValueError('Value must be integer or float.')
+        devs = self._get_devices(psnames, psindices)
+        for dev in devs:
+            dev.fofbacc_filter_gain = value
+        return True
+
+    def check_fofbacc_filter_gain(
+            self, value, psnames=None, psindices=None,
+            atol=DEF_ATOL_ACCFILTERGAIN):
+        """Check accumulator filter gain."""
+        if not self.connected:
+            return False
+        if not isinstance(value, (int, float)):
+            raise ValueError('Value must be integer or float.')
+        devs = self._get_devices(psnames, psindices)
+        impltd = _np.asarray([d.fofbacc_filter_gain for d in devs])
+        value = len(devs) * [value]
+        if _np.allclose(value, impltd, atol=atol):
+            return True
+        return False
+
     # ----- private methods -----
 
     def _get_devices(self, names, indices):

siriuspy/siriuspy/devices/pwrsupply.py

@@ -65,6 +65,8 @@ class _PSDev(_Device):
         'FOFBAccSatMin-SP', 'FOFBAccSatMin-RB',
         'FOFBAcc-Mon',
         'FOFBAccDecimation-SP', 'FOFBAccDecimation-RB',
+        'FOFBAccFilter-SP', 'FOFBAccFilter-RB', 
+        'FOFBAccFilterGain-SP', 'FOFBAccFilterGain-RB',
     )
     _properties_pulsed = (
         'Voltage-SP', 'Voltage-RB', 'Voltage-Mon',
@@ -1031,6 +1033,23 @@ def cmd_fofbacc_clear(self):
         self['FOFBAccClear-Cmd'] = 1
         return True
 
+    @property
+    def fofbacc_filter(self):
+        """FOFB accumulator filter."""
+        return self['FOFBAccFilter-RB']
+
+    @fofbacc_filter.setter
+    def fofbacc_filter(self, value):
+        self['FOFBAccFilter-SP'] = value
+
+    @property
+    def fofbacc_filter_gain(self):
+        """FOFB accumulator filter gain."""
+        return self['FOFBAccFilterGain-RB']
+
+    @fofbacc_filter_gain.setter
+    def fofbacc_filter_gain(self, value):
+        self['FOFBAccFilterGain-SP'] = value
 
 class PowerSupplyFBP(PowerSupply):
     """FBP Power Supply Device."""

siriuspy/siriuspy/fofb/csdev.py

@@ -34,6 +34,7 @@ class ETypes(_csdev.ETypes):
         'LoopInterlockOk', 'SYSIDExcitationDisabled')
 
     DEC_OPT = ('FOFB', 'Monit', 'Custom')
+    FILTER_OPT = ('Unit', 'Switching', 'Custom')
 
 
 _et = ETypes  # syntactic sugar
@@ -67,7 +68,18 @@ class HLFOFBConst(_csdev.Const):
     MeasRespMatCmd = _csdev.Const.register('MeasRespMatCmd', _et.MEAS_RMAT_CMD)
     MeasRespMatMon = _csdev.Const.register('MeasRespMatMon', _et.MEAS_RMAT_MON)
     DecOpt = _csdev.Const.register('DecOpt', _et.DEC_OPT)
-
+    FilterOpt = _csdev.Const.register('FilterOpt', _et.FILTER_OPT)
+
+    # FOFB Switching filter coefficientes
+    _b4 = [9.10339395e-01, -1.11484423e-16, 9.10339395e-01] # freq FOFB/4
+    _a4 = [-1.11484423e-16, 8.20678791e-01] # freq FOFB/4
+    _b2 = [0.83408931895964943947774372645654, 0.83408931895964943947774372645654, 0.0] # freq FOFB/2
+    _a2 = [0.66817863791929887895548745291308, 0.0] # freq FOFB/2
+
+    FILTER_UNIT = [1.0, 0.0, 0.0, 0.0, 0.0]
+    FILTER_SW_4 = _b4 + _a4 
+    FILTER_SW_2 = _b2 + _a2
+
     def __init__(self):
         """Class constructor."""
 
@@ -257,6 +269,26 @@ def get_hlfofb_database(self):
             'FOFBAccDecimation-RB': {
                 'type': 'float', 'value': 1, 'prec': 0, 'lolim': 1,
                 'hilim': 8600, 'unit': 'count'},
+
+            # filter configuration
+            'FOFBAccFilter-Sel': {
+                'type': 'enum', 'enums': _et.FILTER_OPT,
+                'value': self.FilterOpt.Unit, 'unit': 'Unit_Switching_Custom'},
+            'FOFBAccFilter-Sts': {
+                'type': 'enum', 'enums': _et.FILTER_OPT,
+                'value': self.FilterOpt.Unit, 'unit': 'Unit_Switching_Custom'},
+            'FOFBAccFilter-SP': {
+                'type': 'float', 'value': 20*[0], 'prec': 5, 'count': 20, 
+                'unit': 'coef'},
+            'FOFBAccFilter-RB': {
+                'type': 'float', 'value': 20*[0], 'prec': 5, 'count': 20,
+                'unit': 'coef'},
+            'FOFBAccFilterGain-SP': {
+                'type': 'float', 'value': 1.0, 'prec': 5,  
+                'lolim': 0, 'hilim': 1000,'unit': 'gain'},
+            'FOFBAccFilterGain-RB': {
+                'type': 'float', 'value': 1.0, 'prec': 5, 
+                'unit': 'gain'},
 
             # Reference Orbit (same order of SOFB)
             'RefOrbX-SP': {

siriuspy/siriuspy/fofb/main.py

@@ -72,6 +72,9 @@ def __init__(self, tests=False):
         }
         self._corr_accdec_val = 1
         self._corr_accdec_enm = self._const.DecOpt.FOFB
+        self._corr_accfilter_val = 20*[0.0]
+        self._corr_accfilter_enm = self._const.FilterOpt.Unit
+        self._corr_accfilter_gain = 1.0
         self._thread_enbllist = None
         self._abort_thread_enbllist = False
         self._min_sing_val = self._const.MIN_SING_VAL
@@ -152,6 +155,9 @@ def __init__(self, tests=False):
             'CtrlrDsblSYSIDExc-Cmd': self.cmd_fofbctrl_dsblsysid,
             'FOFBAccDecimation-Sel': _part(self.set_corr_accdec, 'enum'),
             'FOFBAccDecimation-SP': _part(self.set_corr_accdec, 'value'),
+            'FOFBAccFilter-Sel':  _part(self.set_corr_accfilter, 'enum'),
+            'FOFBAccFilter-SP': _part(self.set_corr_accfilter, 'value'),
+            'FOFBAccFilterGain-SP': self.set_corr_accfilter_gain,
             'RefOrbX-SP': _part(self.set_reforbit, 'x'),
             'RefOrbY-SP': _part(self.set_reforbit, 'y'),
             'RespMat-SP': self.set_respmat,
@@ -703,6 +709,53 @@ def set_corr_accdec(self, option, value):
         self._update_log('...done!')
 
         return True
+
+    def set_corr_accfilter(self, option, value):
+        """Set corrector accumulator filter."""
+
+        num_biquads = self._llfofb_dev.fofbacc_filter_num_biquads
+        unit = self._const.FILTER_UNIT
+        sw2 = self._const.FILTER_SW_2
+        sw4 = self._const.FILTER_SW_4
+
+        if self._corr_accfilter_enm != self._const.FilterOpt.Custom and \
+                option == 'value':
+            return False
+
+        if option == 'enum':
+            if value == self._const.FilterOpt.Unit:
+                filter = num_biquads * unit
+
+            elif value == self._const.FilterOpt.Switching:
+                filter = sw2 + sw4 + (num_biquads - 2) * unit
+
+            else:
+                filter = self._corr_accfilter_val
+            self._corr_accfilter_enm = value
+            self.run_callbacks('FOFBAccFilter-Sts', value)
+            self.run_callbacks('FOFBAccFilter-SP', filter)
+        else:
+            filter = value
+        self._corr_accfilter_val = filter
+
+        self._update_log('Setting FOFB Acc filter...')
+        self._corrs_dev.set_fofbacc_filter(filter)
+        self._update_log('...done!')
+        self.run_callbacks('FOFBAccFilter-RB', filter)
+
+        return True
+
+    def set_corr_accfilter_gain(self, value):
+        """Set corrector accumulator filter gain."""
+
+        self._corr_accfilter_gain = value
+
+        self._update_log('Setting FOFB Acc filter gain...')
+        self._corrs_dev.set_fofbacc_filter_gain(value)
+        self._update_log('...done!')
+        self.run_callbacks('FOFBAccFilterGain-RB', value)
+
+        return True
 
     def _thread_corr_currzero(self):
         if self._corrs_dev.check_current(0):
@@ -1834,8 +1887,14 @@ def _check_corrs_configs(self):
                 dec = self._corr_accdec_val
                 if not self._corrs_dev.check_fofbacc_decimation(dec):
                     value = _updt_bit(value, 7, 1)
+                # AccFilter
+                if not self._corrs_dev.check_fofbacc_filter(self._corr_accfilter_val):
+                    value = _updt_bit(value, 8, 1)
+                # AccFilterGain
+                if not self._corrs_dev.check_fofbacc_filter_gain(self._corr_accfilter_gain):
+                    value = _updt_bit(value, 9, 1)
             else:
-                value = 0b11111111
+                value = 0b1111111111
 
             self._corr_status = value
             self.run_callbacks('CorrStatus-Mon', self._corr_status)