srvsld.c

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

/**************************************************
 *
 * srvsld.c
 *
 * CVS ID:   $Id: srvsld.c,v 1.15 2007/09/26 15:10:45 dellorto Exp $
 * Author:   Fabio Dell'Orto [FD] - STM
 * Date:     $Date: 2007/09/26 15:10:45 $
 * Revision: $Revision: 1.15 $
 * 
 * Description:
 * 
 *
 ***************************************************
 * 
 * COPYRIGHT (C) ST Microelectronics  2005
 *            All Rights Reserved
 *
 ***************************************************
 *
 * STM CVS Log:
 *
 * $Log: srvsld.c,v $
 * Revision 1.15  2007/09/26 15:10:45  dellorto
 * added support for hf recover fixing
 *
 * Revision 1.14  2007/08/14 15:29:57  dellorto
 * multiple mechanisms
 *
 * Revision 1.13  2007/04/11 14:21:16  belardi
 * Integration of HAVE_CD_MECHA modification by [GP]
 *
 * Revision 1.12  2007/03/16 14:23:45  dellorto
 * used new profiles
 * track count improved (used for TTM recover)
 * fixed sign of rate jumped tracks
 *
 * Revision 1.11  2006/09/18 09:55:26  belardi
 * Corrected CVS keyword usage
 *
 * Revision 1.10  2006/09/18 09:25:48  belardi
 * Added Log CVS keyword into file header
 *
 *
 ***************************************************/
#include "gendef.h"
#include "hwreg.h"
#include "osal.h"
#include "srvinc.h"
#include "pdebug.h"

#if (HAVE_CD_MECHA == 1)

int16 sledge_param;
int16 rate_jumped_tracks;
t_sledge_condition sledge_condition;
t_sledge_state sledge_fsm_state;

#ifdef HAVE_DEBUG_SERVO_SLEDGE_STATE
  static t_sledge_state previous_sledge_fsm_state;   /* debug only*/
#endif
SERVO_SLEDGE_FLAGS_UNION servo_sledge_flags;


/*----------------------------------------------------------------------------*/
/* Sledge defines                                                                */
/*----------------------------------------------------------------------------*/


#define set_sledge_sw_event(e)          do{event_out[SLEDGE_SW_EVENT].event = e;}while (0)
#define set_sledge_status_event(e)      do{event_out[SLEDGE_STATUS_EVENT].event = e;}while (0)


/******************************************************************************/
/* Function:  set_sledge_for_play                                             */
/*                                                                            */
/*! \brief    Adjust sledge servo parameters and DSP settings for tracking
 *  \param    void
 *  \return   void
 *  \remark   Sets coefficients for the PI controller in the DSP and also some
 *            variables used in the DSP for controlling sledge as actuator slave
 */
/******************************************************************************/

void set_sledge_for_play(void)
{
  dsp_write_ymem(COEFF_SLEDGE_ADD + 0, SLEDGE_PLAY_PRO_TAFILT_GAIN);
  dsp_write_ymem(COEFF_SLEDGE_ADD + 1, 0x0000);
  dsp_write_ymem(COEFF_SLEDGE_ADD + 2, SLEDGE_PLAY_PRO_TAFILT_POLE);
  
  dsp_write_xmem(SLEDTIME_ADD, SLEDGE_PLAY_PRO_SLEDTIME);
  dsp_write_xmem(TAWIN_ADD, SLEDGE_PLAY_PRO_TAWIN);
  dsp_write_xmem(SLEDPULSE_FWD_TIME_ADD, SLEDGE_PLAY_PRO_SLEDPULSE_FWD_TIME);
  dsp_write_xmem(SLEDPULSE_BWD_TIME_ADD, 0x0000);
  dsp_write_xmem(SLEDPULSE_FWD_AMP_ADD, SLEDGE_PLAY_PRO_SLEDPULSE_FWD_AMP);
  dsp_write_xmem(SLEDPULSE_BWD_AMP_ADD, 0x0000);
}


/******************************************************************************/
/* Function:  set_sledge_for_jump                                             */
/*                                                                            */
/*! \brief    Adjust sledge servo parameters and DSP settings for jumping
 *  \param    void
 *  \return   void
 *  \remark   Sets coefficients for the PI controller in the DSP and also some
 *            variables used in the DSP for controlling sledge as actuator slave
 */
/******************************************************************************/

