2905 lines
78 KiB
C
2905 lines
78 KiB
C
// 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;
|
||
}
|