srvopu.c

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

 *  \return   void
 *  \remark
 */
/******************************************************************************/

void abcdef_on(void)
{
  ANA_TST_CTRL_AC.all = 0x0F;
  ANA_TST_CTRL_BD.all = 0x0F;
  ANA_TST_CTRL_E.all = 0x0F;
  ANA_TST_CTRL_F.all = 0x0F;
  DEBUG_SERVO(("abcdef_on", 0, 0));
}


/******************************************************************************/
/* Function:  abcdef_off                                                      */
/*                                                                            */
/*! \brief    switch off photodiode channels
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void abcdef_off(void)
{
  ANA_TST_CTRL_AC.all = 0x00;
  ANA_TST_CTRL_BD.all = 0x00;
  ANA_TST_CTRL_E.all = 0x00;
  ANA_TST_CTRL_F.all = 0x00;
  DEBUG_SERVO(("abcdef_off", 0, 0));
}


/******************************************************************************/
/* Function:  channel_offset_calibration                                      */
/*                                                                            */
/*! \brief    calibrates X-channel offset (ANA_LS_X register)
 *  \param    uint32 *ANA_LS_X      Pointer to X-channel offset register address
 *            uint8  CHANNEL_X      X-channel index in RATE block
 *            uint8  *dark_level    Pointer to X-channel dark_level
 *  \return   RETVAL:  OK in case of calibration success
 *                     CALIBRATION_ERROR in case of failure
 *  \remark
 */
/******************************************************************************/

RETVAL channel_offset_calibration(volatile uint32 *ANA_LS_X,
                                         uint8 CHANNEL_X,
                                         volatile uint8 *x_dark_level)
{
  RETVAL channel_offset_calibration_result;
  uint8 current_level;
  uint8 level_shift;
  uint8 inc;
  uint8 updating_state;

  level_shift = 0x00;
  updating_state = STATE_1;
  channel_offset_calibration_result = RUNNING;
  start_timer(SERVO_FUNCTION_TIMER, OPU_PRO_CALIBRATION_TIMEOUT);
  while (channel_offset_calibration_result == RUNNING)
  {
    if (!timer_in_progress(SERVO_FUNCTION_TIMER))
    {
      /* if not executed in 4ms something is wrong
         return error to prevent watchdog reset */
      channel_offset_calibration_result = CALIBRATION_ERROR;
    }
    else
    {
      *ANA_LS_X = level_shift;
      wait_T50us();
      enable_max_min_measurement();                                        /* select max/min measurements */
      start_max_measurement(CHANNEL_X);                                    /* start x_max measurement     */
      stop_max_measurement(CHANNEL_X);                                     /* stop x_max measurement      */
      current_level = read_measurement();                                  /* read x_max measurement      */
      start_min_measurement(CHANNEL_X);                                    /* start x_min measurement     */
      stop_min_measurement(CHANNEL_X);                                     /* stop x_min measurement      */
      current_level = (uint8)((current_level + read_measurement()) >> 1);  /* read x_min measurement and average with x_max measurement */
      *x_dark_level = current_level;

      switch (updating_state)
      {
      case STATE_1:
        if (current_level == 0)
        {
          channel_offset_calibration_result = CALIBRATION_ERROR;
          break;
        }
        else   // (current_level > 0)
        {
          updating_state = STATE_2;
          /* fall through */
        }

      case STATE_2:
        if (current_level == 0x00)
        {
          level_shift--;
          updating_state = STATE_3;
        }
        else if (current_level == 0x01)
        {
          channel_offset_calibration_result = OK;
        }
        else   /* (current_level > 0x01) */
        {
          if (level_shift == 0xFF)
          {
            channel_offset_calibration_result = CALIBRATION_ERROR;
          }
          else   /* (level_shift < 0xFF) */
          {
            inc = find_level_shift_increment((uint8)(current_level - 1), 0x4E,
                                             *(uint32 *)((uint32)ANA_LS_X + 4));
            if (inc == 0x00)
            {
              inc = 0x01;
            }
            /* else not needed   (inc > 0x00) */

            if (level_shift <= (uint8)(0xFF - inc))
            {
              level_shift += inc;
            }
            else   /* (level_shift > (uint8)(0xFF - inc)) */
            {
              level_shift = 0xFF;
            }
          }
        }
        break;

      //case STATE_3:
      default:
        if (current_level == 0x00)
        {
          level_shift--;
        }
        else   /* (current_level > 0x00) */
        {
          channel_offset_calibration_result = OK;
        }
        break;
      }
    }
  }

  stop_timer(SERVO_FUNCTION_TIMER);
  return channel_offset_calibration_result;
}


/******************************************************************************/
/* Function:  abcdef_offset_calibration                                       */
/*                                                                            */
/*! \brief    calibrates AC, BD, E, and F-channel offset
 *  \param    void
 *  \return   OK in case of calibration success for all channels
 *            CALIBRATION_ERROR otherwise
 *  \remark
 */
/******************************************************************************/