void set_sledge_for_jump(void)
{
  dsp_write_ymem(COEFF_SLEDGE_ADD + 0, SLEDGE_JUMP_PRO_TAFILT_GAIN);
  dsp_write_ymem(COEFF_SLEDGE_ADD + 1, 0x0000);
  dsp_write_ymem(COEFF_SLEDGE_ADD + 2, SLEDGE_JUMP_PRO_TAFILT_POLE);

  if ((absi(delta_trk) > SEEK_PRO_SHORT_VERYSHORT_JUMP_THS) ||
      (servo_active_flags.field.servo_mode == PAUSE_MODE) ||
      (servo_active_flags.field.servo_mode == PAUSE_SPFMODE))
  {  
    dsp_write_xmem(SLEDTIME_ADD, SLEDGE_JUMP_PRO_SLEDTIME_1);
  }
  else
  {
    dsp_write_xmem(SLEDTIME_ADD, SLEDGE_JUMP_PRO_SLEDTIME_2);
  }

  dsp_write_xmem(TAWIN_ADD, SLEDGE_JUMP_PRO_TAWIN);
  dsp_write_xmem(SLEDPULSE_FWD_TIME_ADD, SLEDGE_JUMP_PRO_SLEDPULSE_FWD_TIME);
  dsp_write_xmem(SLEDPULSE_BWD_TIME_ADD, SLEDGE_JUMP_PRO_SLEDPULSE_BWD_TIME);
  dsp_write_xmem(SLEDPULSE_FWD_AMP_ADD, (uint16)SLEDGE_JUMP_PRO_SLEDPULSE_FWD_AMP);
  dsp_write_xmem(SLEDPULSE_BWD_AMP_ADD, (uint16)SLEDGE_JUMP_PRO_SLEDPULSE_BWD_AMP);
}


/******************************************************************************/
/* Function:  set_sledge_input                                                */
/*                                                                            */
/*! \brief
 *  \param    int16  sledge_input
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void set_sledge_input(sint16 sledge_input)
{
  DISABLE_INTERRUPTS();
  DSP_SSPDL = (uint8)(sledge_input & 0xFF);
  DSP_SSPDH = (uint8)(sledge_input >> 8);
  ENABLE_INTERRUPTS();
}


/******************************************************************************/
/* Function:  sledge_kick                                                     */
/*                                                                            */
/*! \brief
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void sledge_kick(void)
{
  if (sledge_param > 0)
  {
    set_sledge_input(SLEDGE_PRO_KICK_FORWARD);
  }
  else   // (sledge_param < 0)
  {
    set_sledge_input(SLEDGE_PRO_KICK_BACKWARD);
  }  
}


/******************************************************************************/
/* Function:  sledge_constant                                                 */
/*                                                                            */
/*! \brief
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void sledge_constant(void)
{
  if (sledge_param > 0)
  {
    set_sledge_input(SLEDGE_PRO_CONSTANT_FORWARD);
  }
  else   // (sledge_param < 0)
  {    
    set_sledge_input(SLEDGE_PRO_CONSTANT_BACKWARD);
  }
}


/******************************************************************************/
/* Function:  sledge_brake                                                    */
/*                                                                            */
/*! \brief
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void sledge_brake(void)
{
  if (sledge_param > 0)
  {
    set_sledge_input(SLEDGE_PRO_BRAKE_FORWARD);
  }
  else   // (sledge_param < 0)
  {
    set_sledge_input(SLEDGE_PRO_BRAKE_BACKWARD);
  }
}


/******************************************************************************/
/* Function:  sledge_idle                                                     */
/*                                                                            */
/*! \brief
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void sledge_idle(void)
{
  set_sledge_input(0x0000);
}      


/******************************************************************************/
/* Function:  init_sledge_fsm                                                 */
/*                                                                            */
/*! \brief    Initializer for the sledge state machine.
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void init_sledge_fsm(void)
{
  servo_sledge_flags.field.sledge_switch_position = SLEDGE_SWITCH_UNDEFINED;
  sledge_fsm_state = SLEDGE_STATE_IDLE;
#ifdef HAVE_DEBUG_SERVO_SLEDGE_STATE
  previous_sledge_fsm_state = SLEDGE_STATE_IDLE;
#endif
  event_in[SLEDGE_SW_EVENT].event = SLEDGE_SWITCH_UNDEFINED;
  event_in[SLEDGE_STATUS_EVENT].event = SLEDGE_STATUS_EVENT_STOPPED;
}


/******************************************************************************/
/* Function:  sledge_update                                                   */
/*                                                                            */
/*! \brief    Updates sledge_position out of registers, check for home_switch
 *  \param    t_sledge_caller sledge_caller
 *  \return   t_sledge_sw_event
 *  \remark
 */
