📄 srvopu.c
字号:
}
else
{
fe_adjust_result = check_servo(CHECK_TTM);
if (fe_adjust_result == OK)
{
fe_adjust_result = IN_PROGRESS;
switch (fe_adjust_state)
{
case STATE_1:
DEBUG_SERVO(("fe_adjust start", 4, 0));
event_in[DSP_FOCUS_EVENT].event = FOCUS_EVENT_OPEN;
check_hf_fsm_call(CHECK_HF_CALLER_FORCE_HF_LOST_CMD);
servo_recover_flags.field.focus_missing = 1;
to_dsp_flags_pc(FCS_OFF);
dsp_write_xmem(MODE_ADD, ADJ_MODE);
CDinfo &= ~LOW_REFLECTIVE;
servo_misc_flags.field.channel_saturated = 0;
servo_function_counter = 0;
servo_function_counter_2 = 0;
ANA_GAIN_AC = OPU_PRO_NORMAL_ANA_GAIN_ACBD;
ANA_GAIN_BD = OPU_PRO_NORMAL_ANA_GAIN_ACBD;
ANA_GAIN_E = OPU_PRO_NORMAL_ANA_GAIN_EF;
ANA_GAIN_F = OPU_PRO_NORMAL_ANA_GAIN_EF;
starting_gain = FE_ADJUST_PRO_STARTING_GAIN;
dsp_write_ymem(COEFF_ERROR_ADD + 0, starting_gain); /* starting gain value for AC signal in FE generation */
dsp_write_ymem(COEFF_ERROR_ADD + 1, starting_gain); /* starting gain value for BD signal in FE generation */
start_ramp_up();
start_timer(SERVO_SAFETY_TIMER, FE_ADJUST_PRO_TIMEOUT);
fe_adjust_state = STATE_2;
break;
case STATE_2:
if (ramp_stopped())
{
fe_adjust_state = STATE_3;
}
break;
case STATE_3:
if (OK == abcdef_offset_calibration())
{
if (servo_function_counter_2 != 4)
{
ac_acc = 0;
bd_acc = 0;
e_acc = 0;
f_acc = 0;
error_amp = 0;
servo_function_counter++;
servo_function_counter_2 = 0;
fe_adjust_state = STATE_4;
}
else
{
fe_adjust_state = STATE_6;
}
}
else
{
fe_adjust_result = CALIBRATION_ERROR;
}
break;
case STATE_4:
servo_function_counter_2++;
/* start ABCDEF max measurement */
enable_max_min_measurement(); /* select max/min measurement */
start_max_measurement(CHANNEL_AC); /* start AC max measurement */
start_max_measurement(CHANNEL_BD); /* start BD max measurement */
start_max_measurement(CHANNEL_E); /* start E max measurement */
start_max_measurement(CHANNEL_F); /* start F max measurement */
// start FE measurement
dsp_write_xmem(ADJUST_FLAGS_ADD, ADJUST_MINMAX_START);
if (servo_function_counter_2 & 0x01) /* servo_function_counter odd */
{
start_ramp_down();
}
else /* servo_function_counter even */
{
start_ramp_up();
}
fe_adjust_state = STATE_5;
break;
case STATE_5:
if (ramp_stopped())
{
/* stop ABCDEF max measurement, read and accumulate */
stop_max_measurement(CHANNEL_AC); /* stop AC max measurement */
ac_acc += (uint8)(read_measurement_ac()); /* read AC max measurement and accumulate */
stop_max_measurement(CHANNEL_BD); /* stop BD max measurement */
bd_acc += (uint8)(read_measurement_bd()); /* read BD max measurement and accumulate */
stop_max_measurement(CHANNEL_E); /* stop E max measurement */
e_acc += (uint8)(read_measurement_e()); /* read E max measurement and accumulate */
stop_max_measurement(CHANNEL_F); /* stop F max measurement */
f_acc += (uint8)(read_measurement_f()); /* read F max measurement and accumulate */
/* stop FE measurement, read and accumulate */
error_amp += ((uint16)(dsp_read_xmem(FEMAX_ADD) - dsp_read_xmem(FEMIN_ADD))) >> 2;
if ((servo_function_counter == 1) && (servo_function_counter_2 == 2))
{
find_abcdef_amplitude();
reflectivity = abcd_amp; /* assume reflectivity proportional to ABCD amplitude */
if (abcd_amp < FE_ADJUST_PRO_SUBSTRATE_THS)
{
fe_adjust_result = FE_ADJUST_RETRY;
}
else
{
if (servo_misc_flags.field.channel_saturated)
{
abcdef_gain_calibration();
fe_adjust_state = STATE_3;
}
else if (abcd_amp < FE_ADJUST_PRO_LOW_REFLECTIVE_THS)
{
CDinfo |= LOW_REFLECTIVE;
abcdef_gain_calibration();
fe_adjust_state = STATE_3;
}
else
{
fe_adjust_state = STATE_4;
}
}
}
else if (servo_function_counter_2 == 4)
{
find_abcdef_amplitude();
abcdef_gain_calibration();
fe_adjust_state = STATE_3;
}
else
{
fe_adjust_state = STATE_4;
}
}
break;
case STATE_6:
/* FE amplitude extimation with final photodiode gains */
error_amp = error_amp / abcd_amp;
error_amp = error_amp * FE_ADJUST_PRO_ABCDEF_TARGET_AMPLITUDE;
if (error_amp < 0x3000)
{
fe_adjust_result = FE_ADJUST_ERROR;
}
else
{
/* FE gain calculation */
fe_gain = aux_mult(starting_gain, FE_ADJUST_PRO_FE_TARGET_AMPLITUDE);
fe_gain = aux_divide(fe_gain, error_amp);
DEBUG_SERVO(("fe_gain ", 2, 1, fe_gain));
dsp_write_ymem(COEFF_ERROR_ADD + 0, fe_gain);
dsp_write_ymem(COEFF_ERROR_ADD + 1, fe_gain);
fe_adjust_state = STATE_7;
}
break;
//case STATE_7:
default:
if (focus_off() == OK)
{
servo_startup_flags.field.fe_adjust_done = 1;
fe_adjust_result = OK;
}
break;
}
}
/* else not needed ((fe_adjust_result = check_servo(CHECK_TTM)) == TTM_STOPPED) */
}
return fe_adjust_result;
}
/******************************************************************************/
/* Function: te_adjust (state machine) */
/* */
/*! \brief provides tracking error amplitude normalization
* \param void
* \return OK in case of tracking error adjust success
* IN_PROGRESS in case of tracking error adjust in progress
* \remark
*/
/******************************************************************************/
RETVAL te_adjust(void)
{
RETVAL te_adjust_result;
sint16 te_max;
sint16 te_min;
uint8 corr;
if ((te_adjust_state != STATE_1) && (!timer_in_progress(SERVO_SAFETY_TIMER)))
{
te_adjust_result = TE_ADJUST_TIMEOUT_ERROR;
}
else if (servo_startup_flags.field.te_adjust_done)
{
te_adjust_result = OK;
}
else /* (!servo_startup_flags.field.te_adjust_done) */
{
te_adjust_result = check_servo(CHECK_TTM | CHECK_FOCUS | CHECK_HF);
if (te_adjust_result == OK)
{
te_adjust_result = IN_PROGRESS;
switch (te_adjust_state)
{
case STATE_1:
DEBUG_SERVO(("te_adjust start", 4, 0));
starting_gain = (TE_ADJUST_PRO_STARTING_GAIN);
dsp_write_ymem(COEFF_ERROR_ADD + 3, starting_gain);
dsp_write_xmem(MODE_ADD, ADJ_MODE);
servo_function_counter_2 = 3;
start_timer(SERVO_SAFETY_TIMER, TE_ADJUST_PRO_TIMEOUT);
/* fall through */
case STATE_2:
error_amp = 0;
te_mean = 0;
servo_function_counter = 4;
/* fall through */
case STATE_3:
dsp_write_xmem(ADJUST_FLAGS_ADD, ADJUST_MINMAX_START);
start_timer(SERVO_FUNCTION_TIMER, TE_ADJUST_PRO_MEAS_TIMEOUT);
te_adjust_state = STATE_4;
break;
//case STATE_4:
default:
if (!timer_in_progress(SERVO_FUNCTION_TIMER))
{
te_max = dsp_read_xmem(TEMAX_ADD);
te_min = dsp_read_xmem(TEMIN_ADD);
error_amp += (((uint16)(te_max - te_min)) >> 2);
te_mean += (((sint16)(te_max + te_min)) / 8);
servo_function_counter--;
if (servo_function_counter != 0)
{
te_adjust_state = STATE_3;
}
else
{
if (error_amp < 0x3000)
{
te_adjust_result = TE_ADJUST_ERROR;
}
else
{
if (te_mean > 0)
{
corr = find_level_shift_increment((uint8)((te_mean + 0x80) >> 8), 0xA0, ANA_GAIN_E);
ANA_LS_E += corr;
}
else /* (te_mean <= 0) */
{
corr = find_level_shift_increment((uint8)((-te_mean + 0x80) >> 8), 0xA0, ANA_GAIN_F);
ANA_LS_F += corr;
}
servo_function_counter_2--;
if ((corr == 0) || (servo_function_counter_2 == 0))
{
te_gain = aux_mult(starting_gain, TE_ADJUST_PRO_TE_TARGET_AMPLITUDE);
te_gain = aux_divide(te_gain, error_amp);
DEBUG_SERVO(("te_gain ", 2, 1, te_gain));
dsp_write_ymem(COEFF_ERROR_ADD + 3, te_gain);
servo_startup_flags.field.te_adjust_done = 1;
te_adjust_result = OK;
DEBUG_SERVO(("te_adjust OK", 4, 0));
}
else
{
te_adjust_state = STATE_2;
}
}
}
}
/* else not needed (timer_in_progress(SERVO_FUNCTION_TIMER)) */
break;
}
}
else /* ((te_adjust_result = check_servo(CHECK_TTM | CHECK_FOCUS | CHECK_HF)) == TTM_STOPPED / FOCUS_LOST / HF_LOST) */
{
stop_timer(SERVO_FUNCTION_TIMER);
}
}
return te_adjust_result;
}
#ifdef APM_PICKUP /* [RB] commented out to reduce ROM space */
/******************************************************************************/
/* Function: get_reflectivity */
/* */
/*! \brief
* \param
* \return
* \remark
*/
/******************************************************************************/
uint8 get_reflectivity(void)
{
return reflectivity;
}
#endif
#endif // HAVE_CD_MECHA
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -