srvact.c

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

#endif


#ifdef APM_PICKUP

/******************************************************************************/
/* Function:  focus_on (state machine)                                        */
/*                                                                            */
/*! \brief    provides low level functionalities for focus closing
 *  \param    void
 *  \return   uint8, OK in case of focus closing condition reached
 *            FOCUS_NOT_FOUND in case of focus closing condition
 *            not reached in MAX_FOCUS_ON_RETRIES attempts
 *            IN_PROGRESS in case of focus closing in progress
 *  \remark   assumption: TTM is already started
 */
/******************************************************************************/

RETVAL focus_on(void)
{
  RETVAL focus_on_result;

  focus_on_result = check_servo(CHECK_TTM);
  if (focus_on_result == OK)
  {
    focus_on_result = IN_PROGRESS;

    switch (focus_on_state)
    {
    case STATE_1:
      DEBUG_SERVO(("focus_on", 4, 0));
#ifdef KEEP_FOCUS_DURING_TTM_RECOVER
      if ((servo_recover_flags.field.ttm_recover_active)
       && (!servo_recover_flags.field.focus_missing))
      {
        servo_recover_flags.field.ttm_recover_active = 0;
        start_timer(SERVO_FUNCTION_TIMER, T300ms);
        start_timer(SERVO_FOCUS_FOUND_TIMER, T300ms);
        focus_on_state = STATE_4;
      }
      else
#endif   /* KEEP_FOCUS_DURING_TTM_RECOVER */
      {
        servo_recover_flags.field.ttm_recover_active = 0;
        servo_function_counter = FOCUS_PRO_MAX_RETRIES;
        to_dsp_flags_pc(FCS_OFF);
        dsp_write_xmem(MODE_ADD, ADJ_MODE);
        start_ramp_down();
        start_timer(SERVO_SAFETY_TIMER, FOCUS_PRO_CLOSING_TIMEOUT);
        focus_on_state = STATE_2;
      }
      break;

    case STATE_2:
      if (ramp_stopped())
      {
        to_dsp_flags_pc(FCS_ON);
        start_ramp_up();
        focus_on_state = STATE_3;
      }
      break;

    case STATE_3:
      if (focus_ok())
      {
        stop_ramp();
        servo_recover_flags.field.focus_missing = 0;
        servo_recover_flags.field.focus_recover_mode = FOCUS_PRESENT_IN_CURRENT_POSITION;
        start_timer(SERVO_FUNCTION_TIMER, T300ms);
        start_timer(SERVO_FOCUS_FOUND_TIMER, T300ms);
        focus_on_state = STATE_4;
      }
      else if (ramp_stopped())
      {
        servo_function_counter--;
        if (servo_function_counter != 0)
        {
          if (servo_function_counter & 0x01)   /* servo_function_counter odd */
          {
            start_ramp_down();
          }
          else   /* servo_function_counter even */
          {
            start_ramp_up();
          }
        }
        else   /* (servo_function_counter == 0) */
        {
          to_dsp_flags_pc(FCS_OFF);
          focus_on_result = FOCUS_NOT_FOUND;
        }
      }
      break;

    //case STATE_4:
    default:
      if (check_servo(CHECK_FOCUS) == OK)
      {
        if (timer_in_progress(SERVO_FUNCTION_TIMER))
        {
          if (check_servo(CHECK_HF) == OK)
          {
            servo_startup_flags.field.hf_found = 1;
            servo_recover_flags.field.hf_recover_mode = HF_PRESENT_IN_CURRENT_POSITION;
            stop_timer(SERVO_FUNCTION_TIMER);
            focus_on_result = OK;
          }
          /* else not needed   (check_servo(CHECK_HF) != OK) */
        }
        else   /* (!timer_in_progress(SERVO_FUNCTION_TIMER)) */
        {
          if (!servo_startup_flags.field.hf_found)
          {
            focus_on_result = HF_SEARCH_AFTER_HF_NOT_FOUND;
          }
          else
          {
            focus_on_result = HF_NOT_FOUND;
          }
        }
      }
      else
      {
        focus_on_result = FOCUS_LOST;
      }
      break;
    }
  }
  /* else not needed   ((focus_on_result = check_servo(CHECK_TTM)) == TTM_STOPPED) */

  return focus_on_result;
}

#else   /* (not) APM_PICKUP */

/******************************************************************************/
/* Function:  focus_on (state machine)                                        */
/*                                                                            */
/*! \brief    provides low level functionalities for focus closing
 *  \param    void
 *  \return   uint8, OK in case of focus closing condition reached
 *            FOCUS_NOT_FOUND in case of focus closing condition
 *            not reached in MAX_FOCUS_ON_RETRIES attempts
 *            IN_PROGRESS in case of focus closing in progress
 *  \remark   assumption: TTM can be stopped or already started
 */
/******************************************************************************/