RETVAL abcdef_offset_calibration(void)
{
  if ((channel_offset_calibration(&ANA_LS_AC, CHANNEL_AC, &ac_dark_level) == OK)
   && (channel_offset_calibration(&ANA_LS_BD, CHANNEL_BD, &bd_dark_level) == OK)
   && (channel_offset_calibration(&ANA_LS_E, CHANNEL_E, &e_dark_level) == OK)
   && (channel_offset_calibration(&ANA_LS_F, CHANNEL_F, &f_dark_level) == OK))
  {
    return OK;
  }
  else
  {
    return CALIBRATION_ERROR;
  }
}


/******************************************************************************/
/* Function:  find_channel_amplitude                                          */
/*                                                                            */
/*! \brief    finds X-channel amplitude
 *  \param    uint8 *ANA_GAIN_X   pointer to X-channel maximum (in input) / amplitude (in output)
 *            uint8 x_dark_level  X-channel dark level
 *  \return   void
 *  \remark
 */
/******************************************************************************/

uint8 find_channel_amplitude(uint8 x_acc)
{
  uint8 x_amp;

  if (servo_function_counter_2 == 2)
  {
    x_amp = (uint8)((x_acc + 1) >> 1);
  }
  else
  {
    x_amp = (uint8)((x_acc + 2) >> 2);
  }

  return x_amp;
}


/******************************************************************************/
/* Function:  find_abcdef_amplitude                                           */
/*                                                                            */
/*! \brief    finds AC, BD, E and F-channel amplitude and (AC+BD)/2 mean
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void find_abcdef_amplitude(void)
{
  ac_amp = find_channel_amplitude(ac_acc);
  bd_amp = find_channel_amplitude(bd_acc);
  e_amp = find_channel_amplitude(e_acc);
  f_amp = find_channel_amplitude(f_acc);

  if (servo_function_counter_2 == 2)
  {
    abcd_amp = (uint8)((ac_acc + bd_acc + 2) >> 2);
  }
  else
  {
    abcd_amp = (uint8)((ac_acc + bd_acc + 4) >> 3);
  }

  DEBUG_SERVO(("abcd_amp ", 1, 1, abcd_amp));
}


/******************************************************************************/
/* Function:  channel_gain_calibration                                        */
/*                                                                            */
/*! \brief    calibrates X-channel gain
 *  \param    uint8 *ANA_GAIN_X   pointer to X-channel offset register address
 *            uint8 x_max         X-channel maximum
 *            uint8 x_dark_level  X-channel dark level
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void channel_gain_calibration(volatile uint32 * ANA_GAIN_X, uint8 x_amp)
{
  sint8 new_gain;

  new_gain = (sint8)(*ANA_GAIN_X + amplitude_2_gain(x_amp)
                     - amplitude_2_gain(FE_ADJUST_PRO_ABCDEF_TARGET_AMPLITUDE));

  if (servo_startup_flags.field.ttm_started)
  {
    new_gain += 0x02;
  }

  if (new_gain < 0x00)
  {
    new_gain = 0x00;
  }
  else if (new_gain > 0x3F)
  {
    new_gain = 0x3F;
  }

  *ANA_GAIN_X = (uint8)new_gain;
}


/******************************************************************************/
/* Function:  abcdef_gain_calibration                                         */
/*                                                                            */
/*! \brief    calibrates AC, BD, E and F-channel gain
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void abcdef_gain_calibration(void)
{
  channel_gain_calibration(&ANA_GAIN_AC, ac_amp);
  DEBUG_SERVO(("ch_gain_calib_AC ", 1, 2, ANA_GAIN_AC, ac_amp));
  channel_gain_calibration(&ANA_GAIN_BD, bd_amp);
  DEBUG_SERVO(("ch_gain_calib_BD ", 1, 2, ANA_GAIN_BD, bd_amp));
  channel_gain_calibration(&ANA_GAIN_E, e_amp);
  DEBUG_SERVO(("ch_gain_calib_E  ", 1, 2, ANA_GAIN_E, e_amp));
  channel_gain_calibration(&ANA_GAIN_F, f_amp);
  DEBUG_SERVO(("ch_gain_calib_F  ", 1, 2, ANA_GAIN_F, f_amp));
}


/******************************************************************************/
/* Function:  fe_adjust (state machine)                                       */
/*                                                                            */
/*! \brief    provides focus error amplitude normalization
 *  \param    void
 *  \return   OK in case of focus error adjust success
 *            IN_PROGRESS in case of focus error adjust in progress
 *  \remark
 */
/******************************************************************************/

RETVAL fe_adjust(void)
{
  RETVAL fe_adjust_result;

  if ((fe_adjust_state != STATE_1) && (!timer_in_progress(SERVO_SAFETY_TIMER)))
  {
    fe_adjust_result = FE_ADJUST_TIMEOUT_ERROR;
  }
  else if (servo_startup_flags.field.fe_adjust_done)
  {
    fe_adjust_result = OK;