srvact.c

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

/**************************************************
 *
 * srvact.c
 *
 * CVS ID:   $Id: srvact.c,v 1.24 2007/09/26 15:12:19 dellorto Exp $
 * Author:   Fabio Dell'Orto [FD] - STM
 * Date:     $Date: 2007/09/26 15:12:19 $
 * Revision: $Revision: 1.24 $
 *
 * Description:
 *
 *
 ***************************************************
 *
 * COPYRIGHT (C) ST Microelectronics  2005
 *            All Rights Reserved
 *
 *******************************************************************************
 *  \brief          Servo initialisation & actuator related procedures
 *
 *  \par            Change History:
 *
 * - BB040315       Added output interface initialisation according to module
 *                  configuration settings
 *
 * - BB040503       Because of EMC we use lowest PDM frequency
 *
 * - PH040601a      Modified profile handling. Introduced profile union
 *                  and pointer to it. So it can be switched ROM/RAM
 *
 * - PH040601b      Added value in START_TTM_PROFILE --> idle value for SPINDLE_KICK
 *                  Exchanged value F000
 *
 * - PH040601-      Renamed procedure servo_init_cmd() to dsp_init()
 *
 * - ST-------      Split up dsp_init() into 7 substates to save main_loop time
 *
 * - PH040601-      Changes:
                    - Modified DRIVER ON sequence
 *                  - Switch driver off
 *                  - Enable CTL1 separately
 *                  - Enable CTL2 -->Short circuit braking mode
 *                  - SPIN is below slightly negative
 *
 * - MR040603-      Introduced improved AGC algorithm
 *                  Corrected agc_dis setting in track_on()
 *
 * - PH040608a      Changed SER_MCR1.sled_clk to highes possible frequency
 *
 * - ST040628       Changes:
 *                  - Completed focus_recover() and tracking_recover()
 *                  - Hardware AGC algorithm not enabled in track_on()
 *                    and already disabled in track_off()
 *                  - Modified improved AGC algorithm in agc()
 *
 * - PH040728a      Changes:
 *                  - Changed Order FOCUS_ON->START_TTM to START_TTM->FOCUS_ON
 *                  - HF check moved from start_ttm() over focus_on() to te_adjust()
 *                  - Declared local functions "static"
 *                  - Introduced uint8, int8, int16, uint16, int32, uint32
 *                  - Usage of doxygen headers
 *
 * - PH040916       dsp_init(): Initial half gain for stepper motor output.
 *                  track_off(): Restore low gain for stepping motor
 *
 * - BB040928a      Code rom and ram size optimisation:
 *                  - Optimised check_servo() and evaluation of check_servo() result
 *
 * - BB041012b      Force execution of all calibrations (fe adjust, hf found, te adjust,
 *                  agc focus, agc tracking)  if one at new startup if one failed
 *
 * - BB041013b      Fixed agc() to set default gain values in case of error to
 *                  be able to execute recovery
 *
 * - MR20050106     Problem: Turntable motor often came to a halt or rotated
 *                  backwards after a jump from track 1 to the last track or vice
 *                  versa. The fixes are described in the following points: a, b
 *                  and c.
 * - MR20050106a    Changed the thresholds that determine whether or not the
 *                  correct TTM velocity has been reached in CLV mode. Especially
 *                  the value for the phase was too high.
 * - MR20050106b    Introduced a timer which makes sure that the TTM velocity is
 *                  within the defined range for at certain period of time before
 *                  the DSP notifies the ST7 that the correct speed has been
 *                  reached.
 * - MR20050106c    When the TTM controller switches to hold mode (either by the
 *                  ST7, during a short jump or a defect) it will not be allowed
 *                  to brake the TTM. If the TTM controller output is negative
 *                  during the hold mode then its output is set to zero.
 * - BB050131a      ROM code optimizations:
 *                  - Optimized AGC parameter initilaization
 *                    (drawback bigger profile size)
 * - PH050210a      Modified Setting of CLV_PHASE_ERR_MAX from 0x1000->0x4000
 * - BB050221d      Protected read and write to 16bit DSP subsystem registers
 * - PH050504a      Initialisation of tracking fsm in track_on() made more robust
 * - MR050523b      DSP detects high frequency FG pulses and reports this to the
 *                  ST7. High frequency FG pulses are an indication for an to slow
 *                  spinning or stopped TTM.
 * - MR050523c      Changed limit on TTM controller output from one +/- level to
 *                  a maximum and a minimum level.
 * - HW050701a      set linear TE to come on track and change it back to classic TE
 *                  after on track or timeout.
 * - MR051115       Introduced an extra gain called BW_SWITCH in both radial an focus controller
 *                  in order to increase the BW  of their respective control loop.
 * - HW060116a      new function jump_ok to check if after master jump(short jump) has finished and
 *                  if not to force finish master jump.
 *                  -Funtion is called in focus_recovery and in HF_recovery
 * - FD060929a      Fixed detection of very short focus ramping
 *
 ***************************************************
 *
 * STM CVS Log:
 *
 * $Log: srvact.c,v $
 * Revision 1.24  2007/09/26 15:12:19  dellorto
 * added dithering in delay between tracking recover retries
 *
 * Revision 1.23  2007/08/14 15:30:14  dellorto
 * multiple mechanisms
 *
 * Revision 1.22  2007/04/11 14:21:04  belardi
 * Integration of HAVE_CD_MECHA modification by [GP]
 *
 * Revision 1.21  2007/03/16 14:17:34  dellorto
 * used new profiles
 *
 * Revision 1.20  2006/11/21 13:58:43  dellorto
 * improved hostif reporting of servo parameters in case of restart
 *
 * Revision 1.19  2006/11/10 16:30:20  dellorto
 * added setting of servo parameters for hostif
 *
 * Revision 1.18  2006/10/31 11:09:59  marcucci
 * No more static functions for Jump Table
 *
 * Revision 1.17  2006/10/09 15:08:55  dellorto
 * modified focus_on() in case of DC TTM motor
 *
 * Revision 1.16  2006/09/18 09:55:25  belardi
 * Corrected CVS keyword usage
 *
 * Revision 1.15  2006/09/18 09:25:37  belardi
 * Added Log CVS keyword into file header
 *
 *
 ******************************************************************************/

#include "gendef.h"
#include "hwreg.h"
#include "osal.h"
#include "srvinc.h"
#include "pdebug.h"

#ifndef APM_PICKUP
 #include "hostif_high.h"
#endif


/******************************************************************************/
/* Function:  dsp_init                                                        */
/*                                                                            */
/*! \brief    Load coefficents and profiles into XRAM and YRAM
 *  \param    void
 *  \return   uint8, OK or IN_PROGRESS
 *  \remark
 */
/******************************************************************************/

#if (HAVE_CD_MECHA == 1)
void dsp_init(void)
{
  dsp_write_coef(COEFF_FOCUS_ADD, FOCUS_PRO_CONTROLLER_COEFF);
  dsp_write_coef(COEFF_TRACKING_ADD, TRACKING_PRO_CONTROLLER_COEFF);

  dsp_write_xmem(MODE_ADD, ADJ_MODE);
  dsp_write_ymem(COEFF_ERROR_ADD + 2,  0x0000);   /* FE offset */
  dsp_write_ymem(COEFF_ERROR_ADD + 4,  0x8000);   /* -1, used in generation of always positive HFL */
  dsp_write_ymem(COEFF_ERROR_ADD + 5,  0x5560);
  dsp_write_ymem(COEFF_ERROR_ADD + 6,  0x2ab0);
  dsp_write_ymem(COEFF_ERROR_ADD + 7,  0x0afe);
  dsp_write_ymem(COEFF_ERROR_ADD + 8,  0x7500);
  dsp_write_ymem(COEFF_ERROR_ADD + 9,  0x7000);
  dsp_write_ymem(COEFF_ERROR_ADD + 10, 0x7000);

  dsp_write_ymem(COEFF_MISC_ADD + 10, 0x0040);
  dsp_write_ymem(COEFF_MISC_ADD + 11, 0x7FC0);

  dsp_write_ymem(COEFF_MISC_ADD + 8, 0x7FFF);
  dsp_write_ymem(COEFF_MISC_ADD + 9, 0x0000);

  dsp_write_ymem(FOCUS_GAIN_ADD, (uint16)AGC_PRO_GAIN_FOCUS);
  dsp_write_ymem(TRACKING_GAIN_ADD, (uint16)AGC_PRO_GAIN_TRACKING);

  dsp_write_xmem(RAMP_MAX_ADD, (uint16)RAMP_PRO_RAMP_MAX);
  dsp_write_xmem(RAMP_MIN_ADD, (uint16)RAMP_PRO_RAMP_MIN);
  dsp_write_xmem(RAMP_STEP_ADD, RAMP_PRO_RAMP_SPEED_FAST);
  dsp_write_xmem(FOK_TUP_ADD, OPU_PRO_FOCUS_CLOSED_DETECTION_TIME);
  dsp_write_xmem(FOK_TDN_ADD, OPU_PRO_FOCUS_OPEN_DETECTION_TIME);
  dsp_write_xmem(DEF_TDN_ADD, OPU_PRO_ACQ_DEFECT_ABSENT_DETECTION_TIME);
  dsp_write_xmem(DEF_TUP_ADD, OPU_PRO_ACQ_DEFECT_PRESENT_DETECTION_TIME);
  dsp_write_xmem(FCS_OFFSET_ADD, (uint16)FOCUS_PRO_OFFSET);
  dsp_write_xmem(FOK_THR_ADD, (uint16)FOCUS_PRO_FOCUS_OK_LEVEL);
  dsp_write_xmem(FE_CMP_ADD, (uint16)FOCUS_PRO_CLOSING_THS);
  dsp_write_xmem(HYST_ADD, (uint16)FOCUS_PRO_CLOSING_HYST);
  dsp_write_xmem(TRK_OFFSET_ADD, (uint16)TRACKING_PRO_OFFSET);
  dsp_write_xmem(TE_THR_ADD, TRACKING_PRO_TRACKING_OK_THS);
  to_dsp_flags_pc(FCS_OFF);
  dsp_write_xmem(TTM_SATURATION_ADD, 0x7FFF);
  dsp_write_xmem(SPINDLE_KICK_ADD, (uint16)TTM_PRO_SPINDLE_KICK_IDLE);
  dsp_write_xmem(DISC_MODE_ADD, KICK_MODE);

  /* MR041215 - BEGIN */
  /* Set thresholds used by DSP to determine whether or not */
  /* the correct TTM speed has been reached. */
#ifdef APM_PICKUP
  dsp_write_xmem(SPDL_LIMIT_MIN_ADD, 0x8002);         // MR050523c. Set minimum driving current, such that motor driver does not lose lock during braking.
  dsp_write_xmem(SPDL_LIMIT_MAX_ADD, 0x7ffe);         // MR050523c. Set maximum driving current, should at least be 1 below 0x7fff for the anti-windup functionality in the TTM controller in the DSP.
  dsp_write_xmem(TTM_MIN_PERIOD_ADD, 0x03A9);         // MR050523b. Measured TTM frequencies over 25 Hz are defined to be "high frequent" caused by a too slow spinning or stopped TTM.
  dsp_write_xmem(TTM_MAX_PERIOD_ADD, 0x6000);
  dsp_write_xmem(CAV_PERIOD_ERR_MAX_ADD, 0x500);
#else
  dsp_write_xmem(SPDL_LIMIT_MIN_ADD, (uint16)TTM_PRO_SPINDLE_SATURATION_MIN_12CM);
  dsp_write_xmem(SPDL_LIMIT_MAX_ADD, (uint16)TTM_PRO_SPINDLE_SATURATION_MAX_12CM);
#endif
  dsp_write_xmem(CLV_FREQ_ERR_MAX_ADD, TTM_PRO_CLV_FREQ_ERR_MAX);   /* PH050210a */
  dsp_write_xmem(CLV_PHASE_ERR_MAX_ADD, TTM_PRO_CLV_PHASE_ERR_MAX);  /* PH050210a */
  /* MR041215 - END */

  dsp_write_xmem(BW_SWITCH_ADD, 0x4000);
  dsp_write_xmem(CLV_BUFFER_OK_THRES_ADD, 0x0000);

  SER_STEP_CLK.all = 0x3C;
  SER_STEP_MODE.all = 0x00;
#ifdef APM_PICKUP
  SER_STEP_CTRL.all = 0x18; //PH040916
  dsp_write_xmem(FWD_STEP_ADD, 0x000D);
  dsp_write_xmem(BWD_STEP_ADD, 0x000E);
  /* Reduce gain, setup timeout variables */
  dsp_write_xmem(SLEDGAIN_VAL_ADD, 0x18);
  dsp_write_xmem(SLEDGAIN_TIME_ADD, 5);
#else
  SER_STEP_CTRL.all = 0x00;
#endif
  /* BB040503 - BEGIN */
  SER_CLK_ACT = 0x03;
  SER_MCR1.field.disc_clk = 0x03;
  SER_MCR1.field.sled_clk = 0x00; //PH040608a
  /* BB040503 - END */

  /* Turn DSP processing on. */
  dsp_write_xmem(PROC_ENABLE_ADD, 1);
}