/******************************************************************************/

t_sledge_sw_event sledge_update(t_sledge_caller sledge_caller)
{
  t_sledge_sw_event sledge_sw_event = (t_sledge_sw_event)SLEDGE_SW_NO_EVENT;
  uint16 current_rate_track;
  uint8 prev_sledge_switch_position;
  uint8 prev_switch_reached_moving_inside;

  prev_sledge_switch_position = servo_sledge_flags.field.sledge_switch_position;
  prev_switch_reached_moving_inside = servo_sledge_flags.field.switch_reached_moving_inside;

  servo_sledge_flags.field.sledge_switch_position = SLEDGE_SWITCH;

  if (SLEDGE_CALLER_DSP_FORWARD == sledge_caller)
  {
    servo_sledge_flags.field.switch_reached_moving_inside = 0;
  }
  else if (SLEDGE_CALLER_DSP_BACKWARD == sledge_caller)
  {
    /* nothing to update */
  }

  if (SLEDGE_CONDITION_STORE_DELTA_TRK == sledge_condition)
  {
    start_short_jump_for_tracking_reset();
    start_timer(SERVO_TRACK_COUNT_TIMER, RECOVER_PRO_TTM_RECOVER_TRACK_COUNT_TIMEOUT);
    sledge_condition = SLEDGE_CONDITION_CHECK_DELTA_TRK;
  }
  else if ((SLEDGE_CONDITION_CHECK_DELTA_TRK == sledge_condition)
        && (short_jump_stopped()))
  {
    current_rate_track = RA_nTRACKS_L;
    current_rate_track += (RA_nTRACKS_H << 8);

    if (absi(current_rate_track) > RECOVER_PRO_TTM_RECOVER_TRACK_COUNT_THS)
    {
      sledge_condition = SLEDGE_CONDITION_NORMAL;
      servo_sledge_flags.field.ttm_fsm_call_request = 1;
    }
    else if (!timer_in_progress(SERVO_TRACK_COUNT_TIMER))
    {
      start_short_jump_for_tracking_reset();
      start_timer(SERVO_TRACK_COUNT_TIMER, RECOVER_PRO_TTM_RECOVER_TRACK_COUNT_TIMEOUT);
    }
  }

  if (SLEDGE_CONDITION_CHECK_LONG_JUMP_DISTANCE == sledge_condition)
  {
    rate_jumped_tracks = RA_nTRACKS_L;
    rate_jumped_tracks += (RA_nTRACKS_H << 8);

    if ((absi(delta_trk)) < (absi(rate_jumped_tracks) + SLEDGE_PRO_BRAKE_TRACKS))
    {
      sledge_condition = SLEDGE_CONDITION_NORMAL;
      if (IS_SLEDGE_MOVING(sledge_fsm_state))
      {
        sledge_fsm_state = SLEDGE_STATE_START_BRAKING;
      }
    }
  }
  else
  {
    rate_jumped_tracks = 0;
  }

  if (SLEDGE_STATE_IDLE != sledge_fsm_state)
  {
    if ((SLEDGE_SWITCH_HOME_POSITION == servo_sledge_flags.field.sledge_switch_position)
     && (sledge_param < 0)
     && (!servo_sledge_flags.field.switch_reached_moving_inside))
    {
      servo_sledge_flags.field.switch_reached_moving_inside = 1;
      if ((SLEDGE_CONDITION_HOME_POSITIONING != sledge_condition)
       && (IS_SLEDGE_MOVING(sledge_fsm_state)))
      {
        sledge_fsm_state = SLEDGE_STATE_START_BRAKING;
      }
    }
  }
  else if (SLEDGE_CALLER_START_CMD == sledge_caller)
  {
    if ((SLEDGE_SWITCH_HOME_POSITION == servo_sledge_flags.field.sledge_switch_position)
     && (sledge_param < 0))
    {
      servo_sledge_flags.field.switch_reached_moving_inside = 1;
    }
    else if (sledge_param > 0)
    {
      servo_sledge_flags.field.switch_reached_moving_inside = 0;
    }
  }

  if (servo_sledge_flags.field.sledge_switch_position != prev_sledge_switch_position)
  {
    sledge_sw_event = (SLEDGE_SW_NEW_EVENT | (servo_sledge_flags.field.switch_reached_moving_inside ? SLEDGE_SWITCH_REACHED_MOVING_INSIDE : 0) | servo_sledge_flags.field.sledge_switch_position);

#ifdef HAVE_DEBUG_SERVO_SLEDGE_EVENT
    if (SLEDGE_SWITCH_HOME_POSITION == servo_sledge_flags.field.sledge_switch_position)
    {
      DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Switch home position", 4, 0));
    }
    else   /* (SLEDGE_SW_EVENT_OPEN == servo_misc_flags.field.sledge_switch_position) */
    {
      DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Switch open", 4, 0));
    }
#endif   /* HAVE_DEBUG_SERVO_SLEDGE_EVENT */
  }

  if (servo_sledge_flags.field.switch_reached_moving_inside != prev_switch_reached_moving_inside)
  {
    sledge_sw_event = (SLEDGE_SW_NEW_EVENT | (servo_sledge_flags.field.switch_reached_moving_inside ? SLEDGE_SWITCH_REACHED_MOVING_INSIDE : 0) | servo_sledge_flags.field.sledge_switch_position);

#ifdef HAVE_DEBUG_SERVO_SLEDGE_EVENT
    if (servo_sledge_flags.field.switch_reached_moving_inside)
    {
      DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Switch reached moving inside", 4, 0));
    }
    else   /* (!servo_misc_flags.field.switch_reached_moving_inside) */
    {
      DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Switch left moving outside", 4, 0));
    }
#endif   /* HAVE_DEBUG_SERVO_SLEDGE_EVENT */
  }

  return sledge_sw_event;
}



