sony_servo.c

索尼CX3068VCD伺服的随身听

 * direction.
 */
void SERVO_trackjump_10(qdirection)
int qdirection;
{
    SERVO_send_command(4, 0x5800);	/* FWD/RVS 10 */

    if (qdirection)
		SERVO_send_command(4, 0x4a00);
    else
		SERVO_send_command(4, 0x4b00);

    trackjump_monitor(8);
}


/*
 * sub-routine to sent a command to DSP to jump 2N tracks to the selected
 * direction.
 */
void SERVO_2N_trackjump(int qdirection, int track_diff)
{
    int temp, timeout;

    temp = (track_diff >> 1) | 0x70000;
    SERVO_send_command(5, temp);

    SERVO_CLV_STOP;
    SERVO_send_command(4, 0x5b00);
    SERVO_send_command(4, 0x6200);

    if (qdirection) {
        SERVO_send_command(4, 0x4C00);
    } else {
        SERVO_send_command(4, 0x4D00);
    }
    if (track_diff >= 100)
	timeout = 30;
    else if (track_diff >= 64)
	timeout = 24;
    else if (track_diff >= 32)
	timeout = 16;
    else
	timeout = 10;
    trackjump_monitor(timeout);
/*    if (!servo_focus_on) */
    	zputs("SERVO_2N_trackjump - SERVO_CLV_A - ERROR\n");
    SERVO_CLV_A;

    return;
}

/*
 * M-track move : move sled only
 */
void SERVO_M_trackmove(qdirection, track_diff)
int qdirection, track_diff;
{
#if 0	// Wenxin -- never used variable
    int i;
#endif

	if(!qdirection){
		SERVO_CLV_KICK;
	}else
		SERVO_CLV_STOP;

    SERVO_send_command(5, 0x70000 | track_diff);

    SERVO_send_command(4, 0x5b00);
	
    if (qdirection) {
    SERVO_send_command(4, 0x4e00);
    } else {
    SERVO_send_command(4, 0x4f00);
    }

    long_trackjump_monitor();
    /*SERVO_delay_n_ms(150);	 */
    SERVO_CLV_A;

    /* Add delay time according to searching distance */
/*    for (i = 0; i <= (track_diff >> 11); i++)
	SERVO_delay_n_ms(100);
*/
    SERVO_TRK_SLD_ON;
    return;
}

q/*
 * Comparing the current position with the target position. Return 1 if
 * the difference is in certain range
 */
int SERVO_match(void)
{
    int qresult;
    int track_diff;
    	zputs("match \n");

    if (SERVO_Qarray_new[SERVO_QTRACK] == 0xaa) {

		SERVO_send_command(2, TG_UP);
		SERVO_send_command(6, 0x38148e);
		SERVO_2N_trackjump(0, 100);
		SERVO_send_command(6, 0x38140e);
		SERVO_delay_n_ms(4);
    	SERVO_send_command(2, TG_NOR);
		return 0;
    }

    qresult = (CVbcd2hex[SERVO_Qarray_new[SERVO_QPMIN]] - 
	       CVbcd2hex[target_min]) * 75 * 60 +
	      (CVbcd2hex[SERVO_Qarray_new[SERVO_QPSEC]] - 
	       CVbcd2hex[target_sec]) * 75 +
	      (CVbcd2hex[SERVO_Qarray_new[SERVO_QPFRAME]] - 
	       CVbcd2hex[target_frame]);
    track_diff = abs2trk[CVbcd2hex[SERVO_Qarray_new[SERVO_QPMIN]]] -
	abs2trk[CVbcd2hex[target_min]];

    if ((qresult > -29) && (qresult < -2))
	return (1);

    Do_mute();
    SERVO_send_command(6, 0x38148e);
    SERVO_send_command(2, TG_UP);

    if (qresult > 75*60*3)	{

    	if(track_diff>200)		SERVO_M_trackmove(0, track_diff);
    	else
    	SERVO_2N_trackjump(0, 100);
	}
    else if (qresult > 75*40)  
	SERVO_2N_trackjump(0, 100);
    else if (qresult > 75*4)   
	SERVO_trackjump_10(0);
    else if (qresult > 10)
	SERVO_trackjump_3(0);
    else if (qresult > -2)
	SERVO_trackjump_1(0);
    else if (qresult > -29)	{

    	reset_emergency_flag();
    	SERVO_TRK_SLD_ON;
    	SERVO_send_command(6, 0x38140e);
    	SERVO_delay_n_ms(4);
    	SERVO_send_command(2, TG_NOR);


	return 1;}
    else if (qresult > -74)  
	SERVO_trackjump_1(1);
    else if (qresult > -75*4)   
	SERVO_trackjump_3(1);
    else if (qresult > -75*40)
	SERVO_trackjump_10(1);
    else if (qresult > -75*60*3)
	SERVO_2N_trackjump(1, 100);
    else{
    
    	if(-track_diff > 300) SERVO_M_trackmove(1, -track_diff);
    	else
    	SERVO_2N_trackjump(1,100);
    	}

/*    SERVO_delay_n_ms(10); */
    reset_emergency_flag();

    SERVO_TRK_SLD_ON;
    SERVO_send_command(6, 0x38140e);
    SERVO_delay_n_ms(4);
    SERVO_send_command(2, TG_NOR);

    return (0);
}