/******************************************************************************/
/* Function:  get_ramp_position                                               */
/*                                                                            */
/*! \brief    gets the current focus ramp position
 *  \param    void
 *  \return   int16 focus_ramp_position
 *  \remark
 */
/******************************************************************************/

sint16 get_ramp_position(void)
{
  return (sint16)dsp_read_xmem(FCS_OFFSET_ADD);
}


/******************************************************************************/
/* Function:  start_ramp_up                                                   */
/*                                                                            */
/*! \brief    starts focus ramp generation, single ramp, direction up
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void start_ramp_up(void)
{
  dsp_write_xmem(RAMP_MODE_ADD, SINGLE_RAMP | RAMP_UP | RAMP_ON);
  event_in[DSP_RAMP_EVENT].event = RAMP_EVENT_MOVING;
  DEBUG_SERVO(("ramp_up", 4, 0));
}


/******************************************************************************/
/* Function:  start_ramp_down                                                 */
/*                                                                            */
/*! \brief    starts focus ramp generation, single ramp, direction up
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void start_ramp_down(void)
{
  dsp_write_xmem(RAMP_MODE_ADD, SINGLE_RAMP & RAMP_DOWN | RAMP_ON);
  event_in[DSP_RAMP_EVENT].event = RAMP_EVENT_MOVING;
  DEBUG_SERVO(("ramp_down", 4, 0));
}


/******************************************************************************/
/* Function:  stop_ramp                                                       */
/*                                                                            */
/*! \brief    stops focus ramp generation
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void stop_ramp(void)
{
  dsp_write_xmem(RAMP_MODE_ADD, dsp_read_xmem(RAMP_MODE_ADD) & RAMP_OFF);
}


/******************************************************************************/
/* Function:  ramp_stopped                                                    */
/*                                                                            */
/*! \brief    checks focus ramp working state (in progress or stopped)
 *  \param    void
 *  \return   uint8; focus ramp working state (RAMP_IN_PROGRESS/RAMP_STOPPED)
 *  \remark   in single ramp mode, ramp generation stops automatically when
 *            ramp maximum or ramp minimum are reached; in this case this
 *            routine returns RAMP_STOPPED
 */
/******************************************************************************/

t_bool ramp_stopped(void)
{
  return (t_bool)(RAMP_EVENT_STOPPED == (t_ramp_event) event_in[DSP_RAMP_EVENT].event);
}


#if defined(APM_PICKUP) || (DEBUG_INCLUDE_PCDEB == 1)   /* [RB] commented out to reduce ROM space */
/******************************************************************************/
/* Function:  focus_ramping (for debug purpose only)                          */
/*                                                                            */
/*! \brief    generates a continuous focus ramping
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void focus_ramping(void)
{
  to_dsp_flags_pc(FCS_OFF);
  dsp_write_xmem(MODE_ADD, ADJ_MODE);
  dsp_write_xmem(RAMP_MODE_ADD, dsp_read_xmem(RAMP_MODE_ADD) & CONTINUOUS_RAMP | RAMP_ON);
  event_in[DSP_RAMP_EVENT].event = RAMP_EVENT_MOVING;
}