werunplugged/unplugged-kernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
d30235b04c
unplugged-kernel/drivers/misc/mediatek/lens/mtk/main/common/lc898122af/OisCmd.c

2905 lines
78 KiB
C
Raw Normal View History

Initial commit: Linux kernel 4.19 for Unplugged OS
2025-10-06 13:52:42 +00:00
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (c) 2019 MediaTek Inc.
*/
/* ************************** */
/* Include Header File */
/* ************************** */
#define OISCMD
/* #include "Main.h" */
/* #include "Cmd.h" */
#include "Ois.h"
#include "OisDef.h"
struct stAdjPar StAdjPar; /* Execute Command Parameter */
unsigned char UcOscAdjFlg; /* For Measure trigger */
unsigned long UlH1Coefval; /* H1 coefficient value */
unsigned char UcH1LvlMod; /* H1 level coef mode */
/* ************************** */
/* define */
/* ************************** */
#define MES_XG1 0 /* LXG1 Measure Mode */
#define MES_XG2 1 /* LXG2 Measure Mode */
#define HALL_ADJ 0
#define LOOPGAIN 1
#define THROUGH 2
#define NOISE 3
/* Measure Mode */
#define TNE 80 /* Waiting Time For Movement */
#ifdef HALLADJ_HW
#define __MEASURE_LOOPGAIN 0x00
#define __MEASURE_BIASOFFSET 0x01
#else
/******* Hall calibration Type 1 *******/
#define MARJIN 0x0300 /* Marjin */
#define BIAS_ADJ_BORDER \
0x1998 /* HALL_MAX_GAP < BIAS_ADJ_BORDER < HALL_MIN_GAP(80%) */
#define HALL_MAX_GAP (BIAS_ADJ_BORDER - MARJIN)
#define HALL_MIN_GAP (BIAS_ADJ_BORDER + MARJIN)
#define BIAS_LIMIT 0xFFFF /* HALL BIAS LIMIT */
#define OFFSET_DIV 2 /* Divide Difference For Offset Step */
#define TIME_OUT 40 /* Time Out Count */
/******* Hall calibration Type 2 *******/
#define MARGIN 0x0300 /* Margin */
#define BIAS_ADJ_OVER 0xD998 /* 85% */
#define BIAS_ADJ_RANGE 0xCCCC /* 80% */
#define BIAS_ADJ_SKIP 0xBFFF /* 75% */
#define HALL_MAX_RANGE (BIAS_ADJ_RANGE + MARGIN)
#define HALL_MIN_RANGE (BIAS_ADJ_RANGE - MARGIN)
#define DECRE_CAL 0x0100 /* decrease value */
#endif
#ifdef H1COEF_CHANGER
#ifdef CORRECT_1DEG
#define MAXLMT 0x40400000 /* 3.0 */
#define MINLMT 0x3FE66666 /* 1.8 */
#define CHGCOEF 0xBA195555 /* */
#else
#define MAXLMT 0x40000000 /* 2.0 */
#define MINLMT 0x3F8CCCCD /* 1.1 */
#define CHGCOEF 0xBA4C71C7 /* */
#endif
#define MINLMT_MOV 0x00000000 /* 0.0 */
#define CHGCOEF_MOV 0xB9700000
#endif
/* ************************** */
/* Global Variable */
/* ************************** */
#ifdef HALLADJ_HW
unsigned char UcAdjBsy;
#else
unsigned short UsStpSiz; /* Bias Step Size */
unsigned short UsErrBia, UsErrOfs;
#endif
/* ************************** */
/* Const */
/* ************************** */
/* gxzoom Setting Value */
#define ZOOMTBL 16
const unsigned long ClGyxZom[ZOOMTBL] = {
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000,
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000,
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000};
/* gyzoom Setting Value */
const unsigned long ClGyyZom[ZOOMTBL] = {
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000,
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000,
0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000};
/* DI Coefficient Setting Value */
#define COEFTBL 7
const unsigned long ClDiCof[COEFTBL] = {
DIFIL_S2, /* 0 */
DIFIL_S2, /* 1 */
DIFIL_S2, /* 2 */
DIFIL_S2, /* 3 */
DIFIL_S2, /* 4 */
DIFIL_S2, /* 5 */
DIFIL_S2 /* 6 */
};
unsigned short TneRun(void)
{
unsigned char UcHlySts, UcHlxSts, UcAtxSts, UcAtySts;
unsigned short UsFinSts, UsOscSts; /* Final Adjustment state */
unsigned char UcDrvMod;
#ifndef HALLADJ_HW
union UnDwdVal StTneVal;
#endif
#ifdef USE_EXTCLK_ALL /* 24MHz */
UsOscSts = EXE_END;
#else
#ifdef MODULE_CALIBRATION
/* OSC adjustment */
UsOscSts = OscAdj();
#else
UsOscSts = EXE_END;
#endif
#endif
UcDrvMod = UcPwmMod;
if (UcDrvMod == PWMMOD_CVL)
DrvPwmSw(Mpwm); /* PWM mode */
#ifdef HALLADJ_HW
UcHlySts = BiasOffsetAdj(Y_DIR, 0);
WitTim_LC898122AF(TNE);
UcHlxSts = BiasOffsetAdj(X_DIR, 0);
WitTim_LC898122AF(TNE);
UcHlySts = BiasOffsetAdj(Y_DIR, 1);
WitTim_LC898122AF(TNE);
UcHlxSts = BiasOffsetAdj(X_DIR, 1);
SrvCon(Y_DIR, OFF);
SrvCon(X_DIR, OFF);
if (UcDrvMod == PWMMOD_CVL)
DrvPwmSw(Mlnp); /* PWM mode */
#ifdef NEUTRAL_CENTER
TneHvc();
#endif /* NEUTRAL_CENTER */
#else
/* StbOnnN( OFF , ON ) ; */ /* Y OFF, X ON */
WitTim_LC898122AF(TNE);
StTneVal.UlDwdVal = TnePtp(Y_DIR, PTP_BEFORE);
/* UcHlySts = TneCen( Y_DIR, StTneVal ) ; */
UcHlySts = TneCen(Y2_DIR, StTneVal);
StbOnnN(ON, OFF); /* Y ON, X OFF */
WitTim_LC898122AF(TNE);
StTneVal.UlDwdVal = TnePtp(X_DIR, PTP_BEFORE);
/* UcHlxSts = TneCen( X_DIR, StTneVal ) ; */
UcHlxSts = TneCen(X2_DIR, StTneVal);
StbOnnN(OFF, ON); /* Y OFF, X ON */
WitTim_LC898122AF(TNE);
StTneVal.UlDwdVal = TnePtp(Y_DIR, PTP_AFTER);
/* UcHlySts = TneCen( Y_DIR, StTneVal ) ; */
UcHlySts = TneCen(Y2_DIR, StTneVal);
StbOnnN(ON, OFF); /* Y ON, X OFF */
WitTim_LC898122AF(TNE);
StTneVal.UlDwdVal = TnePtp(X_DIR, PTP_AFTER);
/* UcHlxSts = TneCen( X_DIR, StTneVal ) ; */
UcHlxSts = TneCen(X2_DIR, StTneVal);
SrvCon(Y_DIR, OFF);
SrvCon(X_DIR, OFF);
if (UcDrvMod == PWMMOD_CVL)
DrvPwmSw(Mlnp); /* PWM mode */
#ifdef NEUTRAL_CENTER
TneHvc();
#endif /* NEUTRAL_CENTER */
#endif
WitTim_LC898122AF(TNE);
RamAccFixMod(ON); /* Fix mode */
StAdjPar.StHalAdj.UsAdxOff =
(unsigned short)(0x00010000 -
(unsigned long)StAdjPar.StHalAdj.UsHlxCna);
StAdjPar.StHalAdj.UsAdyOff =
(unsigned short)(0x00010000 -
(unsigned long)StAdjPar.StHalAdj.UsHlyCna);
RamWriteA_LC898122AF(OFF0Z, StAdjPar.StHalAdj.UsAdxOff); /* 0x1450 */
RamWriteA_LC898122AF(OFF1Z, StAdjPar.StHalAdj.UsAdyOff); /* 0x14D0 */
RamReadA_LC898122AF(DAXHLO, &StAdjPar.StHalAdj.UsHlxOff); /* 0x1479 */
RamReadA_LC898122AF(DAXHLB, &StAdjPar.StHalAdj.UsHlxGan); /* 0x147A */
RamReadA_LC898122AF(DAYHLO, &StAdjPar.StHalAdj.UsHlyOff); /* 0x14F9 */
RamReadA_LC898122AF(DAYHLB, &StAdjPar.StHalAdj.UsHlyGan); /* 0x14FA */
RamReadA_LC898122AF(OFF0Z, &StAdjPar.StHalAdj.UsAdxOff); /* 0x1450 */
RamReadA_LC898122AF(OFF1Z, &StAdjPar.StHalAdj.UsAdyOff); /* 0x14D0 */
RamAccFixMod(OFF); /* Float mode */
StbOnn(); /* Slope Mode */
WitTim_LC898122AF(TNE);
#ifdef MODULE_CALIBRATION
/* X Loop Gain Adjust */
UcAtxSts = LopGan(X_DIR);
/* Y Loop Gain Adjust */
UcAtySts = LopGan(Y_DIR);
#else /* default value */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(sxg, &StAdjPar.StLopGan.UsLxgVal); /* 0x10D3 */
RamReadA_LC898122AF(syg, &StAdjPar.StLopGan.UsLygVal); /* 0x11D3 */
RamAccFixMod(OFF); /* Float mode */
UcAtxSts = EXE_END;
UcAtySts = EXE_END;
#endif
TneGvc();
UsFinSts = (unsigned short)(UcHlxSts - EXE_END) +
(unsigned short)(UcHlySts - EXE_END) +
(unsigned short)(UcAtxSts - EXE_END) +
(unsigned short)(UcAtySts - EXE_END) +
(UsOscSts - (unsigned short)EXE_END) +
(unsigned short)EXE_END;
return UsFinSts;
}
#ifndef HALLADJ_HW
unsigned long TnePtp(unsigned char UcDirSel, unsigned char UcBfrAft)
{
union UnDwdVal StTneVal;
MesFil(THROUGH); /* <20><><EFBFBD><EFBFBD><EFBFBD>p<EFBFBD>t<EFBFBD>B?<3F>^<5E>[<5B><><EFBFBD>ݒ肷<DD92><E882B7><EFBFBD>B */
if (!UcDirSel) {
RamWrite32A_LC898122AF(sxsin, HALL_H_VAL); /* 0x10D5 */
SetSinWavePara(0x0A, XHALWAVE);
} else {
RamWrite32A_LC898122AF(sysin, HALL_H_VAL); /* 0x11D5 */
SetSinWavePara(0x0A, YHALWAVE);
}
if (!UcDirSel) { /* AXIS X */
RegWriteA_LC898122AF(WC_MES1ADD0,
(unsigned char)AD0Z); /* 0x0194 */
RegWriteA_LC898122AF(
WC_MES1ADD1,
(unsigned char)((AD0Z >> 8) & 0x0001)); /* 0x0195 */
} else { /* AXIS Y */
RegWriteA_LC898122AF(WC_MES1ADD0,
(unsigned char)AD1Z); /* 0x0194 */
RegWriteA_LC898122AF(
WC_MES1ADD1,
(unsigned char)((AD1Z >> 8) & 0x0001)); /* 0x0195 */
}
RegWriteA_LC898122AF(WC_MESLOOP1,
0x00); /* 0x0193 CmMesLoop[15:8] */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x01); /* 0x0192 CmMesLoop[7:0] */
RamWrite32A_LC898122AF(msmean,
0x3F800000); /* 0x1230 1/CmMesLoop[15:0] */
RamWrite32A_LC898122AF(MSMAX1, 0x00000000); /* 0x1050 */
RamWrite32A_LC898122AF(MSMAX1AV, 0x00000000); /* 0x1051 */
RamWrite32A_LC898122AF(MSMIN1, 0x00000000); /* 0x1060 */
RamWrite32A_LC898122AF(MSMIN1AV, 0x00000000); /* 0x1061 */
RegWriteA_LC898122AF(WC_MESABS, 0x00); /* 0x0198 none ABS */
BsyWit(WC_MESMODE, 0x02); /* 0x0190 Sine wave Measure */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(MSMAX1AV, &StTneVal.StDwdVal.UsHigVal); /* 0x1051 */
RamReadA_LC898122AF(MSMIN1AV, &StTneVal.StDwdVal.UsLowVal); /* 0x1061 */
RamAccFixMod(OFF); /* Float mode */
if (!UcDirSel) { /* AXIS X */
SetSinWavePara(0x00, XHALWAVE); /* STOP */
} else {
SetSinWavePara(0x00, YHALWAVE); /* STOP */
}
if (UcBfrAft == 0) {
if (UcDirSel == X_DIR) {
StAdjPar.StHalAdj.UsHlxCen =
((signed short)StTneVal.StDwdVal.UsHigVal +
(signed short)StTneVal.StDwdVal.UsLowVal) /
2;
StAdjPar.StHalAdj.UsHlxMax = StTneVal.StDwdVal.UsHigVal;
StAdjPar.StHalAdj.UsHlxMin = StTneVal.StDwdVal.UsLowVal;
} else {
StAdjPar.StHalAdj.UsHlyCen =
((signed short)StTneVal.StDwdVal.UsHigVal +
(signed short)StTneVal.StDwdVal.UsLowVal) /
2;
StAdjPar.StHalAdj.UsHlyMax = StTneVal.StDwdVal.UsHigVal;
StAdjPar.StHalAdj.UsHlyMin = StTneVal.StDwdVal.UsLowVal;
}
} else {
if (UcDirSel == X_DIR) {
StAdjPar.StHalAdj.UsHlxCna =
((signed short)StTneVal.StDwdVal.UsHigVal +
(signed short)StTneVal.StDwdVal.UsLowVal) /
2;
StAdjPar.StHalAdj.UsHlxMxa = StTneVal.StDwdVal.UsHigVal;
StAdjPar.StHalAdj.UsHlxMna = StTneVal.StDwdVal.UsLowVal;
} else {
StAdjPar.StHalAdj.UsHlyCna =
((signed short)StTneVal.StDwdVal.UsHigVal +
(signed short)StTneVal.StDwdVal.UsLowVal) /
2;
StAdjPar.StHalAdj.UsHlyMxa = StTneVal.StDwdVal.UsHigVal;
StAdjPar.StHalAdj.UsHlyMna = StTneVal.StDwdVal.UsLowVal;
}
}
StTneVal.StDwdVal.UsHigVal = 0x7fff - StTneVal.StDwdVal.UsHigVal;
/* Maximum Gap = Maximum - Hall Peak Top */
StTneVal.StDwdVal.UsLowVal = StTneVal.StDwdVal.UsLowVal - 0x7fff;
/* Minimum Gap = Hall Peak Bottom - Minimum */
return StTneVal.UlDwdVal;
}
unsigned short UsValBef, UsValNow;
unsigned char TneCen(unsigned char UcTneAxs, union UnDwdVal StTneVal)
{
unsigned char UcTneRst, UcTmeOut, UcTofRst;
unsigned short UsOffDif;
unsigned short UsBiasVal;
UsErrBia = 0;
UsErrOfs = 0;
UcTmeOut = 1;
UsStpSiz = 1;
UcTneRst = FAILURE;
UcTofRst = FAILURE;
while (UcTneRst && UcTmeOut) {
if (UcTofRst == FAILURE) {
StTneVal.UlDwdVal = TneOff(StTneVal, UcTneAxs);
} else {
StTneVal.UlDwdVal = TneBia(StTneVal, UcTneAxs);
UcTofRst = FAILURE;
}
if (!(UcTneAxs & 0xF0)) {
if (StTneVal.StDwdVal.UsHigVal >
StTneVal.StDwdVal
.UsLowVal) {
UsOffDif = (StTneVal.StDwdVal.UsHigVal -
StTneVal.StDwdVal.UsLowVal) /
2;
} else {
UsOffDif = (StTneVal.StDwdVal.UsLowVal -
StTneVal.StDwdVal.UsHigVal) /
2;
}
if (UsOffDif < MARJIN)
UcTofRst = SUCCESS;
else
UcTofRst = FAILURE;
/* Check Tuning Result */
if ((StTneVal.StDwdVal.UsHigVal < HALL_MIN_GAP &&
StTneVal.StDwdVal.UsLowVal < HALL_MIN_GAP) &&
(StTneVal.StDwdVal.UsHigVal > HALL_MAX_GAP &&
StTneVal.StDwdVal.UsLowVal > HALL_MAX_GAP)) {
UcTneRst = SUCCESS;
break;
} else if (UsStpSiz == 0) {
UcTneRst = SUCCESS;
break;
}
UcTneRst = FAILURE;
UcTmeOut++;
} else {
if ((StTneVal.StDwdVal.UsHigVal > MARGIN) &&
(StTneVal.StDwdVal.UsLowVal > MARGIN)) {
UcTofRst = SUCCESS;
UsValBef = UsValNow = 0x0000;
} else if ((StTneVal.StDwdVal.UsHigVal <= MARGIN) &&
(StTneVal.StDwdVal.UsLowVal <= MARGIN)) {
UcTofRst = SUCCESS;
UcTneRst = FAILURE;
} else if (((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) >
BIAS_ADJ_OVER) {
UcTofRst = SUCCESS;
UcTneRst = FAILURE;
} else {
UcTofRst = FAILURE;
UsValBef = UsValNow;
RamAccFixMod(ON); /* Fix mode */
if (!(UcTneAxs & 0x0F))
RamReadA_LC898122AF(
DAXHLO, &UsValNow);
else
RamReadA_LC898122AF(
DAYHLO, &UsValNow);
if ((((UsValBef & 0xFF00) == 0x8000) &&
(UsValNow & 0xFF00) == 0x8000) ||
(((UsValBef & 0xFF00) == 0x7F00) &&
(UsValNow & 0xFF00) == 0x7F00)) {
if (!(UcTneAxs & 0x0F))
RamReadA_LC898122AF(DAXHLB,
&UsBiasVal);
else
RamReadA_LC898122AF(DAYHLB,
&UsBiasVal);
if (UsBiasVal > 0x8000)
UsBiasVal -= 0x8000;
else
UsBiasVal += 0x8000;
if (UsBiasVal > DECRE_CAL)
UsBiasVal -= DECRE_CAL;
UsBiasVal += 0x8000;
if (!(UcTneAxs & 0x0F))
RamWriteA_LC898122AF(DAXHLB,
UsBiasVal);
else
RamWriteA_LC898122AF(DAYHLB,
UsBiasVal);
}
RamAccFixMod(OFF); /* Float mode */
}
if ((((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) < HALL_MAX_RANGE) &&
(((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) > HALL_MIN_RANGE)) {
if (UcTofRst == SUCCESS) {
UcTneRst = SUCCESS;
break;
}
}
UcTneRst = FAILURE;
UcTmeOut++;
}
if (UcTneAxs & 0xF0) {
if ((UcTmeOut / 2) == TIME_OUT)
UcTmeOut = 0;
/* Set Time Out Count */
} else {
if (UcTmeOut == TIME_OUT)
UcTmeOut = 0;
/* Set Time Out Count */
}
}
if (UcTneRst == FAILURE) {
if (!(UcTneAxs & 0x0F)) {
UcTneRst = EXE_HXADJ;
StAdjPar.StHalAdj.UsHlxGan = 0xFFFF;
StAdjPar.StHalAdj.UsHlxOff = 0xFFFF;
} else {
UcTneRst = EXE_HYADJ;
StAdjPar.StHalAdj.UsHlyGan = 0xFFFF;
StAdjPar.StHalAdj.UsHlyOff = 0xFFFF;
}
} else {
UcTneRst = EXE_END;
}
return UcTneRst;
}
unsigned long TneBia(union UnDwdVal StTneVal, unsigned char UcTneAxs)
{
long SlSetBia;
unsigned short UsSetBia;
unsigned char UcChkFst;
static unsigned short UsTneVax; /* Variable For 1/2 Searching */
unsigned short UsDecCal;
UcChkFst = 1;
if (UsStpSiz == 1) {
UsTneVax = 2;
if (UcTneAxs & 0xF0) {
if (((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) > BIAS_ADJ_OVER) {
UcChkFst = 0;
RamAccFixMod(ON); /* Fix mode */
if (!(UcTneAxs &
0x0F)) {
RamReadA_LC898122AF(
DAXHLB, &UsSetBia);
} else {
RamReadA_LC898122AF(
DAYHLB, &UsSetBia);
}
if (UsSetBia > 0x8000)
UsSetBia -= 0x8000;
else
UsSetBia += 0x8000;
if (!UcChkFst)
UsDecCal = (DECRE_CAL << 3);
else
UsDecCal = DECRE_CAL;
if (UsSetBia > UsDecCal)
UsSetBia -= UsDecCal;
UsSetBia += 0x8000;
if (!(UcTneAxs &
0x0F)) {
RamWriteA_LC898122AF(
DAXHLB, UsSetBia);
RamWriteA_LC898122AF(
DAXHLO, 0x0000);
} else {
RamWriteA_LC898122AF(
DAYHLB, UsSetBia);
RamWriteA_LC898122AF(
DAYHLO, 0x0000);
}
UsStpSiz = BIAS_LIMIT / UsTneVax;
RamAccFixMod(OFF); /* Float mode */
}
} else {
if ((StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal) /
2 <
BIAS_ADJ_BORDER) {
UcChkFst = 0;
}
if (!UcTneAxs) {
RamWrite32A_LC898122AF(
DAXHLB, 0xBF800000);
RamWrite32A_LC898122AF(
DAXHLO, 0x00000000);
UsStpSiz = BIAS_LIMIT / UsTneVax;
} else {
RamWrite32A_LC898122AF(
DAYHLB, 0xBF800000);
RamWrite32A_LC898122AF(
DAYHLO,
0x00000000);
UsStpSiz = BIAS_LIMIT / UsTneVax;
}
}
}
RamAccFixMod(ON); /* Fix mode */
if (!(UcTneAxs & 0x0F)) {
RamReadA_LC898122AF(
DAXHLB, &UsSetBia);
SlSetBia = (long)UsSetBia;
} else {
RamReadA_LC898122AF(
DAYHLB, &UsSetBia);
SlSetBia = (long)UsSetBia;
}
if (SlSetBia >= 0x00008000)
SlSetBia |= 0xFFFF0000;
if (UcChkFst) {
if (UcTneAxs & 0xF0) {
/* Calculatiton For Hall BIAS 1/2 Searching */
if (((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) < BIAS_ADJ_RANGE) {
if (((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) <
BIAS_ADJ_SKIP) {
SlSetBia += 0x0400;
} else {
SlSetBia += 0x0100;
}
} else {
if (((unsigned short)0xFFFF -
(StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal)) >
BIAS_ADJ_OVER) {
SlSetBia -= 0x0400;
} else {
SlSetBia -= 0x0100;
}
}
UsStpSiz = 0x0200;
} else {
/* Calculatiton For Hall BIAS 1/2 Searching */
if ((StTneVal.StDwdVal.UsHigVal +
StTneVal.StDwdVal.UsLowVal) /
2 >
BIAS_ADJ_BORDER)
SlSetBia += UsStpSiz;
else
SlSetBia -= UsStpSiz;
UsTneVax = UsTneVax * 2;
UsStpSiz = BIAS_LIMIT / UsTneVax;
}
}
if (SlSetBia > 0x00007FFF)
SlSetBia = 0x00007FFF;
else if (SlSetBia < 0xFFFF8001)
SlSetBia = 0xFFFF8001;
if (!(UcTneAxs & 0x0F))
RamWriteA_LC898122AF(
DAXHLB,
SlSetBia);
else
RamWriteA_LC898122AF(
DAYHLB,
SlSetBia);
RamAccFixMod(OFF); /* Float mode */
StTneVal.UlDwdVal = TnePtp(UcTneAxs & 0x0F, PTP_AFTER);
return StTneVal.UlDwdVal;
}
unsigned long TneOff(union UnDwdVal StTneVal, unsigned char UcTneAxs)
{
long SlSetOff;
unsigned short UsSetOff;
UcTneAxs &= 0x0F;
RamAccFixMod(ON); /* Fix mode */
if (!UcTneAxs) {
RamReadA_LC898122AF(
DAXHLO,
&UsSetOff);
SlSetOff = (long)UsSetOff;
} else {
RamReadA_LC898122AF(
DAYHLO,
&UsSetOff);
SlSetOff = (long)UsSetOff;
}
if (SlSetOff > 0x00008000)
SlSetOff |= 0xFFFF0000;
if (StTneVal.StDwdVal.UsHigVal > StTneVal.StDwdVal.UsLowVal) {
SlSetOff += (StTneVal.StDwdVal.UsHigVal -
StTneVal.StDwdVal.UsLowVal) /
OFFSET_DIV;
/* Calculating Value For Increase Step */
} else {
SlSetOff -= (StTneVal.StDwdVal.UsLowVal -
StTneVal.StDwdVal.UsHigVal) /
OFFSET_DIV;
/* Calculating Value For Decrease Step */
}
if (SlSetOff > 0x00007FFF)
SlSetOff = 0x00007FFF;
else if (SlSetOff < 0xFFFF8001)
SlSetOff = 0xFFFF8001;
if (!UcTneAxs)
RamWriteA_LC898122AF(
DAXHLO,
SlSetOff);
else
RamWriteA_LC898122AF(
DAYHLO,
SlSetOff);
RamAccFixMod(OFF); /* Float mode */
StTneVal.UlDwdVal = TnePtp(UcTneAxs, PTP_AFTER);
return StTneVal.UlDwdVal;
}
#endif
void MesFil(unsigned char UcMesMod)
{
#ifdef USE_EXTCLK_ALL /* 24MHz */
if (!UcMesMod) { /* Hall Bias&Offset Adjust */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3D1E5A40); /* 0x10F0 LPF150Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3D1E5A40); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F6C34C0); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F800000); /* 0x10F5 Through */
RamWrite32A_LC898122AF(mes1bb, 0x00000000); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x00000000); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3D1E5A40); /* 0x11F0 LPF150Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3D1E5A40); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F6C34C0); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F800000); /* 0x11F5 Through */
RamWrite32A_LC898122AF(mes2bb, 0x00000000); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x00000000); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == LOOPGAIN) { /* Loop Gain Adjust */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3E587E00); /* 0x10F0 LPF1000Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3E587E00); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F13C100); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F7DF500); /* 0x10F5 HPF30Hz */
RamWrite32A_LC898122AF(mes1bb, 0xBF7DF500); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x3F7BEA40); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3E587E00); /* 0x11F0 LPF1000Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3E587E00); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F13C100); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F7DF500); /* 0x11F5 HPF30Hz */
RamWrite32A_LC898122AF(mes2bb, 0xBF7DF500); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x3F7BEA40); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == THROUGH) { /* for Through */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa, 0x3F800000); /* 0x10F0 Through */
RamWrite32A_LC898122AF(mes1ab, 0x00000000); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x00000000); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F800000); /* 0x10F5 Through */
RamWrite32A_LC898122AF(mes1bb, 0x00000000); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x00000000); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa, 0x3F800000); /* 0x11F0 Through */
RamWrite32A_LC898122AF(mes2ab, 0x00000000); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x00000000); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F800000); /* 0x11F5 Through */
RamWrite32A_LC898122AF(mes2bb, 0x00000000); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x00000000); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == NOISE) { /* SINE WAVE TEST for NOISE */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3D1E5A40); /* 0x10F0 LPF150Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3D1E5A40); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F6C34C0); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba,
0x3D1E5A40); /* 0x10F5 LPF150Hz */
RamWrite32A_LC898122AF(mes1bb, 0x3D1E5A40); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x3F6C34C0); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3D1E5A40); /* 0x11F0 LPF150Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3D1E5A40); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F6C34C0); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba,
0x3D1E5A40); /* 0x11F5 LPF150Hz */
RamWrite32A_LC898122AF(mes2bb, 0x3D1E5A40); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x3F6C34C0); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
}
#else
if (!UcMesMod) { /* Hall Bias&Offset Adjust */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3CA175C0); /* 0x10F0 LPF150Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3CA175C0); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F75E8C0); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F800000); /* 0x10F5 Through */
RamWrite32A_LC898122AF(mes1bb, 0x00000000); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x00000000); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3CA175C0); /* 0x11F0 LPF150Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3CA175C0); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F75E8C0); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F800000); /* 0x11F5 Through */
RamWrite32A_LC898122AF(mes2bb, 0x00000000); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x00000000); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == LOOPGAIN) { /* Loop Gain Adjust */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3DF21080); /* 0x10F0 LPF1000Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3DF21080); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F437BC0); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F7EF980); /* 0x10F5 HPF30Hz */
RamWrite32A_LC898122AF(mes1bb, 0xBF7EF980); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x3F7DF300); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3DF21080); /* 0x11F0 LPF1000Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3DF21080); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F437BC0); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F7EF980); /* 0x11F5 HPF30Hz */
RamWrite32A_LC898122AF(mes2bb, 0xBF7EF980); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x3F7DF300); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == THROUGH) { /* for Through */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa, 0x3F800000); /* 0x10F0 Through */
RamWrite32A_LC898122AF(mes1ab, 0x00000000); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x00000000); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba, 0x3F800000); /* 0x10F5 Through */
RamWrite32A_LC898122AF(mes1bb, 0x00000000); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x00000000); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa, 0x3F800000); /* 0x11F0 Through */
RamWrite32A_LC898122AF(mes2ab, 0x00000000); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x00000000); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba, 0x3F800000); /* 0x11F5 Through */
RamWrite32A_LC898122AF(mes2bb, 0x00000000); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x00000000); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
} else if (UcMesMod == NOISE) { /* SINE WAVE TEST for NOISE */
/* Measure Filter1 Setting */
RamWrite32A_LC898122AF(mes1aa,
0x3CA175C0); /* 0x10F0 LPF150Hz */
RamWrite32A_LC898122AF(mes1ab, 0x3CA175C0); /* 0x10F1 */
RamWrite32A_LC898122AF(mes1ac, 0x3F75E8C0); /* 0x10F2 */
RamWrite32A_LC898122AF(mes1ad, 0x00000000); /* 0x10F3 */
RamWrite32A_LC898122AF(mes1ae, 0x00000000); /* 0x10F4 */
RamWrite32A_LC898122AF(mes1ba,
0x3CA175C0); /* 0x10F5 LPF150Hz */
RamWrite32A_LC898122AF(mes1bb, 0x3CA175C0); /* 0x10F6 */
RamWrite32A_LC898122AF(mes1bc, 0x3F75E8C0); /* 0x10F7 */
RamWrite32A_LC898122AF(mes1bd, 0x00000000); /* 0x10F8 */
RamWrite32A_LC898122AF(mes1be, 0x00000000); /* 0x10F9 */
/* Measure Filter2 Setting */
RamWrite32A_LC898122AF(mes2aa,
0x3CA175C0); /* 0x11F0 LPF150Hz */
RamWrite32A_LC898122AF(mes2ab, 0x3CA175C0); /* 0x11F1 */
RamWrite32A_LC898122AF(mes2ac, 0x3F75E8C0); /* 0x11F2 */
RamWrite32A_LC898122AF(mes2ad, 0x00000000); /* 0x11F3 */
RamWrite32A_LC898122AF(mes2ae, 0x00000000); /* 0x11F4 */
RamWrite32A_LC898122AF(mes2ba,
0x3CA175C0); /* 0x11F5 LPF150Hz */
RamWrite32A_LC898122AF(mes2bb, 0x3CA175C0); /* 0x11F6 */
RamWrite32A_LC898122AF(mes2bc, 0x3F75E8C0); /* 0x11F7 */
RamWrite32A_LC898122AF(mes2bd, 0x00000000); /* 0x11F8 */
RamWrite32A_LC898122AF(mes2be, 0x00000000); /* 0x11F9 */
}
#endif
}
void SrvCon(unsigned char UcDirSel, unsigned char UcSwcCon)
{
if (UcSwcCon) {
if (!UcDirSel) { /* X Direction */
RegWriteA_LC898122AF(WH_EQSWX, 0x03); /* 0x0170 */
RamWrite32A_LC898122AF(sxggf, 0x00000000); /* 0x10B5 */
} else { /* Y Direction */
RegWriteA_LC898122AF(WH_EQSWY, 0x03); /* 0x0171 */
RamWrite32A_LC898122AF(syggf, 0x00000000); /* 0x11B5 */
}
} else {
if (!UcDirSel) { /* X Direction */
RegWriteA_LC898122AF(WH_EQSWX, 0x02); /* 0x0170 */
RamWrite32A_LC898122AF(SXLMT, 0x00000000); /* 0x1477 */
} else { /* Y Direction */
RegWriteA_LC898122AF(WH_EQSWY, 0x02); /* 0x0171 */
RamWrite32A_LC898122AF(SYLMT, 0x00000000); /* 0x14F7 */
}
}
}
#ifdef MODULE_CALIBRATION
unsigned char LopGan(unsigned char UcDirSel)
{
unsigned char UcLpAdjSts;
#ifdef HALLADJ_HW
UcLpAdjSts = LoopGainAdj(UcDirSel);
#else
MesFil(LOOPGAIN);
/* Servo ON */
SrvCon(X_DIR, ON);
SrvCon(Y_DIR, ON);
/* Wait 300ms */
WitTim_LC898122AF(300);
/* Loop Gain Adjust Initialize */
LopIni(UcDirSel);
/* Loop Gain Adjust */
UcLpAdjSts = LopAdj(UcDirSel);
#endif
/* Servo OFF */
SrvCon(X_DIR, OFF);
SrvCon(Y_DIR, OFF);
if (!UcLpAdjSts)
return EXE_END;
if (!UcDirSel)
return EXE_LXADJ;
else
return EXE_LYADJ;
}
#ifndef HALLADJ_HW
void LopIni(unsigned char UcDirSel)
{
/* Loop Gain Value Initialize */
LopPar(UcDirSel);
/* Sign Wave Output Setting */
LopSin(UcDirSel, ON);
}
#endif
void LopPar(unsigned char UcDirSel)
{
unsigned short UsLopGan;
RamAccFixMod(ON); /* Fix mode */
if (!UcDirSel) {
UsLopGan = SXGAIN_LOP;
RamWriteA_LC898122AF(sxg, UsLopGan); /* 0x10D3 */
} else {
UsLopGan = SYGAIN_LOP;
RamWriteA_LC898122AF(syg, UsLopGan); /* 0x11D3 */
}
RamAccFixMod(OFF); /* Float mode */
}
#ifndef HALLADJ_HW
void LopSin(unsigned char UcDirSel, unsigned char UcSonOff)
{
unsigned short UsFreqVal;
unsigned char UcEqSwX, UcEqSwY;
RegReadA_LC898122AF(WH_EQSWX, &UcEqSwX); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcEqSwY); /* 0x0171 */
if (UcSonOff) {
#ifdef USE_EXTCLK_ALL /* 24MHz */
/* Freq = CmSinFrq * 11.718kHz / 65536 / 16 */
#ifdef ACTREG_6P5OHM
/* UsFreqVal = 0x30EE ; */ /* 139.9Hz */
UsFreqVal = 0x29F1; /* 119.9Hz */
#endif
#ifdef ACTREG_10P2OHM
UsFreqVal = 0x29F1; /* 119.9Hz */
#endif
#ifdef ACTREG_15OHM
UsFreqVal = 0x3B6B; /* 169.9Hz */
#endif
#else
/* Freq = CmSinFrq * 23.4375kHz / 65536 / 16 */
#ifdef ACTREG_6P5OHM
/* UsFreqVal = 0x1877 ; */ /* 139.9Hz */
UsFreqVal = 0x14F8; /* 119.9Hz */
#endif
#ifdef ACTREG_10P2OHM
UsFreqVal = 0x14F8; /* 119.9Hz */
#endif
#ifdef ACTREG_15OHM
UsFreqVal = 0x1DB5; /* 169.9Hz */
#endif
#endif
RegWriteA_LC898122AF(WC_SINFRQ0,
(unsigned char)
UsFreqVal); /* 0x0181 Freq L */
RegWriteA_LC898122AF(WC_SINFRQ1,
(unsigned char)(UsFreqVal >> 8));
if (!UcDirSel) {
UcEqSwX |= 0x10;
UcEqSwY &= ~EQSINSW;
RamWrite32A_LC898122AF(sxsin,
0x3CA3D70A); /* 0x10D5 -34dB */
} else {
UcEqSwX &= ~EQSINSW;
UcEqSwY |= 0x10;
RamWrite32A_LC898122AF(sysin,
0x3CA3D70A); /* 0x11D5 -34dB */
}
RegWriteA_LC898122AF(WC_SINPHSX,
0x00); /* 0x0183 X Sine phase */
RegWriteA_LC898122AF(WC_SINPHSY,
0x00); /* 0x0184 Y Sine phase */
RegWriteA_LC898122AF(WH_EQSWX,
UcEqSwX); /* 0x0170 Switch control */
RegWriteA_LC898122AF(WH_EQSWY,
UcEqSwY); /* 0x0171 Switch control */
RegWriteA_LC898122AF(WC_SINON,
0x01); /* 0x0180 Sine wave */
} else {
UcEqSwX &= ~EQSINSW;
UcEqSwY &= ~EQSINSW;
RegWriteA_LC898122AF(WC_SINON,
0x00); /* 0x0180 Sine wave */
if (!UcDirSel)
RamWrite32A_LC898122AF(sxsin, 0x00000000); /* 0x10D5 */
else
RamWrite32A_LC898122AF(sysin, 0x00000000); /* 0x11D5 */
RegWriteA_LC898122AF(WH_EQSWX,
UcEqSwX); /* 0x0170 Switch control */
RegWriteA_LC898122AF(WH_EQSWY,
UcEqSwY); /* 0x0171 Switch control */
}
}
unsigned char LopAdj(unsigned char UcDirSel)
{
unsigned char UcAdjSts = FAILURE;
unsigned short UsRtnVal;
float SfCmpVal;
unsigned char UcIdxCnt;
unsigned char UcIdxCn1;
unsigned char UcIdxCn2;
union UnFltVal UnAdcXg1, UnAdcXg2, UnRtnVa;
float DfGanVal[5];
float DfTemVal;
if (!UcDirSel) {
RegWriteA_LC898122AF(WC_MES1ADD0,
(unsigned char)SXGZ); /* 0x0194 */
RegWriteA_LC898122AF(
WC_MES1ADD1,
(unsigned char)((SXGZ >> 8) & 0x0001)); /* 0x0195 */
RegWriteA_LC898122AF(WC_MES2ADD0,
(unsigned char)SXG3Z); /* 0x0196 */
RegWriteA_LC898122AF(
WC_MES2ADD1,
(unsigned char)((SXG3Z >> 8) & 0x0001)); /* 0x0197 */
} else {
RegWriteA_LC898122AF(WC_MES1ADD0,
(unsigned char)SYGZ); /* 0x0194 */
RegWriteA_LC898122AF(
WC_MES1ADD1,
(unsigned char)((SYGZ >> 8) & 0x0001)); /* 0x0195 */
RegWriteA_LC898122AF(WC_MES2ADD0,
(unsigned char)SYG3Z); /* 0x0196 */
RegWriteA_LC898122AF(
WC_MES2ADD1,
(unsigned char)((SYG3Z >> 8) & 0x0001)); /* 0x0197 */
}
/* 5 Times Average Value Calculation */
for (UcIdxCnt = 0; UcIdxCnt < 5; UcIdxCnt++) {
LopMes(); /* Loop Gain Mesurement Start */
UnAdcXg1.UlLngVal = GinMes(MES_XG1); /* LXG1 Measure */
UnAdcXg2.UlLngVal = GinMes(MES_XG2); /* LXG2 Measure */
SfCmpVal = UnAdcXg2.SfFltVal /
UnAdcXg1.SfFltVal; /* Compare Coefficient Value */
if (!UcDirSel)
RamRead32A_LC898122AF(sxg,
&UnRtnVa.UlLngVal); /* 0x10D3 */
else
RamRead32A_LC898122AF(syg,
&UnRtnVa.UlLngVal); /* 0x11D3 */
UnRtnVa.SfFltVal = UnRtnVa.SfFltVal * SfCmpVal;
DfGanVal[UcIdxCnt] = UnRtnVa.SfFltVal;
}
for (UcIdxCn1 = 0; UcIdxCn1 < 4; UcIdxCn1++) {
for (UcIdxCn2 = UcIdxCn1 + 1; UcIdxCn2 < 5; UcIdxCn2++) {
if (DfGanVal[UcIdxCn1] > DfGanVal[UcIdxCn2]) {
DfTemVal = DfGanVal[UcIdxCn1];
DfGanVal[UcIdxCn1] = DfGanVal[UcIdxCn2];
DfGanVal[UcIdxCn2] = DfTemVal;
}
}
}
UnRtnVa.SfFltVal = (DfGanVal[1] + DfGanVal[2] + DfGanVal[3]) / 3;
LopSin(UcDirSel, OFF);
if (UnRtnVa.UlLngVal < 0x3F800000) /* Adjust Error */
UcAdjSts = SUCCESS; /* Status OK */
if (UcAdjSts) {
if (!UcDirSel) {
RamWrite32A_LC898122AF(sxg, 0x3F800000); /* 0x10D3 */
StAdjPar.StLopGan.UsLxgVal = 0x7FFF;
StAdjPar.StLopGan.UsLxgSts = 0x0000;
} else {
RamWrite32A_LC898122AF(syg, 0x3F800000); /* 0x11D3 */
StAdjPar.StLopGan.UsLygVal = 0x7FFF;
StAdjPar.StLopGan.UsLygSts = 0x0000;
}
} else {
if (!UcDirSel) {
RamWrite32A_LC898122AF(sxg,
UnRtnVa.UlLngVal); /* 0x10D3 */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(sxg, &UsRtnVal); /* 0x10D3 */
StAdjPar.StLopGan.UsLxgVal = UsRtnVal;
StAdjPar.StLopGan.UsLxgSts = 0xFFFF;
} else {
RamWrite32A_LC898122AF(syg,
UnRtnVa.UlLngVal); /* 0x11D3 */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(syg, &UsRtnVal); /* 0x11D3 */
StAdjPar.StLopGan.UsLygVal = UsRtnVal;
StAdjPar.StLopGan.UsLygSts = 0xFFFF;
}
RamAccFixMod(OFF); /* Float mode */
}
return UcAdjSts;
}
void LopMes(void)
{
ClrGyr(0x1000, CLR_FRAM1); /* Measure Filter RAM Clear */
RamWrite32A_LC898122AF(MSABS1AV, 0x00000000); /* 0x1041 Clear */
RamWrite32A_LC898122AF(MSABS2AV, 0x00000000); /* 0x1141 Clear */
RegWriteA_LC898122AF(WC_MESLOOP1, 0x04); /* 0x0193 */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x00); /* 0x0192 1024 Times Measure */
RamWrite32A_LC898122AF(msmean,
0x3A800000); /* 0x1230 1/CmMesLoop[15:0] */
RegWriteA_LC898122AF(WC_MESABS, 0x01); /* 0x0198 ABS */
RegWriteA_LC898122AF(WC_MESWAIT, 0x00); /* 0x0199 0 cross wait */
BsyWit(WC_MESMODE, 0x01); /* 0x0190 Sin Wave Measure */
}
unsigned long GinMes(unsigned char UcXg1Xg2)
{
unsigned long UlMesVal;
if (!UcXg1Xg2)
RamRead32A_LC898122AF(MSABS1AV, &UlMesVal); /* 0x1041 */
else
RamRead32A_LC898122AF(MSABS2AV, &UlMesVal); /* 0x1141 */
return UlMesVal;
}
#endif
#endif
#define LIMITH 0x0FA0
#define LIMITL 0xF060
#define INITVAL 0x0000
unsigned char TneGvc(void)
{
unsigned char UcRsltSts;
/* A/D Offset Clear */
RegWriteA_LC898122AF(
IZAH, (unsigned char)(INITVAL >> 8));
RegWriteA_LC898122AF(
IZAL,
(unsigned char)
INITVAL);
RegWriteA_LC898122AF(
IZBH, (unsigned char)(INITVAL >> 8));
RegWriteA_LC898122AF(
IZBL,
(unsigned char)
INITVAL);
MesFil(THROUGH);
/* //////// */
/* X */
/* //////// */
RegWriteA_LC898122AF(WC_MES1ADD0, 0x00); /* 0x0194 */
RegWriteA_LC898122AF(WC_MES1ADD1, 0x00); /* 0x0195 */
ClrGyr(0x1000, CLR_FRAM1);
StAdjPar.StGvcOff.UsGxoVal = (unsigned short)GenMes(AD2Z, 0);
RegWriteA_LC898122AF(IZAH,
(unsigned char)(StAdjPar.StGvcOff.UsGxoVal >> 8));
RegWriteA_LC898122AF(IZAL, (unsigned char)(StAdjPar.StGvcOff.UsGxoVal));
/* //////// */
/* Y */
/* //////// */
RegWriteA_LC898122AF(WC_MES1ADD0, 0x00); /* 0x0194 */
RegWriteA_LC898122AF(WC_MES1ADD1, 0x00); /* 0x0195 */
ClrGyr(0x1000, CLR_FRAM1); /* Measure Filter RAM Clear */
StAdjPar.StGvcOff.UsGyoVal = (unsigned short)GenMes(AD3Z, 0);
/* 64<36><34><EFBFBD>̕<EFBFBD><CC95>ϒl<CF92><6C><EFBFBD><EFBFBD> GYRMON2(0x1111) <- GYADZ(0x14CA) */
RegWriteA_LC898122AF(IZBH,
(unsigned char)(StAdjPar.StGvcOff.UsGyoVal >> 8));
/* 0x02A2 Set Offset High byte */
RegWriteA_LC898122AF(IZBL, (unsigned char)(StAdjPar.StGvcOff.UsGyoVal));
/* 0x02A3 Set Offset Low byte */
UcRsltSts = EXE_END; /* Clear Status */
StAdjPar.StGvcOff.UsGxoSts = 0xFFFF;
if (((short)StAdjPar.StGvcOff.UsGxoVal < (short)LIMITL) ||
((short)StAdjPar.StGvcOff.UsGxoVal > (short)LIMITH)) {
UcRsltSts |= EXE_GXADJ;
StAdjPar.StGvcOff.UsGxoSts = 0x0000;
}
StAdjPar.StGvcOff.UsGyoSts = 0xFFFF;
if (((short)StAdjPar.StGvcOff.UsGyoVal < (short)LIMITL) ||
((short)StAdjPar.StGvcOff.UsGyoVal > (short)LIMITH)) {
UcRsltSts |= EXE_GYADJ;
StAdjPar.StGvcOff.UsGyoSts = 0x0000;
}
return UcRsltSts;
}
unsigned char RtnCen(unsigned char UcCmdPar)
{
unsigned char UcCmdSts;
UcCmdSts = EXE_END;
GyrCon(OFF); /* Gyro OFF */
if (!UcCmdPar) { /* X,Y Centering */
StbOnn(); /* Slope Mode */
} else if (UcCmdPar == 0x01) { /* X Centering Only */
SrvCon(X_DIR, ON); /* X only Servo ON */
SrvCon(Y_DIR, OFF);
} else if (UcCmdPar == 0x02) { /* Y Centering Only */
SrvCon(X_DIR, OFF); /* Y only Servo ON */
SrvCon(Y_DIR, ON);
}
return UcCmdSts;
}
void GyrCon(unsigned char UcGyrCon)
{
/* Return HPF Setting */
RegWriteA_LC898122AF(WG_SHTON, 0x00); /* 0x0107 */
if (UcGyrCon == ON) { /* Gyro ON */
#ifdef GAIN_CONT
/* Gain3 Register */
/* AutoGainControlSw( ON ) ; */ /* Auto Gain Control Mode ON */
#endif
ClrGyr(0x000E, CLR_FRAM1); /* Gyro Delay RAM Clear */
RamWrite32A_LC898122AF(sxggf, 0x3F800000); /* 0x10B5 */
RamWrite32A_LC898122AF(syggf, 0x3F800000); /* 0x11B5 */
} else if (UcGyrCon == SPC) { /* Gyro ON for LINE */
#ifdef GAIN_CONT
/* Gain3 Register */
/* AutoGainControlSw( ON ) ; */ /* Auto Gain Control Mode ON */
#endif
RamWrite32A_LC898122AF(sxggf, 0x3F800000); /* 0x10B5 */
RamWrite32A_LC898122AF(syggf, 0x3F800000); /* 0x11B5 */
} else { /* Gyro OFF */
RamWrite32A_LC898122AF(sxggf, 0x00000000); /* 0x10B5 */
RamWrite32A_LC898122AF(syggf, 0x00000000); /* 0x11B5 */
#ifdef GAIN_CONT
/* Gain3 Register */
/* AutoGainControlSw( OFF ); */
#endif
}
}
void OisEna(void)
{
/* Servo ON */
SrvCon(X_DIR, ON);
SrvCon(Y_DIR, ON);
GyrCon(ON);
}
void OisEnaLin(void)
{
/* Servo ON */
SrvCon(X_DIR, ON);
SrvCon(Y_DIR, ON);
GyrCon(SPC);
}
void TimPro(void)
{
#ifdef MODULE_CALIBRATION
if (UcOscAdjFlg) {
if (UcOscAdjFlg == MEASSTR) {
RegWriteA_LC898122AF(
OSCCNTEN,
0x01); /* 0x0258 OSC Cnt enable */
UcOscAdjFlg = MEASCNT;
} else if (UcOscAdjFlg == MEASCNT) {
RegWriteA_LC898122AF(
OSCCNTEN,
0x00); /* 0x0258 OSC Cnt disable */
UcOscAdjFlg = MEASFIX;
}
}
#endif
}
void S2cPro(unsigned char uc_mode)
{
if (uc_mode == 1) {
#ifdef H1COEF_CHANGER
SetH1cMod(S2MODE); /* cancel Lvl change */
#endif
/* HPF<50><46>Through Setting */
RegWriteA_LC898122AF(WG_SHTON, 0x11); /* 0x0107 */
RamWrite32A_LC898122AF(gxh1c, DIFIL_S2); /* 0x1012 */
RamWrite32A_LC898122AF(gyh1c, DIFIL_S2); /* 0x1112 */
} else {
RamWrite32A_LC898122AF(gxh1c, UlH1Coefval); /* 0x1012 */
RamWrite32A_LC898122AF(gyh1c, UlH1Coefval); /* 0x1112 */
/* HPF<50><46>Through Setting */
RegWriteA_LC898122AF(WG_SHTON, 0x00); /* 0x0107 */
#ifdef H1COEF_CHANGER
SetH1cMod(UcH1LvlMod); /* Re-setting */
#endif
}
}
short GenMes(unsigned short UsRamAdd, unsigned char UcMesMod)
{
short SsMesRlt;
RegWriteA_LC898122AF(WC_MES1ADD0, (unsigned char)UsRamAdd); /* 0x0194 */
RegWriteA_LC898122AF(WC_MES1ADD1, (unsigned char)((UsRamAdd >> 8) &
0x0001)); /* 0x0195 */
RamWrite32A_LC898122AF(MSABS1AV, 0x00000000); /* 0x1041 Clear */
if (!UcMesMod) {
RegWriteA_LC898122AF(WC_MESLOOP1, 0x04); /* 0x0193 */
RegWriteA_LC898122AF(
WC_MESLOOP0,
0x00); /* 0x0192 1024 Times Measure */
RamWrite32A_LC898122AF(
msmean,
0x3A7FFFF7); /* 0x1230 1/CmMesLoop[15:0] */
} else {
RegWriteA_LC898122AF(WC_MESLOOP1, 0x01); /* 0x0193 */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x00); /* 0x0192 1 Times Measure */
RamWrite32A_LC898122AF(
msmean,
0x3F800000); /* 0x1230 1/CmMesLoop[15:0] */
}
RegWriteA_LC898122AF(WC_MESABS, 0x00); /* 0x0198 none ABS */
BsyWit(WC_MESMODE, 0x01); /* 0x0190 normal Measure */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(MSABS1AV, (unsigned short *)&SsMesRlt); /* 0x1041 */
RamAccFixMod(OFF); /* Float mode */
return SsMesRlt;
}
#ifdef USE_EXTCLK_ALL /* 24MHz */
/********* Parameter Setting *********/
/* Servo Sampling Clock = 11.71875kHz */
/* Freq = CmSinFreq*Fs/65536/16 */
/* 05 00 XX MM XX:Freq MM:Sin or Circle */
const unsigned short CucFreqVal[17] = {
0xFFFF, /* 0: Stop */
0x0059, /* 1: 0.994653Hz */
0x00B2, /* 2: 1.989305Hz */
0x010C, /* 3: 2.995133Hz */
0x0165, /* 4: 3.989786Hz */
0x01BF, /* 5: 4.995614Hz */
0x0218, /* 6: 5.990267Hz */
0x0272, /* 7: 6.996095Hz */
0x02CB, /* 8: 7.990748Hz */
0x0325, /* 9: 8.996576Hz */
0x037E, /* A: 9.991229Hz */
0x03D8, /* B: 10.99706Hz */
0x0431, /* C: 11.99171Hz */
0x048B, /* D: 12.99754Hz */
0x04E4, /* E: 13.99219Hz */
0x053E, /* F: 14.99802Hz */
0x0597 /* 10: 15.99267Hz */
};
#else
/********* Parameter Setting *********/
/* Servo Sampling Clock = 23.4375kHz */
/* Freq = CmSinFreq*Fs/65536/16 */
/* 05 00 XX MM XX:Freq MM:Sin or Circle */
const unsigned short CucFreqVal[17] = {
0xFFFF, /* 0: Stop */
0x002C, /* 1: 0.983477Hz */
0x0059, /* 2: 1.989305Hz */
0x0086, /* 3: 2.995133Hz */
0x00B2, /* 4: 3.97861Hz */
0x00DF, /* 5: 4.984438Hz */
0x010C, /* 6: 5.990267Hz */
0x0139, /* 7: 6.996095Hz */
0x0165, /* 8: 7.979572Hz */
0x0192, /* 9: 8.9854Hz */
0x01BF, /* A: 9.991229Hz */
0x01EC, /* B: 10.99706Hz */
0x0218, /* C: 11.98053Hz */
0x0245, /* D: 12.98636Hz */
0x0272, /* E: 13.99219Hz */
0x029F, /* F: 14.99802Hz */
0x02CB /* 10: 15.9815Hz */
};
#endif
#define USE_SINLPF
void SetSinWavePara(unsigned char UcTableVal, unsigned char UcMethodVal)
{
unsigned short UsFreqDat;
unsigned char UcEqSwX, UcEqSwY;
if (UcTableVal > 0x10)
UcTableVal = 0x10; /* Limit */
UsFreqDat = CucFreqVal[UcTableVal];
if (UcMethodVal == SINEWAVE) {
RegWriteA_LC898122AF(WC_SINPHSX, 0x00); /* 0x0183 */
RegWriteA_LC898122AF(WC_SINPHSY, 0x00); /* 0x0184 */
} else if (UcMethodVal == CIRCWAVE) {
RegWriteA_LC898122AF(WC_SINPHSX, 0x00); /* 0x0183 */
RegWriteA_LC898122AF(WC_SINPHSY, 0x20); /* 0x0184 */
} else {
RegWriteA_LC898122AF(WC_SINPHSX, 0x00); /* 0x0183 */
RegWriteA_LC898122AF(WC_SINPHSY, 0x00); /* 0x0184 */
}
#ifdef USE_SINLPF
if ((UcMethodVal != XHALWAVE) && (UcMethodVal != YHALWAVE))
MesFil(NOISE); /* LPF */
#endif
if (UsFreqDat == 0xFFFF) { /* Sine<6E>g?<3F>~ */
RegReadA_LC898122AF(WH_EQSWX, &UcEqSwX); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcEqSwY); /* 0x0171 */
UcEqSwX &= ~EQSINSW;
UcEqSwY &= ~EQSINSW;
RegWriteA_LC898122AF(WH_EQSWX, UcEqSwX); /* 0x0170 */
RegWriteA_LC898122AF(WH_EQSWY, UcEqSwY); /* 0x0171 */
#ifdef USE_SINLPF
if ((UcMethodVal != XHALWAVE) && (UcMethodVal != YHALWAVE)) {
RegWriteA_LC898122AF(
WC_DPON, 0x00);
RegWriteA_LC898122AF(
WC_DPO1ADD0,
0x00); /* 0x01B8 output initial */
RegWriteA_LC898122AF(
WC_DPO1ADD1,
0x00); /* 0x01B9 output initial */
RegWriteA_LC898122AF(
WC_DPO2ADD0,
0x00); /* 0x01BA output initial */
RegWriteA_LC898122AF(
WC_DPO2ADD1,
0x00); /* 0x01BB output initial */
RegWriteA_LC898122AF(
WC_DPI1ADD0,
0x00); /* 0x01B0 input initial */
RegWriteA_LC898122AF(
WC_DPI1ADD1,
0x00); /* 0x01B1 input initial */
RegWriteA_LC898122AF(
WC_DPI2ADD0,
0x00); /* 0x01B2 input initial */
RegWriteA_LC898122AF(
WC_DPI2ADD1,
0x00); /* 0x01B3 input initial */
/* Ram Access */
RamAccFixMod(ON); /* Fix mode */
RamWriteA_LC898122AF(
SXOFFZ1,
UsCntXof); /* 0x1461 set optical value */
RamWriteA_LC898122AF(
SYOFFZ1,
UsCntYof); /* 0x14E1 set optical value */
/* Ram Access */
RamAccFixMod(OFF); /* Float mode */
RegWriteA_LC898122AF(WC_MES1ADD0, 0x00); /* 0x0194 */
RegWriteA_LC898122AF(WC_MES1ADD1, 0x00); /* 0x0195 */
RegWriteA_LC898122AF(WC_MES2ADD0, 0x00); /* 0x0196 */
RegWriteA_LC898122AF(WC_MES2ADD1, 0x00); /* 0x0197 */
}
#endif
RegWriteA_LC898122AF(WC_SINON,
0x00); /* 0x0180 Sine wave */
} else {
RegReadA_LC898122AF(WH_EQSWX, &UcEqSwX); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcEqSwY); /* 0x0171 */
if ((UcMethodVal != XHALWAVE) && (UcMethodVal != YHALWAVE)) {
#ifdef USE_SINLPF
RegWriteA_LC898122AF(WC_DPI1ADD0,
(unsigned char)MES1BZ2);
/* 0x01B0 input Meas-Fil */
RegWriteA_LC898122AF(
WC_DPI1ADD1,
(unsigned char)((MES1BZ2 >> 8) & 0x0001));
/* 0x01B1 input Meas-Fil */
RegWriteA_LC898122AF(WC_DPI2ADD0,
(unsigned char)MES2BZ2);
/* 0x01B2 input Meas-Fil */
RegWriteA_LC898122AF(
WC_DPI2ADD1,
(unsigned char)((MES2BZ2 >> 8) & 0x0001));
/* 0x01B3 input Meas-Fil */
RegWriteA_LC898122AF(WC_DPO1ADD0,
(unsigned char)SXOFFZ1);
/* 0x01B8 output SXOFFZ1 */
RegWriteA_LC898122AF(
WC_DPO1ADD1,
(unsigned char)((SXOFFZ1 >> 8) & 0x0001));
/* 0x01B9 output SXOFFZ1 */
RegWriteA_LC898122AF(WC_DPO2ADD0,
(unsigned char)SYOFFZ1);
/* 0x01BA output SYOFFZ1 */
RegWriteA_LC898122AF(
WC_DPO2ADD1,
(unsigned char)((SYOFFZ1 >> 8) & 0x0001));
/* 0x01BA output SYOFFZ1 */
RegWriteA_LC898122AF(WC_MES1ADD0,
(unsigned char)SINXZ); /* 0x0194 */
RegWriteA_LC898122AF(
WC_MES1ADD1,
(unsigned char)((SINXZ >> 8) & 0x0001));
RegWriteA_LC898122AF(WC_MES2ADD0,
(unsigned char)SINYZ); /* 0x0196 */
RegWriteA_LC898122AF(
WC_MES2ADD1,
(unsigned char)((SINYZ >> 8) & 0x0001));
RegWriteA_LC898122AF(
WC_DPON,
0x03); /* 0x0105 Data pass[1:0] on */
UcEqSwX &= ~EQSINSW;
UcEqSwY &= ~EQSINSW;
#else
UcEqSwX |= 0x08;
UcEqSwY |= 0x08;
#endif
} else {
if (UcMethodVal == XHALWAVE) {
UcEqSwX = 0x22; /* SW[5] */
/* UcEqSwY = 0x03 ; */
} else {
/* UcEqSwX = 0x03 ; */
UcEqSwY = 0x22; /* SW[5] */
}
}
RegWriteA_LC898122AF(WC_SINFRQ0, (unsigned char)UsFreqDat);
RegWriteA_LC898122AF(WC_SINFRQ1,
(unsigned char)(UsFreqDat >> 8));
RegWriteA_LC898122AF(WC_MESSINMODE,
0x00); /* 0x0191 Sine 0 cross */
RegWriteA_LC898122AF(WH_EQSWX, UcEqSwX); /* 0x0170 */
RegWriteA_LC898122AF(WH_EQSWY, UcEqSwY); /* 0x0171 */
RegWriteA_LC898122AF(WC_SINON,
0x01); /* 0x0180 Sine wave */
}
}
#ifdef STANDBY_MODE
void SetStandby(unsigned char UcContMode)
{
unsigned char UcStbb0, UcClkon;
switch (UcContMode) {
case STB1_ON:
#ifdef AF_PWMMODE
#else
RegWriteA_LC898122AF(DRVFCAF, 0x00);
#endif
RegWriteA_LC898122AF(STBB0, 0x00);
RegWriteA_LC898122AF(STBB1, 0x00);
RegWriteA_LC898122AF(
PWMA, 0x00); /* 0x0010 PWM Standby */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(
CVA, 0x00); /* 0x0020 LINEAR PWM mode standby */
DrvSw(OFF); /* Driver OFF */
AfDrvSw(OFF); /* AF Driver OFF */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA,
0x00); /* 0x0030 Monitor Standby */
#endif
SelectGySleep(ON); /* Gyro Sleep */
break;
case STB1_OFF:
SelectGySleep(OFF); /* Gyro Wake Up */
RegWriteA_LC898122AF(PWMMONA,
0x80); /* 0x0030 Monitor Active */
DrvSw(ON); /* Driver Mode setting */
AfDrvSw(ON); /* AF Driver Mode setting */
RegWriteA_LC898122AF(
CVA, 0xC0); /* 0x0020 Linear PWM mode enable */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(PWMA, 0xC0); /* 0x0010 PWM enable */
RegWriteA_LC898122AF(STBB1, 0x05);
RegWriteA_LC898122AF(STBB0, 0xDF);
break;
case STB2_ON:
#ifdef AF_PWMMODE
#else
RegWriteA_LC898122AF(DRVFCAF, 0x00);
#endif
RegWriteA_LC898122AF(STBB0, 0x00);
RegWriteA_LC898122AF(STBB1, 0x00);
RegWriteA_LC898122AF(
PWMA, 0x00); /* 0x0010 PWM Standby */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(
CVA, 0x00); /* 0x0020 LINEAR PWM mode standby */
DrvSw(OFF); /* Drvier Block Ena=0 */
AfDrvSw(OFF); /* AF Drvier Block Ena=0 */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA,
0x00); /* 0x0030 Monitor Standby */
#endif
SelectGySleep(ON); /* Gyro Sleep */
RegWriteA_LC898122AF(CLKON,
0x00);
break;
case STB2_OFF:
RegWriteA_LC898122AF(CLKON, 0x1F);
SelectGySleep(OFF); /* Gyro Wake Up */
RegWriteA_LC898122AF(PWMMONA,
0x80); /* 0x0030 Monitor Active */
DrvSw(ON); /* Driver Mode setting */
AfDrvSw(ON); /* AF Driver Mode setting */
RegWriteA_LC898122AF(
CVA, 0xC0); /* 0x0020 Linear PWM mode enable */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(PWMA, 0xC0); /* 0x0010 PWM enable */
RegWriteA_LC898122AF(STBB1, 0x05);
RegWriteA_LC898122AF(STBB0, 0xDF);
break;
case STB3_ON:
#ifdef AF_PWMMODE
#else
RegWriteA_LC898122AF(DRVFCAF, 0x00);
/* 0x0081 Drv.MODEAF=0,Drv.ENAAF=0,MODE-0 */
#endif
RegWriteA_LC898122AF(STBB0, 0x00);
RegWriteA_LC898122AF(STBB1, 0x00);
RegWriteA_LC898122AF(
PWMA, 0x00); /* 0x0010 PWM Standby */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(
CVA, 0x00); /* 0x0020 LINEAR PWM mode standby */
DrvSw(OFF); /* Drvier Block Ena=0 */
AfDrvSw(OFF); /* AF Drvier Block Ena=0 */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA,
0x00); /* 0x0030 Monitor Standby */
#endif
SelectGySleep(ON); /* Gyro Sleep */
RegWriteA_LC898122AF(CLKON,
0x00);
RegWriteA_LC898122AF(
I2CSEL,
0x01); /* 0x0248 I2C Noise Cancel circuit OFF */
RegWriteA_LC898122AF(
OSCSTOP,
0x02); /* 0x0256 Source Clock Input OFF */
break;
case STB3_OFF:
RegWriteA_LC898122AF(
OSCSTOP, 0x00); /* 0x0256 Source Clock Input ON */
RegWriteA_LC898122AF(
I2CSEL,
0x00); /* 0x0248 I2C Noise Cancel circuit ON */
RegWriteA_LC898122AF(CLKON, 0x1F);
SelectGySleep(OFF); /* Gyro Wake Up */
RegWriteA_LC898122AF(PWMMONA, 0x80);
/* 0x0030 Monitor Active */
DrvSw(ON); /* Driver Mode setting */
AfDrvSw(ON); /* AF Driver Mode setting */
RegWriteA_LC898122AF(CVA, 0xC0);
/* 0x0020 Linear PWM mode enable */
RegWriteA_LC898122AF(PWMAAF, 0x00);
/* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(PWMA, 0xC0);
/* 0x0010 PWM enable */
RegWriteA_LC898122AF(STBB1, 0x05);
RegWriteA_LC898122AF(STBB0, 0xDF);
break;
case STB4_ON:
#ifdef AF_PWMMODE
#else
RegWriteA_LC898122AF(DRVFCAF, 0x00);
#endif
RegWriteA_LC898122AF(STBB0, 0x00);
RegWriteA_LC898122AF(STBB1, 0x00);
RegWriteA_LC898122AF(
PWMA, 0x00); /* 0x0010 PWM Standby */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(
CVA, 0x00); /* 0x0020 LINEAR PWM mode standby */
DrvSw(OFF); /* Drvier Block Ena=0 */
AfDrvSw(OFF); /* AF Drvier Block Ena=0 */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA,
0x00); /* 0x0030 Monitor Standby */
#endif
GyOutSignalCont(); /* Gyro Continuos mode */
RegWriteA_LC898122AF(CLKON, 0x04);
/* 0x020B Servo & PWM Clock OFF + D-Gyro I/F ON */
break;
case STB4_OFF:
RegWriteA_LC898122AF(CLKON, 0x1F);
SelectGySleep(OFF); /* Gyro OIS mode */
RegWriteA_LC898122AF(PWMMONA,
0x80); /* 0x0030 Monitor Active */
DrvSw(ON); /* Driver Mode setting */
AfDrvSw(ON); /* AF Driver Mode setting */
RegWriteA_LC898122AF(
CVA, 0xC0); /* 0x0020 Linear PWM mode enable */
RegWriteA_LC898122AF(
PWMAAF, 0x00); /* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(PWMA, 0xC0); /* 0x0010 PWM enable */
RegWriteA_LC898122AF(STBB1, 0x05);
RegWriteA_LC898122AF(STBB0, 0xDF);
break;
/************** special mode ************/
case STB2_OISON:
RegReadA_LC898122AF(STBB0, &UcStbb0);
UcStbb0 &= 0x80;
RegWriteA_LC898122AF(STBB0, UcStbb0);
RegWriteA_LC898122AF(
PWMA, 0x00); /* 0x0010 PWM Standby */
RegWriteA_LC898122AF(
CVA, 0x00); /* 0x0020 LINEAR PWM mode standby */
DrvSw(OFF); /* Drvier Block Ena=0 */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA,
0x00);
#endif
SelectGySleep(ON); /* Gyro Sleep */
RegReadA_LC898122AF(CLKON, &UcClkon);
UcClkon &= 0x1A;
RegWriteA_LC898122AF(CLKON, UcClkon);
break;
case STB2_OISOFF:
RegReadA_LC898122AF(CLKON, &UcClkon);
/* 0x020B PWM Clock OFF + D-Gyro I/F ON */
UcClkon |= 0x05;
RegWriteA_LC898122AF(CLKON, UcClkon);
SelectGySleep(OFF); /* Gyro Wake Up */
RegWriteA_LC898122AF(PWMMONA, 0x80);
/* 0x0030 Monitor Active */
DrvSw(ON);
/* Driver Mode setting */
RegWriteA_LC898122AF(CVA, 0xC0);
/* 0x0020 Linear PWM mode enable */
RegWriteA_LC898122AF(PWMA, 0xC0);
/* 0x0010 PWM enable */
RegReadA_LC898122AF(STBB0, &UcStbb0);
UcStbb0 |= 0x5F;
RegWriteA_LC898122AF(STBB0, UcStbb0);
break;
case STB2_AFON:
#ifdef AF_PWMMODE
#else
RegWriteA_LC898122AF(DRVFCAF, 0x00);
/* 0x0081 Drv.MODEAF=0,Drv.ENAAF=0,MODE-0 */
#endif
RegReadA_LC898122AF(STBB0, &UcStbb0);
UcStbb0 &= 0x7F;
RegWriteA_LC898122AF(STBB0, UcStbb0);
RegWriteA_LC898122AF(STBB1, 0x00);
RegWriteA_LC898122AF(PWMAAF, 0x00);
/* 0x0090 AF PWM Standby */
AfDrvSw(OFF); /* AF Drvier Block Ena=0 */
#ifdef MONITOR_OFF
#else
RegWriteA_LC898122AF(PWMMONA, 0x00);
/* 0x0030 Monitor Standby */
#endif
RegReadA_LC898122AF(CLKON, &UcClkon);
UcClkon &= 0x07;
RegWriteA_LC898122AF(CLKON, UcClkon);
break;
case STB2_AFOFF:
RegReadA_LC898122AF(CLKON, &UcClkon);
/* 0x020B OPAF Clock ON + AFPWM ON */
UcClkon |= 0x18;
RegWriteA_LC898122AF(CLKON, UcClkon);
AfDrvSw(ON);
/* AF Driver Mode setting */
RegWriteA_LC898122AF(PWMAAF, 0x00);
/* 0x0090 AF PWM Standby */
RegWriteA_LC898122AF(STBB1, 0x05);
RegReadA_LC898122AF(STBB0, &UcStbb0);
UcStbb0 |= 0x80;
RegWriteA_LC898122AF(STBB0, UcStbb0);
break;
/************** special mode ************/
}
}
#endif
void SetZsp(unsigned char UcZoomStepDat)
{
unsigned long UlGyrZmx, UlGyrZmy, UlGyrZrx, UlGyrZry;
/* Zoom Step */
if (UcZoomStepDat > (ZOOMTBL - 1))
UcZoomStepDat = (ZOOMTBL - 1); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ZOOMTBL-1<>ɐݒ肷<DD92><E882B7> */
if (UcZoomStepDat == 0) { /* initial setting */
UlGyrZmx = ClGyxZom[0]; /* Same Wide Coefficient */
UlGyrZmy = ClGyyZom[0]; /* Same Wide Coefficient */
/* Initial Rate value = 1 */
} else {
UlGyrZmx = ClGyxZom[UcZoomStepDat];
UlGyrZmy = ClGyyZom[UcZoomStepDat];
}
/* Zoom Value Setting */
RamWrite32A_LC898122AF(gxlens, UlGyrZmx); /* 0x1022 */
RamWrite32A_LC898122AF(gylens, UlGyrZmy); /* 0x1122 */
RamRead32A_LC898122AF(gxlens, &UlGyrZrx); /* 0x1022 */
RamRead32A_LC898122AF(gylens, &UlGyrZry); /* 0x1122 */
/* Zoom Value Setting Error Check */
if (UlGyrZmx != UlGyrZrx)
RamWrite32A_LC898122AF(gxlens, UlGyrZmx); /* 0x1022 */
if (UlGyrZmy != UlGyrZry)
RamWrite32A_LC898122AF(gylens, UlGyrZmy); /* 0x1122 */
}
void StbOnn(void)
{
unsigned char UcRegValx, UcRegValy; /* Registor value */
unsigned char UcRegIni;
RegReadA_LC898122AF(WH_EQSWX, &UcRegValx); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcRegValy); /* 0x0171 */
if (((UcRegValx & 0x01) != 0x01) && ((UcRegValy & 0x01) != 0x01)) {
RegWriteA_LC898122AF(WH_SMTSRVON, 0x01);
SrvCon(X_DIR, ON);
SrvCon(Y_DIR, ON);
UcRegIni = 0x11;
while ((UcRegIni & 0x77) != 0x66)
RegReadA_LC898122AF(RH_SMTSRVSTT, &UcRegIni);
RegWriteA_LC898122AF(WH_SMTSRVON, 0x00);
} else {
SrvCon(X_DIR, ON);
SrvCon(Y_DIR, ON);
}
}
void StbOnnN(unsigned char UcStbY, unsigned char UcStbX)
{
unsigned char UcRegIni;
unsigned char UcSttMsk = 0;
RegWriteA_LC898122AF(WH_SMTSRVON,
0x01); /* 0x017C Smooth Servo ON */
if (UcStbX == ON)
UcSttMsk |= 0x07;
if (UcStbY == ON)
UcSttMsk |= 0x70;
SrvCon(X_DIR, UcStbX);
SrvCon(Y_DIR, UcStbY);
UcRegIni = 0x11;
while ((UcRegIni & UcSttMsk) != (0x66 & UcSttMsk))
RegReadA_LC898122AF(RH_SMTSRVSTT, &UcRegIni);
RegWriteA_LC898122AF(WH_SMTSRVON, 0x00);
}
void OptCen(unsigned char UcOptmode, unsigned short UsOptXval,
unsigned short UsOptYval)
{
RamAccFixMod(ON); /* Fix mode */
switch (UcOptmode) {
case VAL_SET:
RamWriteA_LC898122AF(SXOFFZ1, UsOptXval);
RamWriteA_LC898122AF(SYOFFZ1, UsOptYval);
break;
case VAL_FIX:
UsCntXof = UsOptXval;
UsCntYof = UsOptYval;
RamWriteA_LC898122AF(SXOFFZ1, UsCntXof);
RamWriteA_LC898122AF(SYOFFZ1, UsCntYof);
break;
case VAL_SPC:
RamReadA_LC898122AF(SXOFFZ1, &UsOptXval);
RamReadA_LC898122AF(SYOFFZ1, &UsOptYval);
UsCntXof = UsOptXval;
UsCntYof = UsOptYval;
break;
}
RamAccFixMod(OFF); /* Float mode */
}
#ifdef MODULE_CALIBRATION
#define RRATETABLE 8
#define CRATETABLE 16
const signed char ScRselRate[RRATETABLE] = {
-12, /* -12% */
-9, /* -9% */
-6, /* -6% */
-3, /* -3% */
0, /* 0% */
3, /* 3% */
7, /* 7% */
11 /* 11% */
};
const signed char ScCselRate[CRATETABLE] = {
-14, /* -14% */
-12, /* -12% */
-10, /* -10% */
-8, /* -8% */
-6, /* -6% */
-4, /* -4% */
-2, /* -2% */
0, /* 0% */
0, /* 0% */
2, /* 2% */
4, /* 4% */
6, /* 6% */
8, /* 8% */
10, /* 10% */
12, /* 12% */
14 /* 14% */
};
#define TARGET_FREQ 48000.0F /* 48MHz */
#define START_RSEL 0x04 /* Typ */
#define START_CSEL 0x08 /* Typ bit4:OSCPMSEL */
#define MEAS_MAX 32 /* <20><><EFBFBD><EFBFBD>32<33><32> */
/* Measure Status (UcClkJdg) */
#define UNDR_MEAS 0x00
#define FIX_MEAS 0x01
#define JST_FIX 0x03
#define OVR_MEAS 0x80
/* Measure Check Flag (UcMeasFlg) */
#define RSELFX 0x08
#define RSEL1ST 0x01
#define RSEL2ND 0x02
#define CSELFX 0x80
#define CSELPLS 0x10
#define CSELMNS 0x20
unsigned short OscAdj(void)
{
unsigned char UcMeasFlg; /* Measure check flag */
union UnWrdVal StClkVal; /* Measure value */
unsigned char UcMeasCnt; /* Measure counter */
unsigned char UcOscrsel, UcOsccsel; /* Reg set value */
unsigned char UcSrvDivBk; /* back up value */
unsigned char UcClkJdg; /* State flag */
float FcalA, FcalB; /* calcurate value */
signed char ScTblRate_Val, ScTblRate_Now, ScTblRate_Tgt;
float FlRatePbk, FlRateMbk;
unsigned char UcOsccselP, UcOsccselM; /* Reg set value */
unsigned short UsResult;
/* unsigned char UcOscsetBk ; */ /* Reg set value */
UcMeasFlg = 0; /* Clear Measure check flag */
UcMeasCnt = 0; /* Clear Measure counter */
UcClkJdg = UNDR_MEAS; /* under Measure */
UcOscrsel = START_RSEL;
UcOsccsel = START_CSEL;
/* check */
/* RegReadA_LC898122AF( OSCSET, &UcOscsetBk ) ; // 0x0264 */
/* UcOscrsel = ( UcOscsetBk & 0xE0 ) >> 5 ; */
/* UcOsccsel = ( UcOscsetBk & 0x1E ) >> 1 ; */
/**/ RegReadA_LC898122AF(SRVDIV, &UcSrvDivBk); /* 0x0211 */
RegWriteA_LC898122AF(SRVDIV,
0x00); /* 0x0211 SRV Clock = Xtalck */
RegWriteA_LC898122AF(OSCSET,
(UcOscrsel << 5) | (UcOsccsel << 1)); /* 0x0257 */
while (UcClkJdg == UNDR_MEAS) {
UcMeasCnt++; /* Measure count up */
UcOscAdjFlg = MEASSTR; /* Start trigger ON */
while (UcOscAdjFlg != MEASFIX)
;
UcOscAdjFlg = 0x00; /* Clear Flag */
RegReadA_LC898122AF(OSCCK_CNTR0,
&StClkVal.StWrdVal.UcLowVal); /* 0x025E */
RegReadA_LC898122AF(OSCCK_CNTR1,
&StClkVal.StWrdVal.UcHigVal); /* 0x025F */
FcalA = (float)StClkVal.UsWrdVal;
FcalB = TARGET_FREQ / FcalA;
FcalB = FcalB - 1.0F;
FcalB *= 100.0F;
if (FcalB == 0.0F) {
UcClkJdg = JST_FIX; /* Just 36MHz */
UcMeasFlg |= (CSELFX | RSELFX); /* Fix Flag */
break;
}
/* Rsel check */
if (!(UcMeasFlg & RSELFX)) {
if (UcMeasFlg & RSEL1ST)
UcMeasFlg |= (RSELFX | RSEL2ND);
else
UcMeasFlg |= RSEL1ST;
ScTblRate_Now = ScRselRate[UcOscrsel]; /* <20><><EFBFBD><EFBFBD>Rate */
ScTblRate_Tgt = ScTblRate_Now + (short)FcalB;
if (ScTblRate_Now > ScTblRate_Tgt) {
while (1) {
if (UcOscrsel == 0)
break;
UcOscrsel -= 1;
ScTblRate_Val = ScRselRate[UcOscrsel];
if (ScTblRate_Tgt >= ScTblRate_Val)
break;
}
} else if (ScTblRate_Now < ScTblRate_Tgt) {
while (1) {
if (UcOscrsel == (RRATETABLE - 1))
break;
UcOscrsel += 1;
ScTblRate_Val = ScRselRate[UcOscrsel];
if (ScTblRate_Tgt <= ScTblRate_Val)
break;
}
} else {
;
}
} else {
/* Csel check */
if (FcalB > 0) { /* Plus */
UcMeasFlg |= CSELPLS;
FlRatePbk = FcalB;
UcOsccselP = UcOsccsel;
if (UcMeasFlg & CSELMNS) {
UcMeasFlg |= CSELFX;
UcClkJdg = FIX_MEAS; /* OK */
} else if (UcOsccsel == (CRATETABLE - 1)) {
if (UcOscrsel < (RRATETABLE - 1)) {
UcOscrsel += 1;
UcOsccsel = START_CSEL;
UcMeasFlg = 0; /* Clear */
} else {
UcClkJdg = OVR_MEAS; /* Over */
}
} else {
UcOsccsel += 1;
}
} else { /* Minus */
UcMeasFlg |= CSELMNS;
FlRateMbk = (-1) * FcalB;
UcOsccselM = UcOsccsel;
if (UcMeasFlg & CSELPLS) {
UcMeasFlg |= CSELFX;
UcClkJdg = FIX_MEAS; /* OK */
} else if (UcOsccsel == 0x00) {
if (UcOscrsel > 0) {
UcOscrsel -= 1;
UcOsccsel = START_CSEL;
UcMeasFlg = 0; /* Clear */
} else {
UcClkJdg = OVR_MEAS; /* Over */
}
} else {
UcOsccsel -= 1;
}
}
if (UcMeasCnt >= MEAS_MAX)
UcClkJdg = OVR_MEAS; /* Over */
}
RegWriteA_LC898122AF(OSCSET,
(UcOscrsel << 5) |
(UcOsccsel << 1)); /* 0x0257 */
}
UsResult = EXE_END;
if (UcClkJdg == FIX_MEAS) {
if (FlRatePbk < FlRateMbk)
UcOsccsel = UcOsccselP;
else
UcOsccsel = UcOsccselM;
RegWriteA_LC898122AF(OSCSET,
(UcOscrsel << 5) |
(UcOsccsel << 1)); /* 0x0264 */
/* check */
/* RegReadA_LC898122AF( OSCSET, &UcOscsetBk ) ; // 0x0257
*/
}
StAdjPar.UcOscVal = ((UcOscrsel << 5) | (UcOsccsel << 1));
if (UcClkJdg == OVR_MEAS) {
UsResult = EXE_OCADJ;
StAdjPar.UcOscVal = 0x00;
}
RegWriteA_LC898122AF(SRVDIV,
UcSrvDivBk); /* 0x0211 SRV Clock set */
return UsResult;
}
#endif
#ifdef HALLADJ_HW
void SetSineWave(unsigned char UcJikuSel, unsigned char UcMeasMode)
{
#ifdef USE_EXTCLK_ALL /* 24MHz */
unsigned short UsFRQ[] = {0x30EE, 0x037E};
#else
unsigned short UsFRQ[] = {0x1877, 0x01BF};
#endif
unsigned long UlAMP[2][2] = {
{0x3CA3D70A, 0x3CA3D70A}, /* Loop Gain { X amp , Y amp } */
{0x3F800000, 0x3F800000} }; /* Bias/offset { X amp , Y amp } */
unsigned char UcEqSwX, UcEqSwY;
UcMeasMode &= 0x01;
UcJikuSel &= 0x01;
/* Phase parameter 0deg */
RegWriteA_LC898122AF(WC_SINPHSX, 0x00); /* 0x0183 */
RegWriteA_LC898122AF(WC_SINPHSY, 0x00); /* 0x0184 */
/* wait 0 cross */
RegWriteA_LC898122AF(WC_MESSINMODE,
0x00); /* 0x0191 Sine 0 cross */
RegWriteA_LC898122AF(WC_MESWAIT, 0x00); /* 0x0199 0 cross wait */
/* Manually Set Amplitude */
RamWrite32A_LC898122AF(sxsin, UlAMP[UcMeasMode][X_DIR]); /* 0x10D5 */
RamWrite32A_LC898122AF(sysin, UlAMP[UcMeasMode][Y_DIR]); /* 0x11D5 */
/* Freq */
RegWriteA_LC898122AF(WC_SINFRQ0,
(unsigned char)
UsFRQ[UcMeasMode]); /* 0x0181 Freq L */
RegWriteA_LC898122AF(WC_SINFRQ1,
(unsigned char)(UsFRQ[UcMeasMode] >> 8));
/* Clear Optional Sine wave input address */
RegReadA_LC898122AF(WH_EQSWX, &UcEqSwX); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcEqSwY); /* 0x0171 */
if (!UcMeasMode && !UcJikuSel) { /* Loop Gain mode X-axis */
UcEqSwX |= 0x10; /* SW[4] */
UcEqSwY &= ~EQSINSW;
} else if (!UcMeasMode && UcJikuSel) { /* Loop Gain mode Y-Axis */
UcEqSwX &= ~EQSINSW;
UcEqSwY |= 0x10; /* SW[4] */
} else if (UcMeasMode && !UcJikuSel) { /* Bias/Offset mode X-Axis */
UcEqSwX = 0x22; /* SW[5] */
UcEqSwY = 0x03;
} else { /* Bias/Offset mode Y-Axis */
UcEqSwX = 0x03;
UcEqSwY = 0x22; /* SW[5] */
}
RegWriteA_LC898122AF(WH_EQSWX, UcEqSwX); /* 0x0170 */
RegWriteA_LC898122AF(WH_EQSWY, UcEqSwY); /* 0x0171 */
}
void StartSineWave(void)
{
/* Start Sine Wave */
RegWriteA_LC898122AF(WC_SINON, 0x01); /* 0x0180 Sine wave */
}
void StopSineWave(void)
{
unsigned char UcEqSwX, UcEqSwY;
RegWriteA_LC898122AF(WC_SINON, 0x00); /* 0x0180 Sine wave Stop */
RegReadA_LC898122AF(WH_EQSWX, &UcEqSwX); /* 0x0170 */
RegReadA_LC898122AF(WH_EQSWY, &UcEqSwY); /* 0x0171 */
UcEqSwX &= ~EQSINSW;
UcEqSwY &= ~EQSINSW;
RegWriteA_LC898122AF(WH_EQSWX,
UcEqSwX); /* 0x0170 Switch control */
RegWriteA_LC898122AF(WH_EQSWY,
UcEqSwY); /* 0x0171 Switch control */
}
void SetMeasFil(unsigned char UcFilSel)
{
MesFil(UcFilSel); /* Set Measure filter */
}
void ClrMeasFil(void)
{
/* Measure Filters clear */
ClrGyr(0x1000, CLR_FRAM1); /* MEAS-FIL Delay RAM Clear */
}
#ifdef MODULE_CALIBRATION
unsigned char LoopGainAdj(unsigned char UcJikuSel)
{
unsigned short UsRltVal;
unsigned char UcAdjSts = FAILURE;
UcJikuSel &= 0x01;
StbOnn(); /* Slope Mode */
/* Wait 200ms */
WitTim_LC898122AF(200);
/* set start gain */
LopPar(UcJikuSel);
/* set sine wave */
SetSineWave(UcJikuSel, __MEASURE_LOOPGAIN);
/* Measure count */
RegWriteA_LC898122AF(WC_MESLOOP1, 0x00); /* 0x0193 */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x40); /* 0x0192 64 Times Measure */
RamWrite32A_LC898122AF(msmean,
0x3C800000); /* 0x1230 1/CmMesLoop[15:0] */
RegWriteA_LC898122AF(WC_MESABS, 0x01); /* 0x0198 ABS */
/* Set Adjustment Limits */
RamWrite32A_LC898122AF(
LGMax,
0x3F800000); /* 0x1092 Loop gain adjustment max limit */
RamWrite32A_LC898122AF(
LGMin,
0x3E000100); /* 0x1091 Loop gain adjustment min limit */
RegWriteA_LC898122AF(WC_AMJLOOP1,
0x00); /* 0x01A3 Time-Out time */
RegWriteA_LC898122AF(WC_AMJLOOP0,
0x41); /* 0x01A2 Time-Out time */
RegWriteA_LC898122AF(WC_AMJIDL1, 0x00); /* 0x01A5 wait */
RegWriteA_LC898122AF(WC_AMJIDL0, 0x00); /* 0x01A4 wait */
/* set Measure Filter */
SetMeasFil(LOOPGAIN);
/* Clear Measure Filters */
ClrMeasFil();
/* Start Sine Wave */
StartSineWave();
/* Enable Loop Gain Adjustment */
/* Check Busy Flag */
BsyWit(WC_AMJMODE,
(0x0E |
(UcJikuSel << 4))); /* 0x01A0 Loop Gain adjustment */
RegReadA_LC898122AF(RC_AMJERROR, &UcAdjBsy); /* 0x01AD */
/* Ram Access */
RamAccFixMod(ON); /* Fix mode */
if (UcAdjBsy) {
if (UcJikuSel == X_DIR) {
RamReadA_LC898122AF(sxg, &UsRltVal); /* 0x10D3 */
StAdjPar.StLopGan.UsLxgVal = UsRltVal;
StAdjPar.StLopGan.UsLxgSts = 0x0000;
} else {
RamReadA_LC898122AF(syg, &UsRltVal); /* 0x11D3 */
StAdjPar.StLopGan.UsLygVal = UsRltVal;
StAdjPar.StLopGan.UsLygSts = 0x0000;
}
} else {
if (UcJikuSel == X_DIR) {
RamReadA_LC898122AF(sxg, &UsRltVal); /* 0x10D3 */
StAdjPar.StLopGan.UsLxgVal = UsRltVal;
StAdjPar.StLopGan.UsLxgSts = 0xFFFF;
} else {
RamReadA_LC898122AF(syg, &UsRltVal); /* 0x11D3 */
StAdjPar.StLopGan.UsLygVal = UsRltVal;
StAdjPar.StLopGan.UsLygSts = 0xFFFF;
}
UcAdjSts = SUCCESS; /* Status OK */
}
/* Ram Access */
RamAccFixMod(OFF); /* Float mode */
/* Stop Sine Wave */
StopSineWave();
return UcAdjSts;
}
#endif
unsigned char BiasOffsetAdj(unsigned char UcJikuSel, unsigned char UcMeasCnt)
{
unsigned char UcHadjRst;
unsigned long UlTgtVal[2][5] = {{0x3F800000, 0x3D200140, 0xBD200140,
0x3F547AE1, 0x3F451EB8}, /* ROUGH */
{0x3F000000, 0x3D200140, 0xBD200140,
0x3F50A3D7, 0x3F48F5C3} }; /* FINE */
if (UcMeasCnt > 1)
UcMeasCnt = 1;
UcJikuSel &= 0x01;
/* Set Sine Wave */
SetSineWave(UcJikuSel, __MEASURE_BIASOFFSET);
/* Measure count */
RegWriteA_LC898122AF(WC_MESLOOP1, 0x00); /* 0x0193 */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x04); /* 0x0192 4 Times Measure */
RamWrite32A_LC898122AF(
msmean, 0x3E000000); /* 0x10AE 1/CmMesLoop[15:0]/2 */
RegWriteA_LC898122AF(WC_MESABS, 0x00); /* 0x0198 ABS */
/* Set Adjustment Limits */
RamWrite32A_LC898122AF(
HOStp,
UlTgtVal[UcMeasCnt][0]); /* 0x1070 Hall Offset Stp */
RamWrite32A_LC898122AF(
HOMax, UlTgtVal[UcMeasCnt]
[1]); /* 0x1072 Hall Offset max limit */
RamWrite32A_LC898122AF(
HOMin, UlTgtVal[UcMeasCnt]
[2]); /* 0x1071 Hall Offset min limit */
RamWrite32A_LC898122AF(
HBStp, UlTgtVal[UcMeasCnt][0]); /* 0x1080 Hall Bias Stp */
RamWrite32A_LC898122AF(
HBMax,
UlTgtVal[UcMeasCnt][3]); /* 0x1082 Hall Bias max limit */
RamWrite32A_LC898122AF(
HBMin,
UlTgtVal[UcMeasCnt][4]); /* 0x1081 Hall Bias min limit */
RegWriteA_LC898122AF(WC_AMJLOOP1,
0x00); /* 0x01A3 Time-Out time */
RegWriteA_LC898122AF(WC_AMJLOOP0,
0x40); /* 0x01A2 Time-Out time */
RegWriteA_LC898122AF(WC_AMJIDL1, 0x00); /* 0x01A5 wait */
RegWriteA_LC898122AF(WC_AMJIDL0, 0x00); /* 0x01A4 wait */
/* Set Measure Filter */
SetMeasFil(HALL_ADJ);
/* Clear Measure Filters */
ClrMeasFil();
/* Start Sine Wave */
StartSineWave();
/* Check Busy Flag */
BsyWit(
WC_AMJMODE,
(0x0C |
(UcJikuSel
<< 4))); /* 0x01A0 Hall bais/offset ppt adjustment */
RegReadA_LC898122AF(RC_AMJERROR, &UcAdjBsy); /* 0x01AD */
if (UcAdjBsy) {
if (UcJikuSel == X_DIR)
UcHadjRst = EXE_HXADJ;
else
UcHadjRst = EXE_HYADJ;
} else {
UcHadjRst = EXE_END;
}
/* Stop Sine Wave */
StopSineWave();
/* Set Servo Filter */
/* Ram Access */
RamAccFixMod(ON); /* Fix mode */
if (UcJikuSel == X_DIR) {
RamReadA_LC898122AF(
MSPP1AV,
&StAdjPar.StHalAdj
.UsHlxMxa); /* 0x1042 Max width value */
RamReadA_LC898122AF(
MSCT1AV,
&StAdjPar.StHalAdj.UsHlxCna); /* 0x1052 offset value */
} else {
RamReadA_LC898122AF(
MSPP1AV,
&StAdjPar.StHalAdj
.UsHlyMxa); /* 0x1042 Max width value */
RamReadA_LC898122AF(
MSCT1AV,
&StAdjPar.StHalAdj.UsHlyCna); /* 0x1052 offset value */
}
StAdjPar.StHalAdj.UsHlxCna =
(unsigned short)((signed short)StAdjPar.StHalAdj.UsHlxCna << 1);
StAdjPar.StHalAdj.UsHlyCna =
(unsigned short)((signed short)StAdjPar.StHalAdj.UsHlyCna << 1);
/* Ram Access */
RamAccFixMod(OFF); /* Float mode */
return UcHadjRst;
}
#endif
void GyrGan(unsigned char UcGygmode, unsigned long UlGygXval,
unsigned long UlGygYval)
{
switch (UcGygmode) {
case VAL_SET:
RamWrite32A_LC898122AF(gxzoom, UlGygXval); /* 0x1020 */
RamWrite32A_LC898122AF(gyzoom, UlGygYval); /* 0x1120 */
break;
case VAL_FIX:
RamWrite32A_LC898122AF(gxzoom, UlGygXval); /* 0x1020 */
RamWrite32A_LC898122AF(gyzoom, UlGygYval); /* 0x1120 */
break;
case VAL_SPC:
RamRead32A_LC898122AF(gxzoom, &UlGygXval); /* 0x1020 */
RamRead32A_LC898122AF(gyzoom, &UlGygYval); /* 0x1120 */
break;
}
}
void SetPanTiltMode(unsigned char UcPnTmod)
{
switch (UcPnTmod) {
case OFF:
RegWriteA_LC898122AF(WG_PANON, 0x00);
break;
case ON:
RegWriteA_LC898122AF(WG_PANON, 0x01);
break;
}
}
#ifdef GAIN_CONT
unsigned char TriSts(void)
{
unsigned char UcRsltSts = 0;
unsigned char UcVal;
RegReadA_LC898122AF(WG_ADJGANGXATO, &UcVal); /* 0x0129 */
if (UcVal & 0x03) { /* Gain control enable? */
RegReadA_LC898122AF(RG_LEVJUGE, &UcVal); /* 0x01F4 */
UcRsltSts = UcVal & 0x11; /* bit0, bit4 set */
UcRsltSts |= 0x80; /* bit7 ON */
}
return UcRsltSts;
}
#endif
unsigned char DrvPwmSw(unsigned char UcSelPwmMod)
{
switch (UcSelPwmMod) {
case Mlnp:
RegWriteA_LC898122AF(DRVFC, 0xF0);
UcPwmMod = PWMMOD_CVL;
break;
case Mpwm:
#ifdef PWM_BREAK
RegWriteA_LC898122AF(DRVFC, 0x00);
#else
RegWriteA_LC898122AF(DRVFC, 0xC0);
#endif
UcPwmMod = PWMMOD_PWM;
break;
}
return UcSelPwmMod << 4;
}
#ifdef NEUTRAL_CENTER
unsigned char TneHvc(void)
{
unsigned char UcRsltSts;
unsigned short UsMesRlt1;
unsigned short UsMesRlt2;
SrvCon(X_DIR, OFF); /* X Servo OFF */
SrvCon(Y_DIR, OFF); /* Y Servo OFF */
WitTim_LC898122AF(500);
/* <20><><EFBFBD>ϒl<CF92><6C><EFBFBD><EFBFBD> */
MesFil(THROUGH); /* Set Measure Filter */
RegWriteA_LC898122AF(WC_MESLOOP1,
0x00); /* 0x0193 CmMesLoop[15:8] */
RegWriteA_LC898122AF(WC_MESLOOP0,
0x40); /* 0x0192 CmMesLoop[7:0] */
RamWrite32A_LC898122AF(msmean,
0x3C800000); /* 0x1230 1/CmMesLoop[15:0] */
RegWriteA_LC898122AF(WC_MES1ADD0, (unsigned char)AD0Z); /* 0x0194 */
RegWriteA_LC898122AF(WC_MES1ADD1, (unsigned char)((AD0Z >> 8) &
0x0001)); /* 0x0195 */
RegWriteA_LC898122AF(WC_MES2ADD0, (unsigned char)AD1Z); /* 0x0196 */
RegWriteA_LC898122AF(WC_MES2ADD1, (unsigned char)((AD1Z >> 8) &
0x0001)); /* 0x0197 */
RamWrite32A_LC898122AF(MSABS1AV, 0x00000000); /* 0x1041 */
RamWrite32A_LC898122AF(MSABS2AV, 0x00000000); /* 0x1141 */
RegWriteA_LC898122AF(WC_MESABS, 0x00); /* 0x0198 none ABS */
BsyWit(WC_MESMODE, 0x01); /* 0x0190 Normal Measure */
RamAccFixMod(ON); /* Fix mode */
RamReadA_LC898122AF(MSABS1AV,
&UsMesRlt1); /* 0x1041 Measure Result */
RamReadA_LC898122AF(MSABS2AV,
&UsMesRlt2); /* 0x1141 Measure Result */
RamAccFixMod(OFF); /* Float mode */
StAdjPar.StHalAdj.UsHlxCna = UsMesRlt1; /* Measure Result Store */
StAdjPar.StHalAdj.UsHlxCen = UsMesRlt1; /* Measure Result Store */
StAdjPar.StHalAdj.UsHlyCna = UsMesRlt2; /* Measure Result Store */
StAdjPar.StHalAdj.UsHlyCen = UsMesRlt2; /* Measure Result Store */
UcRsltSts = EXE_END; /* Clear Status */
return UcRsltSts;
}
#endif /* NEUTRAL_CENTER */
void SetGcf(unsigned char UcSetNum)
{
/* Zoom Step */
if (UcSetNum > (COEFTBL - 1))
UcSetNum = (COEFTBL - 1); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>COEFTBL-1<>ɐݒ肷<DD92><E882B7> */
UlH1Coefval = ClDiCof[UcSetNum];
/* Zoom Value Setting */
RamWrite32A_LC898122AF(gxh1c, UlH1Coefval); /* 0x1012 */
RamWrite32A_LC898122AF(gyh1c, UlH1Coefval); /* 0x1112 */
#ifdef H1COEF_CHANGER
SetH1cMod(UcSetNum); /* Re-setting */
#endif
}
#ifdef H1COEF_CHANGER
void SetH1cMod(unsigned char UcSetNum)
{
switch (UcSetNum) {
case (ACTMODE): /* initial */
IniPtMovMod(OFF); /* Pan/Tilt setting (Still) */
/* enable setting */
/* Zoom Step */
UlH1Coefval = ClDiCof[0];
UcH1LvlMod = 0;
/* Limit value Value Setting */
RamWrite32A_LC898122AF(gxlmt6L, MINLMT); /* 0x102D L-Limit */
RamWrite32A_LC898122AF(gxlmt6H, MAXLMT); /* 0x102E H-Limit */
RamWrite32A_LC898122AF(gylmt6L, MINLMT); /* 0x112D L-Limit */
RamWrite32A_LC898122AF(gylmt6H, MAXLMT); /* 0x112E H-Limit */
RamWrite32A_LC898122AF(gxhc_tmp,
UlH1Coefval); /* 0x100E Base Coef */
RamWrite32A_LC898122AF(
gxmg, CHGCOEF); /* 0x10AA Change coefficient gain */
RamWrite32A_LC898122AF(gyhc_tmp,
UlH1Coefval); /* 0x110E Base Coef */
RamWrite32A_LC898122AF(
gymg, CHGCOEF); /* 0x11AA Change coefficient gain */
RegWriteA_LC898122AF(
WG_HCHR, 0x12); /* 0x011B GmHChrOn[1]=1 Sw ON */
break;
case (S2MODE): /* cancel lvl change mode */
RegWriteA_LC898122AF(
WG_HCHR, 0x10); /* 0x011B GmHChrOn[1]=0 Sw OFF */
break;
case (MOVMODE): /* Movie mode */
IniPtMovMod(ON); /* Pan/Tilt setting (Movie) */
RamWrite32A_LC898122AF(gxlmt6L,
MINLMT_MOV); /* 0x102D L-Limit */
RamWrite32A_LC898122AF(gylmt6L,
MINLMT_MOV); /* 0x112D L-Limit */
RamWrite32A_LC898122AF(
gxmg, CHGCOEF_MOV); /* 0x10AA Change coefficient gain */
RamWrite32A_LC898122AF(
gymg, CHGCOEF_MOV); /* 0x11AA Change coefficient gain */
RamWrite32A_LC898122AF(gxhc_tmp,
UlH1Coefval); /* 0x100E Base Coef */
RamWrite32A_LC898122AF(gyhc_tmp,
UlH1Coefval); /* 0x110E Base Coef */
RegWriteA_LC898122AF(
WG_HCHR, 0x12); /* 0x011B GmHChrOn[1]=1 Sw ON */
break;
default:
IniPtMovMod(OFF); /* Pan/Tilt setting (Still) */
UcH1LvlMod = UcSetNum;
RamWrite32A_LC898122AF(gxlmt6L, MINLMT); /* 0x102D L-Limit */
RamWrite32A_LC898122AF(gylmt6L, MINLMT); /* 0x112D L-Limit */
RamWrite32A_LC898122AF(
gxmg, CHGCOEF); /* 0x10AA Change coefficient gain */
RamWrite32A_LC898122AF(
gymg, CHGCOEF); /* 0x11AA Change coefficient gain */
RamWrite32A_LC898122AF(gxhc_tmp,
UlH1Coefval); /* 0x100E Base Coef */
RamWrite32A_LC898122AF(gyhc_tmp,
UlH1Coefval); /* 0x110E Base Coef */
RegWriteA_LC898122AF(
WG_HCHR, 0x12); /* 0x011B GmHChrOn[1]=1 Sw ON */
break;
}
}
#endif
unsigned short RdFwVr(void)
{
unsigned short UsVerVal;
UsVerVal = (unsigned short)((MDL_VER << 8) | FW_VER);
return UsVerVal;
}
Reference in New Issue Copy Permalink