/*
 * Read jitter value
 */
void SERVO_rfjsmp(void)
{
    unsigned int count;
    unsigned char jitter_value;
    unsigned int servo_per_total;
    int servo_fb_cnt;

    SET_SERVO_SQCK;

    servo_per_total = 0;

    for (servo_fb_cnt=0; servo_fb_cnt< FB_SPL; servo_fb_cnt++)
    {
    SERVO_delay_n_ms(2);
    	/* Set SQSO as Jitter readout mode */
    	SERVO_send_8X_command(1, 0);	/* $80260030 */

        jitter_value = (SERVO_SQSO_HIGH) ? 0x80 : 0x0;
        for (count = 0; count < 7; count++) {
            /* getting 7 other bits */
			SERVO_Wait750();
		    CLEAR_SERVO_SQCK;
			SERVO_Wait750();
		    SET_SERVO_SQCK;
		    jitter_value >>= 1; 
 		    if (SERVO_SQSO_HIGH) { jitter_value |= 0x80; } 
   	     }

         servo_per_total += jitter_value;
    } /* end for loop */

    /* Set SQSO as subQ readout mode */
    SERVO_send_8X_command(0, 0);
/*    SERVO_send_8X_command(0, 1); */

    servo_per_ave = udiv(servo_per_total, FB_SPL);

    return; 
}

/*
 * E:F Balance Adjustment Task
 */ 
void SERVO_EF_balance(void)
{
    zputs("SERVO_EF_balance\n");
    if (servo_EF_balance_ok == 1) {
		SERVO_AGC_CANCEL;
		servo_pres_m = FBIAS_10;
		servo_task = SERVO_fbias;
    } else {
	/* need to do E:F: balance */
        SERVO_send_command(6, 0x3814AA);/* Set TBLM to 1, $3814AA */
        SERVO_send_command(2, TOFF_SFWD);
    	servo_sys_timer = T200MS;
		servo_pres_m = EFV_10;
		servo_task = SERVO_efv_10;
    }

    return;
}

void SERVO_efv_10()
{
    if (servo_focus_drop || servo_gfs_drop) {
	SERVO_TRK_SLD_OFF;
	SERVO_emrchk();
    } else if (servo_sys_timer == 0) {
	servo_EF_balance_ok = 1;
	SERVO_AGC_CANCEL;
	SERVO_fcson();
   }
}


/*
 * Focus Bias Adjustment
 *
 * SERVO_fbias(void)
 * Finding and setting the auto-adjusted focus biasing value.
 */
extern void mainloop();
void SERVO_fbias(void)
{
    /*int jcnt, r_bias;*/
    /*int min_jitter_index;*/
    unsigned short jitter[FB_R_LIMIT*2/FB_STP+1];
    int bias_A, bias_B, min_jitter, min_jitter_fbias=0,min_jitter_index=0;
	int jcnt;
#if 0	// Wenxin -- never used variable
	int i;
#endif

    zputs("SERVO_fbias\n");

	if (servo_fb_ok == 1) {
        /* bias adjustment has completed */
    	zputs("SERVO_fbias OK \n");
	servo_pres_m = AGC_10;
	servo_task = SERVO_agc;
        return;
    } else {
	/* let's do focus bias adjustment */
    	zputs("SERVO_fbias do focus\n");
        for (bias_A = 0; bias_A < FB_R_LIMIT*2/FB_STP+1; bias_A++)
            jitter[bias_A] = 0;
        bias_A = 0;

        jcnt = FB_R_LIMIT / FB_STP;
	SERVO_send_command(6, 0x3a4000);	/* FBON */

        /* start sampling from FB=0 */
        servo_fbias = 0x0;
		min_jitter = 0x0fff;  /* init to an impossibly high value */

        /* increment focus bias, searching for the minimum jitter point */
        while (1) {
            SERVO_fb_dtr(); /* send out focus bias value */
            SERVO_rfjsmp();
	    	if (servo_focus_drop || servo_gfs_drop) {
				servo_fbias = 0;	/* Set to default value */
				SERVO_fb_dtr();
				SERVO_emrchk();
				return;
	    	}
            jitter[jcnt] = servo_per_ave;
            if (servo_per_ave < min_jitter) {
                min_jitter = servo_per_ave;
                min_jitter_fbias = servo_fbias;
                min_jitter_index= jcnt;
            }
	    	if (servo_per_ave > (min_jitter+3)) {
				bias_A=servo_fbias;
				break;
	    	} else if (servo_fbias >= FB_R_LIMIT) {
				bias_A=0x80;
				break;	/* Do not go beyond limit */
            }
            servo_fbias += FB_STP;
	    	jcnt ++;
    		/*if( cdm_check_exit() ) { 
    		
    			FIX_KYopcl();
				mainloop();
    			return; 
    		} */ //kzy changed 12.10
    		/*if( cdm_check_exit() ) { 
    	      if (plyMode==PLY_MP3)
    	      {
    	        if( exit_code == EXIT_OPEN ) {
                 handle_open();
    	         }
    	      else{ 
    	      
              if( exit_code == EXIT_OPEN ) 
              {
                    FIX_KYopcl();
                 }
              else if( exit_code == EXIT_STDBY ) 
              {
                    FIX_KYstdby();
                  }
                  }
                 mainloop();
                   return; 
                   }
               }
*/
SERVO_Idle();

if( cdm_check_exit() ) { 
#ifdef ENABLE_MP3      
    	      if (plyMode==PLY_MP3)
    	      {
    	        if( exit_code == EXIT_OPEN ) {
                 handle_open();
    	         }
    	      }
    	      else
#endif    	      
    	      { 
    	      
              if( exit_code == EXIT_OPEN ) 
              {
                    FIX_KYopcl();
                 }
              else if( exit_code == EXIT_STDBY ) 
              {
                    FIX_KYstdby();
                  }
              }
              mainloop();
              return; 
       }

}
/*	r_bias = jcnt;*/

	/* Start from current min_jitter_fbias */
        servo_fbias = min_jitter_fbias - FB_STP;
        SERVO_fb_dtr();
        SERVO_delay_n_ms(5);	/* wait some time for focus to moving */

        jcnt = min_jitter_index -1;
        /* decrement focus bias, continue searching for min jitter */
        while (1) {
            SERVO_rfjsmp();
	    if (servo_focus_drop || servo_gfs_drop) {
		servo_fbias = 0;	/* Set to default value */
		SERVO_fb_dtr();
		SERVO_emrchk();
		return;
	    }
            jitter[jcnt] = servo_per_ave;
            if (servo_per_ave < min_jitter) {
                min_jitter = servo_per_ave;
                min_jitter_fbias = servo_fbias;
                min_jitter_index= jcnt;
            }
	    if (servo_per_ave > (min_jitter+3)) {
		bias_B=servo_fbias;
		break;
	    } else if (servo_fbias <= FB_L_LIMIT) {
	    bias_B=-0x80;
		break;	/* Do not go beyond limit */
            }

            servo_fbias -= FB_STP;
            SERVO_fb_dtr();
	    jcnt --;
	}
 
        /* done finding minimum, and also found Point A */
/*        bias_A = servo_fbias;*/

	/* Looking for Point B */           
/*        for (i= min_jitter_index+1; i<=r_bias; i++) {
            if (jitter[i] > (min_jitter + 3)) {
                break;
            }
        }*/
/*	if (i > r_bias)	
	    i = r_bias;
        bias_B = (i-(FB_R_LIMIT/FB_STP)) * FB_STP;
*/
        /* calculate mid_point between bias_A and bias_B */
        servo_fbias = (bias_B + bias_A) >> 1;

        /*SERVO_delay_n_ms(10);*/
        /* send out auto-adjustment focus biasing value */
        SERVO_fb_dtr();
        SERVO_delay_n_ms(2);
        servo_fb_ok = 1;
	servo_task = SERVO_agc;
    }

    return;
}

/*
 * Set focus bias data
 */
void SERVO_fb_dtr()
{  
    unsigned int fb_data;

    fb_data = 0x34f400 | ((servo_fbias & 0x1ff) << 1);
    SERVO_send_command(6, fb_data);

    return;
}
    
/******************************************************
 *             Automatic Gain Control Task *
 ******************************************************
 */
void SERVO_agc(void)
{
    /* Focus AGC adjustment complete ? */
    zputs("SERVO_agc\n");
    if (servo_AGF_ok == 1) {
	SERVO_ag_trk();
    } else {
	servo_sys_timer = T4SECOND;
/*	servo_sys_timer = T2SECOND; */
	SERVO_send_command(6, 0x38168e);	/* set AGF to 1 */
	servo_pres_m = AGC_10;
	servo_task = SERVO_agc_10;
    }

    return;
}

void SERVO_agc_10(void)
{
    /* Emergency detection */
    if (servo_focus_drop || servo_gfs_drop) {
	SERVO_AGC_CANCEL;
	SERVO_emrchk();
	return;
    }

   	enable_interrupt = 0;
    do {
    SERVO_send_command_no_latch(2, 0x38);	/* read SENS */
    }while(SERVO_SENS_HIGH); 
   	enable_interrupt = 1;

	servo_AGF_ok = 1;
	SERVO_ag_trk();
	return;
    /*else {
	if (servo_sys_timer == 0) {
	    SERVO_AGC_CANCEL;
	    SERVO_ag_trk();
	}
    }*/
}

/* Tracking Auto Gain */
void SERVO_ag_trk(void)
{
    zputs("SERVO_ag_trk\n");
    /* Tracking AGC adjustment complete ? */
    if (servo_AGT_ok == 1) {
		track_ready = 1;
		focus_ready=1;
		servo_pres_m = PLAY_10;
		servo_task = null_func;
    	zputs("TRACK OK\n");
    } else {
	SERVO_send_command(6, 0x38158e);	/* Set AGT to 1 */
	SERVO_short_delay(3);
	servo_sys_timer = T3SECOND;
/*	servo_sys_timer = T2SECOND; */

	servo_pres_m = AGC_20;
	servo_task = SERVO_agc_20;
    }

    return;
}

void SERVO_agc_20(void)
{
    zputs("SERVO_agc_20\n");
    /* Emergency detection */
    if (servo_focus_drop || servo_gfs_drop) {
/*    	IoPrint2("SERVO_agc_20 drop %d %d\n",servo_focus_drop,servo_gfs_drop); */
		SERVO_AGC_CANCEL;
		SERVO_emrchk();
		return;
    }