RETVAL focus_on(void)
{
  RETVAL focus_on_result;

  focus_on_result = IN_PROGRESS;

  switch (focus_on_state)
  {
  case STATE_1:
    DEBUG_SERVO(("focus_on", 4, 0));
    servo_function_counter = FOCUS_PRO_MAX_RETRIES;
    to_dsp_flags_pc(FCS_OFF);
    dsp_write_xmem(MODE_ADD, ADJ_MODE);
    start_ramp_down();
    start_timer(SERVO_SAFETY_TIMER, FOCUS_PRO_CLOSING_TIMEOUT);
    focus_on_state = STATE_2;
    break;

  case STATE_2:
    if (ramp_stopped())
    {
      to_dsp_flags_pc(FCS_ON);
      dsp_write_xmem(RAMP_STEP_ADD, RAMP_PRO_RAMP_SPEED_SLOW);
      start_ramp_up();
      focus_on_state = STATE_3;
    }
    break;

  //case STATE_3:
  default:
    if (focus_ok())
    {
      stop_ramp();
      servo_recover_flags.field.focus_missing = 0;
      servo_recover_flags.field.focus_recover_mode = FOCUS_PRESENT_IN_CURRENT_POSITION;
      start_timer(SERVO_FOCUS_FOUND_TIMER, FOCUS_PRO_FOCUS_FOUND_TIMEOUT);
      focus_on_result = OK;
    }
    else if (ramp_stopped())
    {
      servo_function_counter--;
      if (servo_function_counter != 0)
      {
        if (servo_function_counter & 0x01)   /* servo_function_counter odd */
        {
          start_ramp_down();
        }
        else   /* servo_function_counter even */
        {
          start_ramp_up();
        }
      }
      else   /* (servo_function_counter == 0) */
      {
        to_dsp_flags_pc(FCS_OFF);
        focus_on_result = FOCUS_NOT_FOUND;
      }
    }
    break;
  }

  return focus_on_result;
}

#endif   /* APM_PICKUP */


/******************************************************************************/
/* Function:  focus_closing (state machine)                                   */
/*                                                                            */
/*! \brief    provides focus closing
 *  \param    void
 *  \return   uint8, OK in case of focus closing condition reached
 *            FOCUS_NOT_FOUND in case of focus closing condition
 *            not reached in MAX_FOCUS_ON_RETRIES attempts
 *            IN_PROGRESS in case of focus closing in progress
 *  \remark
 */
/******************************************************************************/

RETVAL focus_closing(void)
{
  RETVAL focus_closing_result;

  if ((focus_closing_state != STATE_1) && (!timer_in_progress(SERVO_SAFETY_TIMER)))
  {
    focus_closing_result = FOCUS_CLOSING_TIMEOUT_ERROR;
  }
  else
  {
    focus_closing_result = IN_PROGRESS;

    switch (focus_closing_state)
    {
    case STATE_1:
      start_timer(SERVO_SAFETY_TIMER, FOCUS_PRO_CLOSING_TIMEOUT);
      focus_closing_state = STATE_2;
      break;

    case STATE_2:
      focus_closing_result = focus_on();
      break;
    }
  }

  return focus_closing_result;
}


/******************************************************************************/
/* Function:  focus_off                                                       */
/*                                                                            */
/*! \brief    provides focus opening
 *  \param    void
 *  \return   uint8
 *  \remark
 */
/******************************************************************************/

RETVAL focus_off(void)
{
  RETVAL focus_off_result;
  int16 ramp_position;

  focus_off_result = IN_PROGRESS;

  switch (focus_off_state)
  {
  case STATE_1:
    DEBUG_SERVO(("focus_off", 4, 0));
    event_in[DSP_FOCUS_EVENT].event = FOCUS_EVENT_OPEN;
    check_hf_fsm_call(CHECK_HF_CALLER_FORCE_HF_LOST_CMD);
    stop_ramp();
    focus_off_state = STATE_2;
    break;

  case STATE_2:
    if (ramp_stopped())
    {
      to_dsp_flags_pc(FCS_OFF);
      dsp_write_xmem(MODE_ADD, ADJ_MODE);
#ifndef APM_PICKUP
      dsp_write_xmem(RAMP_STEP_ADD, RAMP_PRO_RAMP_SPEED_FAST);
#endif
      ramp_position = get_ramp_position();
      if (ramp_position == 0)
      {
        focus_off_result = OK;
      }
      else if (ramp_position > 0)
      {
        dsp_write_xmem(RAMP_MIN_ADD, 0x0000);
        start_ramp_down();
      }
      else   /* (ramp_position < 0) */
      {
        dsp_write_xmem(RAMP_MAX_ADD, 0x0000);
        start_ramp_up();
      }
      focus_off_state = STATE_3;
    }
    /* else not needed   (ramp moving (just wait the confirmation of DSP stop ramp execution)) */
    break;

  //case STATE_3:
  default:
    if (ramp_stopped())
    {
      dsp_write_xmem(RAMP_MAX_ADD, (uint16) RAMP_PRO_RAMP_MAX);
      dsp_write_xmem(RAMP_MIN_ADD, (uint16) RAMP_PRO_RAMP_MIN);
      focus_off_result = OK;
    }
    /* else not needed   (ramp moving) */
    break;
  }

  return focus_off_result;
}


/******************************************************************************/
/* Function:  tracking_recover_dithering                                      */
/*                                                                            */
/*! \brief    add dithering to tracking recover delay time
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void tracking_recover_dithering(void)
{
  if (servo_active_flags.field.servo_mode == PLAY_MODE)
  {
    tracking_recover_dither_counter++;
    if (tracking_recover_dither_counter == TRACKING_PRO_RETRY_MAX_DITHER_COUNTER)
    {
      tracking_recover_dither_counter = 0;
    }
  }
  else if (servo_active_flags.field.servo_mode == PAUSE_MODE)
  {
    target_T = (uint32)((int32)target_T + PAUSE_DRIFT_SECTORS_TRK_REC);
    if (target_T < CDT_2SEC)
    {
      target_T = CDT_2SEC;
    }
  }
}