Subversion Repositories ssbcc

################################################################################
#
# Copyright 2015, Sinclair R.F., Inc.
#
################################################################################
 
import math
 
from ssbccPeripheral import SSBCCperipheral
from ssbccUtil import CeilLog2
from ssbccUtil import SSBCCException
 
class servo_motor(SSBCCperipheral):
  """
  Servo Motor driver:\n
  Creates PWM modulated signals to operate micro servos for UAVs and similar.\n
  Usage:
    PERIPHERAL servo_motor      outport=O_name                          \\
                                {outsignal|outsignaln}=o_name           \\
                                freq_hz=FREQ_HZ                         \\
                                min_width=XXX{s|ms|us|ns}               \\
                                max_width=XXX{s|ms|us|ns}               \\
                                [default_width=XXX{s|ms|us|ns}]         \\
                                {period=XXX{s|ms|us|ns}|sync=o_name}    \\
                                [inperiod=I_name]                       \\
                                [scale=C_name]                          \\
                                [scale_max=C_name]\n
  Where:
    outport=O_name
      specifies the symbol used to write the 8-bit PWM control to this
      peripheral
      Note:  The name must start with "O_".
    {outsignal|outsignaln}=o_name
      specifies the name of the output PWM signal
      Note:  outsignal creates a positive pulse for the PWM control and
             outsignaln creates an inverted pulse for the PWM control.
      Note:  The name must start with "o_".
    freq_hz=FREQ_HZ
      specifies the processor clock speed to the peripheral
    min_width=XXX{s|ms|us|ns}
      specifies the minimum pulse width
      Note:  XXX may be an integer or a real number
      Note:  The minimum width must be a realizable positive value (since a
             pulse width of zero means no control is being given to the servo).
    max_width=XXX{s|ms|us|ns}
      specifies the maximum pulse width
      Note:  XXX may be an integer or a real number
    default_width=XXX{s|ms|us|ns}
      optionally specifies the default width of the PWM before it is set by the
      processor
      Note:  If the default width is not specified then the minimum width is
             used as the default width.
    {period=XXX{s|ms|us|ns}|sync=o_name}
      either specifies the rate at which the PWM is generated or synchronize the
      PWM generation to a preceding servo_motor peripheral with the output
      signal o_name
      Note:  XXX may be an integer or a real number
      Note:  When sync is specified the leading edges of the PWMs will coincide.
    inperiod=I_name
      optionally specifies an input port to receive the strobe generated by the
      associated period
      Note:  This optional parameter requires that period be specified.  It is
             not compatible with the sync parameter.
    scale=C_name
      optionally creates a constant named "C_name" which states how many clock
      cycles are used for each count in the 8-bit PWM control
      Note:  The name must start with "C_".
      Example:  A PWM range of 1000 to 1500 usec and a clock frequency of 8 MHz
                produces a value of ceil((1500-1000)*8/(256-1)) = 16 clock
                cycles per 8-bit control value count.
    scale_max=C_name
      optionally creates a constant named "C_name" wich states the upper limit
      of the continuous control range
      Note:  The name must start with "C_".
      Example:  For the example in "scale=C_name" the upper limit would be
                ceil((1500-1000)*8/16) = 250.  I.e., control values of 251
                through 255 inclusive will produce the same PWM width as the
                control value 250.
  Example:
    A micro servo that responds to positive PWM pulses between 1000 usec and
    1500 usec once every 20 msec and the processor clock is 8 MHz.  The
    architecture file would include the following:\n
      CONSTANT          C_FREQ_HZ       8_000_000
      PORTCOMMENT       servo motor control
      PERIPHERAL        servo_motor    outport=O_SERVO                  \\
                                       outsignal=o_servo                \\
                                       freq_hz=C_FREQ_HZ                \\
                                       min_width=1000us                 \\
                                       max_width=1500us                 \\
                                       period=20ms                      \\
                                       scale=C_servo_scale              \\
                                       scale_max=C_servo_scale_max\n
    will create a peripheral generating the desired PWM signal.\n
    The constants C_servo_scale and C_servo_scale_max could be reported by the
    micro controller to a controlling application to specify the sensitivity and
    upper limit for the servo motor controller.\n
  Example:
    Synchronize a second servo motor with PWM pulses between 1000 usec and 2500
    usec to the preceding servo motor controller:\n
      PERIPHERAL        servo_motor     outport=O_SERVO_2               \\
                                        outsignal=o_servo2              \\
                                        freq_hz=C_FREQ_HZ               \\
                                        min_width=1.0ms                 \\
                                        max_width=2.5ms                 \\
                                        sync=o_servo\n
  """
 
  def __init__(self,peripheralFile,config,param_list,loc):
    # Use the externally provided file name for the peripheral
    self.peripheralFile = peripheralFile;
    # Get the parameters.
    allowables = (
      ( 'default_width',        r'\S+',         lambda v : self.TimeMethod(config,v), ),
      ( 'freq_hz',              r'\S+$',        lambda v : self.IntMethod(config,v), ),
      ( 'inperiod',             r'I_\w+$',      None,   ),
      ( 'max_width',            r'\S+$',        lambda v : self.TimeMethod(config,v), ),
      ( 'min_width',            r'\S+$',        lambda v : self.TimeMethod(config,v), ),
      ( 'outport',              r'O_\w+$',      None,   ),
      ( 'outsignal',            r'o_\w+$',      None,   ),
      ( 'outsignaln',           r'o_\w+$',      None,   ),
      ( 'period',               r'\S+$',        lambda v : self.TimeMethod(config,v), ),
      ( 'scale',                r'C_\w+$',      None,   ),
      ( 'scale_max',            r'C_\w+$',      None,   ),
      ( 'sync',                 r'o_\w+$',      None,   ),
    )
    names = [a[0] for a in allowables];
    for param_tuple in param_list:
      param = param_tuple[0];
      if param not in names:
        raise SSBCCException('Unrecognized parameter "%s" at %s' % (param,loc,));
      param_test = allowables[names.index(param)];
      self.AddAttr(config,param,param_tuple[1],param_test[1],loc,param_test[2]);
    # Ensure the required parameters are provided.
    for paramname in (
      'outport',
      'freq_hz',
      'min_width',
      'max_width',
    ):
      if not hasattr(self,paramname):
        raise SSBCCException('Required parameter "%s" is missing at %s' % (paramname,loc,));
    # Ensure exactly one of mandatory exclusive pairs are specified.
    for exclusivepair in (
      ( 'outsignal',    'outsignaln',   ),
      ( 'period',       'sync',         ),
    ):
      if not hasattr(self,exclusivepair[0]) and not hasattr(self,exclusivepair[1]):
        raise SSBCCException('One of %s or %s must be specified at %s', (exclusivepair[0], exclusivepair[1], loc, ));
      if hasattr(self,exclusivepair[0]) and hasattr(self,exclusivepair[1]):
        raise SSBCCException('Only one of %s or %s may be specified at %s', (exclusivepair[0], exclusivepair[1], loc, ));
    # Set optional signals
    if not hasattr(self,'default_width'):
      self.default_width = self.min_width;
    # Ensure signal values are reasonable.
    if self.min_width >= self.max_width:
      raise SSBCCException('min_width must be smaller than max_width at %s' % loc);
    if not self.min_width <= self.default_width <= self.max_width:
      raise SSBCCException('default_width is not between min_width and max_width at %s' % loc);
    # Ensure the optionally provided "sync" servo_motor peripheral has been specified.
    if hasattr(self,'sync'):
      for p in config.peripheral:
        if (str(p.__class__) == str(self.__class__)) and (p.outsignal == self.sync):
          break;
      else:
        raise SSBCCException('Can\'t find preceding servo_motor peripheral with outsignal=%s at %s ' % (self.sync,loc,));
      if not hasattr(p,'period'):
        raise SSBCCException('servo_motor peripherial with outsignal=%s must have period specified to be used at %s' % (self.sync,loc,));
    # Translate the outsignal specification into a single member for the signal
    # name and a specification as to whether or not the signal is inverted.
    if hasattr(self,'outsignaln'):
      self.outsignal = self.outsignaln;
      self.invertOutsignal = True;
    else:
      self.invertOutsignal = False;
    # Set the string used to identify signals associated with this peripheral.
    self.namestring = self.outsignal;
    # Calculate the name of the signal to start the PWM.
    self.periodSignal = 's__%s__period_done' % (self.namestring if hasattr(self,'period') else self.sync)
    # Calculate the scaling and set the optionally specified constants.
    # TODO -- ensure the realizable min_width is positive
    self.scaleValue = int(math.ceil((self.max_width-self.min_width)*self.freq_hz/2**config.Get('data_width')));
    self.scale_maxValue = int(math.ceil((self.max_width-self.min_width)*self.freq_hz/self.scaleValue));
    for scalingPair in (
      ( 'scaling',      'scaleValue',           ),
      ( 'scale_max',    'scale_maxValue',       ),
    ):
      if hasattr(self,scalingPair[0]):
        config.AddConstant(scalingPair[1], getAttr(self,scalingPair[1]));
    # Add the I/O port, internal signals, and the INPORT and OUTPORT symbols for this peripheral.
    config.AddIO(self.outsignal,1,'output',loc);
    if hasattr(self,'period'):
      config.AddSignal(self.periodSignal, 1, loc);
    self.ix_outport = config.NOutports();
    config.AddOutport((self.outport,
                       False,
                       # empty list
                      ),loc);
    if hasattr(self,'inperiod'):
      config.AddSignal('s_SETRESET_%s' % self.periodSignal,1,loc);
      config.AddInport((self.inperiod,
                        (self.periodSignal, 1, 'set-reset',),
                       ),loc);
 
  def GenVerilog(self,fp,config):
    body = self.LoadCore(self.peripheralFile,'.v');
    if hasattr(self,'period'):
      body = re.sub(r'@PERIOD_BEGIN@\n','',body);
      body = re.sub(r'@PERIOD_END@\n','',body);
    else:
      body = re.sub(r'@PERIOD_BEGIN@.*?@PERIOD_END@\n','',body,flags=re.DOTALL);
    nbits_scale = CeilLog2(self.scaleValue);
    if nbits_scale == 0:
      body = re.sub(r'@SCALE_0_BEGIN@\n','',body);
      body = re.sub(r'@SCALE_0_ELSE@.*?@SCALE_0_END@\n','',body,flags=re.DOTALL);
    else:
      body = re.sub(r'@SCALE_0_BEGIN@.*?@SCALE_0_ELSE@\n','',body,flags=re.DOTALL);
      body = re.sub(r'@SCALE_0_END@\n','',body);
    scaled_min_width = int(math.floor(self.min_width*self.freq_hz/self.scaleValue));
    scaled_default_width = int(math.floor(self.default_width*self.freq_hz/self.scaleValue));
    scaled_max_width = int(math.floor(self.max_width*self.freq_hz/self.scaleValue));
    nbits_pwm = max(config.Get('data_width')+1,CeilLog2(scaled_max_width));
    pwm_formula = "%d'd%d + { %d'd0, s_N }" % (nbits_pwm,scaled_min_width-1,nbits_pwm-8,);
    if hasattr(self,'period'):
      period = self.period * self.freq_hz / self.scaleValue;
      nbits_period = CeilLog2(period);
    else:
      period = 1;
      nbits_period = 0;
    for subpair in (
      ( r'@DEFAULT_PWM@',       "%d'd%d" % (nbits_pwm,scaled_default_width-1,), ),
      ( r'@INVERT@',            '!' if self.invertOutsignal else '',            ),
      ( r'@IX_OUTPORT@',        "8'd%d" % self.ix_outport,                      ),
      ( r'@NAME@',              self.namestring,                                ),
      ( r'@NBITS_PERIOD@',      str(nbits_period),                              ),
      ( r'@NBITS_PWM@',         str(nbits_pwm),                                 ),
      ( r'@NBITS_SCALE@',       str(nbits_scale),                               ),
      ( r'@ONE_PERIOD@',        "%d'd1" % nbits_period,                         ),
      ( r'@ONE_PWM@',           "%d'd1" % nbits_pwm,                            ),
      ( r'@ONE_SCALE@',         "%d'd1" % nbits_scale,                          ),
      ( r'@OUTSIGNAL@',         self.outsignal,                                 ),
      ( r'@PERIOD_MINUS_ONE@',  "%d'd%d" % (nbits_period,period-1,),            ),
      ( r'@PWM_FORMULA@',       pwm_formula,                                    ),
      ( r'@SCALE_MINUS_ONE@',   "%d'd%d" % (nbits_scale,self.scaleValue-1,),    ),
      ( r'\bgen__',             'gen__%s__' % self.namestring,                  ),
      ( r'\bs__',               's__%s__' % self.namestring,                    ),
      ( r'@PERIOD_SIGNAL@',     self.periodSignal,                              ), # must be after ( r'\bs__', ...
    ):
      body = re.sub(subpair[0],subpair[1],body);
    body = self.GenVerilogFinal(config,body);
    fp.write(body);