/******************************************************************************/
/* Function:  sledge_fsm                                                      */
/*                                                                            */
/*! \brief    Sledge moving state machine (Start, acceleration, driving, braking)
 *  \param    t_sledge_caller sledge_caller
 *  \return   t_sledge_status_event
 *  \remark
 */
/******************************************************************************/

t_sledge_status_event sledge_fsm(t_sledge_caller sledge_caller)
{
  t_sledge_status_event sledge_status_event = (t_sledge_status_event)SLEDGE_STATUS_NO_EVENT;
  
  if (SLEDGE_CALLER_INIT_CMD == sledge_caller)
  {
    /* just check sledge switch for at least one time */
  }
  else if (SLEDGE_CALLER_START_CMD == sledge_caller)
  {
    if (((!servo_sledge_flags.field.switch_reached_moving_inside) && (sledge_param != 0))
     || (sledge_condition == SLEDGE_CONDITION_HOME_POSITIONING))
    {
      if (SLEDGE_STATE_MOVING_CONSTANT == sledge_fsm_state)
      {
        sledge_fsm_state = SLEDGE_STATE_MOVING_CHANGE_PARAM;
      }
      else
      {
        sledge_fsm_state = SLEDGE_STATE_START_MOVING;
        sledge_status_event = SLEDGE_STATUS_EVENT_MOVING;
        DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Sledge moving", 4, 0));
      }
    }
  }
  else if (SLEDGE_CALLER_STOP_CMD == sledge_caller)
  {
    if (IS_SLEDGE_MOVING(sledge_fsm_state))
    {
      sledge_fsm_state = SLEDGE_STATE_START_BRAKING;
    }
  }

#ifdef HAVE_DEBUG_SERVO_SLEDGE_STATE
  if (previous_sledge_fsm_state != sledge_fsm_state)
  {
    previous_sledge_fsm_state = sledge_fsm_state;
    DEBUG_SERVO_SLEDGE_STATE(("SLEDGE_STATE:", 1, 1, sledge_fsm_state));
  }
#endif   /* HAVE_DEBUG_SERVO_SLEDGE_STATE */

  switch (sledge_fsm_state)
  {
  case SLEDGE_STATE_IDLE:
    break;

  case SLEDGE_STATE_START_BRAKING:
    sledge_brake();
    start_timer(SERVO_SLEDGE_TIMER, SLEDGE_PRO_BRAKE_TIME);
    sledge_fsm_state = SLEDGE_STATE_BRAKING;
    break;

  case SLEDGE_STATE_START_MOVING: