int_proc.h

本程序为ST公司开发的源代码

    accu1 = (saturate(accu1 + delay[2])) * c[8];     /* Saturate before multiplying (multiplier only 17x16 bits). */
    pacc = accu1 * c[10];
    pacc *= bw_switch;
    dy0 = c[9];
    accu0 = pacc << (int)dy0;      /* Results of combined I and PD parts in accu0. */

    /*--------------- Offset and AGC injection ---------------*/
    accu1 = accu0 + fcs_off; /* Add offset. */
    accu0 = accu1 + fosc;    /* Add oscillator signal for AGC. */
    fa  = accu0;             /* Focus actuator signal after AGC injection point. */
    fa2 = accu1;             /* Focus actuator signal before AGC injection point. */

#if OPTIMIZED_NOISE_SHAPING  /* MR20060201b */
    /* Apply noise shaping when converting the 16 bits result to the 10 bits output. */
    accu0 = saturate(accu0 + delay[6]);   /* Accumulate 6 bits lost in the previous cycle. */
    FACT = accu0;                         /* Drive the focus actuator. */
    delay[6] = accu0 - FACT;              /* Store 6 lsb which are not output via FACT. */
#else
    /* Apply noise shaping when converting the 16 bits result to the 10 bits output. */
    accu0 = saturate(accu0 + delay[6]);   /* Accumulate 6 bits lost in the previous cycle. */
    dx0 = accu0 >> 6;           /* Remove 6 lsb by shifting 6 to the right, storing the */
    accu1 = dx0 << 6;           /* intermediate result in dx0 before shifting to the left again. */
    delay[6] = accu0 - accu1;   /* Store 6 lsb which are not output via FACT. */

    /* Drive the focus actuator. */
    FACT = accu0;
#endif
  }
  else   /* Focus loop open. */
  {
    dsp_state = get_clr_focus_closed(dsp_state);
    /* Clear PID when focus loop is opened. */
    /* Register accu0 conains 0 upon entering this part of the code. */
    for (int_accu1 = 0; int_accu1 < MAX_DIM_DELAY_FOCUS; int_accu1++) {   /* Pitty loop() does not work inside interrupt routine. */
      *delay++ = accu0;
    }

    /* Output focus offset only, used during ramping. */
    accu1 = fcs_off;
    fa = accu1;
    FACT = accu1;
  }

}
#endm

MACRO init_track()
{
#if (1 == OPTIMIZED_INIT)
  /* Nothing to be done, everything is already reset to zero. */
#else
  fraction * delay;
  integer   i;

  trk_off = 0;
  tosc = 0;
  delay = delay_tracking;
  for (i = 0; i < MAX_DIM_DELAY_TRACKING; i++)
  {
    delay[i] = 0;
  }
#endif
}
#endm

/************************************************************************/

MACRO track(void)
{
  fraction * delay;
  memoryY CONST * c;
  register fraction pacc at(p);
  register fraction accu0 at(acc[0]);
  register fraction accu1 at(acc[1]);
  register fraction dx0 at(dx[0]);
  register fraction dy0 at(dy[0]);
  register fraction dy1 at(dy[1]);
  register int int_accu1 at(acc[1]);

  delay = delay_tracking;
  c = coeff_tracking;


  /**************************************************************************/
  /* Nonlinear PID controller for the radial loop, including sledge control */
  /**************************************************************************/
  accu0 = TCR_ON_INT;
  if (accu0 != 0)    /* Radial loop closed. */
  {

    /**************************************************************************/
    /* Nonlinear control  (START)                                             */
    /* ---------------------------------------------------------------------- */
    /* Input:  te    - radial tracking error signal                           */
    /* Output: accu0 - input to linear PID controler                          */
    /* Param:  nlc_delta - Deadzone threshold                                 */
    /*         nlc_alpha - Deadzone slope                                     */
    /*         coeff_tracking[11..15] - Notch coeff                           */
    /* States: delay_tracking[ 8..11] - Notch states                          */
    /**************************************************************************/

    dx0 = te;   /* Input of the controller (radial tracking error) in dx0. */
    if (!(gainadj & 0x01))  /* Nonlinear control only when AGC is turned off, for two reasons: */
                            /* (1) Radial bandwidth set incorrectly when nonlinear control is on. */
                            /* (2) AGC and nonlinear on together costs too much DSP processing time. */
    {
      /**************************************************************************/
      /* Deadzone.                                                              */
      /* ---------------------------------------------------------------------- */
      /* Input:  dx0       - radial tracking error                              */
      /* Output: accu0     - input to notch                                     */
      /* Param:  nlc_delta - Deadzone threshold                                 */
      /*         nlc_alpha - Deadzone slope                                     */
      /**************************************************************************/
#if (1 == OPTIMIZED_DEADZONE)
      /* MR20060201c */
      /* Deadzone: y = f(x, nlc_delta, nlc_alpha) */
      /* The actual deadzone equals 0x7fff-nlc_delta. */
      dy0 = nlc_delta;
      accu0 = saturate(dx0 + dy0) - dy0;
      accu0 = saturate(accu0 - dy0) + dy0;
      accu0 = (dx0 - accu0) * nlc_alpha;
#else
     /* Deadzone with threshold nlc_delta. */
      /* The actual deadzone equals nlc_delta. */
      dy0 = nlc_delta;
      if (dx0 > dy0)
      {
        accu0 = dx0 - dy0;
      }
      else
      {
        accu1 = -dy0;
        if (dx0 < accu1)
        {
          accu0 = dx0 + dy0;
        }
        else
        {
          accu0 = 0;
        }
      }
      /* Gain of the nonlinear control, i.e. slope of deadzone. */
      dx0 = nlc_alpha;
      accu0 *= dx0;
#endif

      /**************************************************************************/
      /* Notch.                                        */
      /* ---------------------------------------------------------------------- */
      /* Input:  accu0                                 */
      /* Output: accu1                                 */
      /* Param:  coeff_tracking[11..15] - Notch coeff                           */
      /* States: delay_tracking[ 8..11] - Notch states                          */
      /**************************************************************************/
      accu0 *= c[11];
      pacc = c[12] * delay[8];
      accu1 = accu0 + pacc;
      accu1 += pacc;
      accu1 += c[13] * delay[9];
      accu1 += c[14] * delay[10];
      pacc = c[15] * delay[11];
      accu1 += pacc;
      accu1 += pacc;
      dx0 = delay[8];
      delay[8] = accu0;
      accu0 = dx0;
      delay[9] = accu0;
      dx0 = delay[11];
      delay[11] = accu1;
      accu0 = dx0;
      delay[10] = accu0;

      /**************************************************************************/
      /* Combine linear and nonlinear paths.           */
      /* ---------------------------------------------------------------------- */
      /* Input:  te    - radial tracking error                                  */
      /*         accu1 - notch output                                           */
      /* Output: accu0                                 */
      /**************************************************************************/
      accu0 = te;
      accu0 = accu0 >> NLC_LOG2_MAXGAIN;
        accu0 += accu1;

    }
    else    /* Only linear control when AGC is performed. */
    {
      accu0 = dx0 >> NLC_LOG2_MAXGAIN;
    }

    /**************************************************************************/
    /* Nonlinear control (END)                                                */
    /**************************************************************************/


    /**************************************************************************/
    /* Slope filter.                                 */
    /* --------------------------------------------- */
    /* Generates the interpolated te-sginal value    */
    /* during a defect, when not in short jump mode. */
    /* --------------------------------------------- */
    /* Input:  accu0 - output for nonlinear part     */
    /*         trk_defect - active during defect     */
    /*         jump_mode - active during short jump  */
    /* Output: accu0                                 */
    /* Param:  coeff_tracking[0] - slope             */
    /* States: delay_tracking[2] - interpolated val. */
    /*************************************************/
    dx0 = delay[2];
    if (trk_defect & (~jump_mode))  /* Interpolate te during a defect. */
    {
      accu0 = dx0;
    }
    else                            /* Update slope filter when no defect is present. */
    {
      dy0 = c[0];
      if (accu0 > dx0)
      {
        accu1 = dx0 + dy0;
      }
      else
      {
        accu1 = dx0 - dy0;
      }
      delay[2] = accu1;
    }

    /**************************************************************************/
    /* Linear PID controller for radial tracking.                             */
    /* ---------------------------------------------------------------------- */
    /* Input:  accu0      - Interpolated value after nlc part.                */
    /* Output: ta/ta2     - Actuator signal after/before AGC injection point  */
    /*                      but after offset addition.                        */
    /*         TACT       - Signal to radial actuator.                        */
    /* Param:  coeff_tracking[0..10]                                          */
    /* States: delay_tracking[0,1,3..7]                                       */
    /**************************************************************************/

    te_pid_in = accu0; /* Store to be used as input of the PD part of PID. */

    /* I part of PID (with decimation). */
    /* accu0 is input of filter (te_pid_in). */
    accu0 = accu0 >> 4;                 /* Decimation using an accumulate-and-drop filter. */
    accu0 += *((ext_fraction *)delay);  /* d0:d1 - double precision delay for decmation accumulation part. */
    *((ext_fraction *)delay) = accu0;
    if (count16 == 13)
    {
      accu0 = accu0 * c[1];
      accu0 += c[2] * delay[3];
      if (!trk_defect)                  /* Hold integrator during a defect. */
      {
        delay[3] = accu0;
      }
      *((ext_fraction *)delay) = 0;     /* The "drop" part of the accumulate-and-drop filter. */
    }

    /* PD part of PID. */
    accu1 = te_pid_in * c[3];
    accu0 = accu1 + c[4] * delay[4] + c[5] * delay[5];
    delay[4] = accu1;
    delay[5] = accu0;
    accu0 = accu0 + c[6] * delay[6];
    dy0 = c[7];
    delay[6] = accu0;
    accu1 = accu0 << (int)dy0;  /* Result of the PD part in accu1. */

    /* Combine I and PD parts, result is accu0. */
    accu1 = saturate(accu1 + delay[3]) * c[8];  /* Saturate before multiplying (multiplier only 17x16 bits): */
    pacc = accu1 * c[10];
    pacc *= bw_switch;
    dy0 = c[9];
    accu0 = pacc << (int)dy0;   /* Result combined I and PD part in accu0 and ta_pid_out. */

    /* Decimation of PID controller output for sledge control using */
    /* an accumulate-and-drop filter. */
    tajump += (accu0 >> 4);         /* Accumulation part of sledge decimation filter. */

    /* Offset and AGC injection. */
    accu0 = accu0 + trk_off;        /* Add offset. */
    accu1 = accu0 + tosc;           /* Add oscillator signal for AGC. */
    ta = accu1;                     /* Radial actuator signal after injection point. */
    ta2 = accu0;                    /* Radial actuator signal before injection point. */

#if OPTIMIZED_NOISE_SHAPING   /* MR20060201b */
    /* Apply noise shaping when converting the 16 bits result to the 10 bits output. */
    accu1 = saturate(accu1 + delay[7]);   /* Accumulate 6 bits lost in the previous cycle. */
    TACT = accu1;                         /* Drive the tracking actuator. */
    delay[7] = accu1 - TACT;              /* Store 6 lsb which are not output via TACT. */
#else
    /* Apply noise shaping when converting the 16 bits actuator signal into a 10 bits output value. */
    accu0 = saturate(accu1 + delay[7]);       /* Accumulate the 6 bits lost in the previous cycle. */
    dx0 = accu0 >> 6;               /* Remove 6 lsb by shifting 6 to the right, storing the */
    accu1 = dx0 << 6;               /* intermediate result in dx0 before shifting to the left again. */
    delay[7] = accu0 - accu1;       /* Store the 6 lsb, which are not output via TACT, for the next cycle. */

    /* Drive the tracking actuator. */
    TACT = accu0;
#endif


    /**************************************************************************/
    /* Sledge control.                                                        */
    /**************************************************************************/
    if (sledpulse_cnt) 
    {	
      sledpulse_cnt--;
    }
    else
    {
      DSP_SSPD = 0;
    }
    if (count16 == 2)
    {
      /* Filtering the PID output for slegde control. */
      c = coeff_sledge;
      accu0 = tajump * c[0];
      accu1 = accu0 + c[1] * delay[12] + c[2] * delay[13];
      delay[12] = accu0;
      delay[13] = accu1;
      ta_filt = accu1;                      /* Result of filtering in ta_filt. */

      tajump = 0;                           /* The "drop" part of sledge decimation filter. */

      sledcnt++;                            /* Advance sledge-step timer. */
      if (sledcnt >= sledtime)              /* Sledge timer expired, check whether a step must be made or gain should be reduced. */
      {
        if (~trk_defect | jump_mode)        /* No stepping during a defect. */
        {
          if (ta_filt > tawin)              /* Forward step if DC value of PID output larger than threshold tawin. */
          {
            if (!(jump_mode & jmp_dir))     /* Block stepping in other direction than jump direction during jumping. */
            {
              /* Move Sledge Forward */
              DSP_SSPD = sledpulse_fwd_amp;
              sledpulse_cnt = sledpulse_fwd_time;
              dsp_state = get_equ_sledge_moved(dsp_state, SLEDGE_MOVED_FORWARD);
            }
          }
          else if (ta_filt < -tawin)        /* Backward step if DC value of PID output smaller than threshold -tawin. */
          {
            if (!(jump_mode & (~jmp_dir)))  /* Block stepping in other direction than jump direction during jumping. */
            {
              /* Move Sledge Backward */
              DSP_SSPD = sledpulse_bwd_amp;
              sledpulse_cnt = sledpulse_bwd_time;
              dsp_state = get_equ_sledge_moved(dsp_state, SLEDGE_MOVED_BACKWARD);
            }
          }
        }
        sledcnt = 0;                        /* Restart sledge-step timer. */
      }
    }
  }
  else
  {
    /* Clear PID when radial loop is opened. */
    /* Register accu0 conains 0 upon entering this part of the code. */
    for (int_accu1 = 0; int_accu1 < MAX_DIM_DELAY_TRACKING; int_accu1++) {   /* Pitty loop() does not work inside interrupt routine. */
      *delay++ = accu0;
    }

    /* Output tracking offset only. */
    accu1 = trk_off + tosc;
    ta = accu1;

    /* Drive the tracking actuator. */
    TACT = accu1;
  }