1617 lines
60 KiB
C
1617 lines
60 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
||
/*
|
||
* Copyright (c) 2019 MediaTek Inc.
|
||
*/
|
||
|
||
|
||
|
||
/* ************************** */
|
||
/* Include Header File */
|
||
/* ************************** */
|
||
/* #define OISINI */
|
||
|
||
/* #include "Main.h" */
|
||
/* #include "Cmd.h" */
|
||
#include "Ois.h"
|
||
#include "OisDef.h"
|
||
#include "OisFil.h"
|
||
|
||
unsigned short UsCntXof; /* OPTICAL Center Xvalue */
|
||
unsigned short UsCntYof; /* OPTICAL Center Yvalue */
|
||
unsigned char UcPwmMod; /* PWM MODE */
|
||
unsigned char UcCvrCod; /* CverCode */
|
||
|
||
const struct STFILREG CsFilReg[] = {{0x0111, 0x00}, /*00,0111 */
|
||
{0x0113, 0x00}, /*00,0113 */
|
||
{0x0114, 0x00}, /*00,0114 */
|
||
{0x0172, 0x00}, /*00,0172 */
|
||
{0x01E3, 0x00}, /*00,01E3 */
|
||
{0x01E4, 0x00}, /*00,01E4 */
|
||
{0xFFFF, 0xFF} };
|
||
|
||
/* 32bit */
|
||
const struct STFILRAM CsFilRam[] = {
|
||
{0x1000, 0x3F800000}, /*3F800000,1000,0dB,invert=0 */
|
||
{0x1001, 0x3F800000}, /*3F800000,1001,0dB,invert=0 */
|
||
{0x1002, 0x00000000}, /*00000000,1002,Cutoff,invert=0 */
|
||
{0x1003, 0x3F800000}, /*3F800000,1003,0dB,invert=0 */
|
||
{0x1004, 0x3828A700}, /*3828A700,1004,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1005, 0x3828A700}, /*3828A700,1005,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1006, 0x3F7FFAC0}, /*3F7FFAC0,1006,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1007, 0x3F800000}, /*3F800000,1007,0dB,invert=0 */
|
||
{0x1008, 0xBF800000}, /*BF800000,1008,0dB,invert=1 */
|
||
{0x1009, 0x3F800000}, /*3F800000,1009,0dB,invert=0 */
|
||
{0x100A, 0x3F800000}, /*3F800000,100A,0dB,invert=0 */
|
||
{0x100B, 0x3F800000}, /*3F800000,100B,0dB,invert=0 */
|
||
{0x100C, 0x3F800000}, /*3F800000,100C,0dB,invert=0 */
|
||
{0x100E, 0x3F800000}, /*3F800000,100E,0dB,invert=0 */
|
||
{0x1010, 0x3DA2AD80}, /*3DA2AD80,1010 */
|
||
{0x1011, 0x00000000}, /*00000000,1011,Free,fs/1,invert=0 */
|
||
{0x1012, 0x3F7FFE00}, /*3F7FFE00,1012,Free,fs/1,invert=0 */
|
||
{0x1013,
|
||
0x3FB26DC0}, /*3FB26DC0,1013,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1014,
|
||
0xBFB00DC0}, /*BFB00DC0,1014,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1015,
|
||
0x3F75E8C0}, /*3F75E8C0,1015,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1016,
|
||
0x3F1B2780}, /*3F1B2780,1016,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1017,
|
||
0xBF1B2400}, /*BF1B2400,1017,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1018,
|
||
0x3F7FFC80}, /*3F7FFC80,1018,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1019, 0x3F800000}, /*3F800000,1019,Through,0dB,fs/1,invert=0 */
|
||
{0x101A, 0x00000000}, /*00000000,101A,Through,0dB,fs/1,invert=0 */
|
||
{0x101B, 0x00000000}, /*00000000,101B,Through,0dB,fs/1,invert=0 */
|
||
{0x101C, 0x3F800000}, /*3F800000,101C,0dB,invert=0 */
|
||
{0x101D, 0x00000000}, /*00000000,101D,Cutoff,invert=0 */
|
||
{0x101E, 0x3F800000}, /*3F800000,101E,0dB,invert=0 */
|
||
{0x1020, 0x3F800000}, /*3F800000,1020,0dB,invert=0 */
|
||
{0x1021, 0x3F800000}, /*3F800000,1021,0dB,invert=0 */
|
||
{0x1022, 0x3F800000}, /*3F800000,1022,0dB,invert=0 */
|
||
{0x1023, 0x3F800000}, /*3F800000,1023,Through,0dB,fs/1,invert=0 */
|
||
{0x1024, 0x00000000}, /*00000000,1024,Through,0dB,fs/1,invert=0 */
|
||
{0x1025, 0x00000000}, /*00000000,1025,Through,0dB,fs/1,invert=0 */
|
||
{0x1026, 0x00000000}, /*00000000,1026,Through,0dB,fs/1,invert=0 */
|
||
{0x1027, 0x00000000}, /*00000000,1027,Through,0dB,fs/1,invert=0 */
|
||
{0x1030, 0x3F800000}, /*3F800000,1030,Through,0dB,fs/1,invert=0 */
|
||
{0x1031, 0x00000000}, /*00000000,1031,Through,0dB,fs/1,invert=0 */
|
||
{0x1032, 0x00000000}, /*00000000,1032,Through,0dB,fs/1,invert=0 */
|
||
{0x1033, 0x3F800000}, /*3F800000,1033,Through,0dB,fs/1,invert=0 */
|
||
{0x1034, 0x00000000}, /*00000000,1034,Through,0dB,fs/1,invert=0 */
|
||
{0x1035, 0x00000000}, /*00000000,1035,Through,0dB,fs/1,invert=0 */
|
||
{0x1036, 0x3F800000}, /*3F800000,1036,Through,0dB,fs/1,invert=0 */
|
||
{0x1037, 0x00000000}, /*00000000,1037,Through,0dB,fs/1,invert=0 */
|
||
{0x1038, 0x00000000}, /*00000000,1038,Through,0dB,fs/1,invert=0 */
|
||
{0x1039, 0x3F800000}, /*3F800000,1039,Through,0dB,fs/1,invert=0 */
|
||
{0x103A, 0x00000000}, /*00000000,103A,Through,0dB,fs/1,invert=0 */
|
||
{0x103B, 0x00000000}, /*00000000,103B,Through,0dB,fs/1,invert=0 */
|
||
{0x103C, 0x3F800000}, /*3F800000,103C,Through,0dB,fs/1,invert=0 */
|
||
{0x103D, 0x00000000}, /*00000000,103D,Through,0dB,fs/1,invert=0 */
|
||
{0x103E, 0x00000000}, /*00000000,103E,Through,0dB,fs/1,invert=0 */
|
||
{0x1043, 0x39D2BD40}, /*39D2BD40,1043,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1044, 0x39D2BD40}, /*39D2BD40,1044,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1045, 0x3F7FCB40}, /*3F7FCB40,1045,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1046, 0x388C8A40}, /*388C8A40,1046,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1047, 0x388C8A40}, /*388C8A40,1047,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1048, 0x3F7FF740}, /*3F7FF740,1048,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1049, 0x390C87C0}, /*390C87C0,1049,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x104A, 0x390C87C0}, /*390C87C0,104A,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x104B, 0x3F7FEE80}, /*3F7FEE80,104B,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x104C, 0x398C8300}, /*398C8300,104C,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x104D, 0x398C8300}, /*398C8300,104D,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x104E, 0x3F7FDCC0}, /*3F7FDCC0,104E,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x1053, 0x3F800000}, /*3F800000,1053,Through,0dB,fs/1,invert=0 */
|
||
{0x1054, 0x00000000}, /*00000000,1054,Through,0dB,fs/1,invert=0 */
|
||
{0x1055, 0x00000000}, /*00000000,1055,Through,0dB,fs/1,invert=0 */
|
||
{0x1056, 0x3F800000}, /*3F800000,1056,Through,0dB,fs/1,invert=0 */
|
||
{0x1057, 0x00000000}, /*00000000,1057,Through,0dB,fs/1,invert=0 */
|
||
{0x1058, 0x00000000}, /*00000000,1058,Through,0dB,fs/1,invert=0 */
|
||
{0x1059, 0x3F800000}, /*3F800000,1059,Through,0dB,fs/1,invert=0 */
|
||
{0x105A, 0x00000000}, /*00000000,105A,Through,0dB,fs/1,invert=0 */
|
||
{0x105B, 0x00000000}, /*00000000,105B,Through,0dB,fs/1,invert=0 */
|
||
{0x105C, 0x3F800000}, /*3F800000,105C,Through,0dB,fs/1,invert=0 */
|
||
{0x105D, 0x00000000}, /*00000000,105D,Through,0dB,fs/1,invert=0 */
|
||
{0x105E, 0x00000000}, /*00000000,105E,Through,0dB,fs/1,invert=0 */
|
||
{0x1063, 0x3F800000}, /*3F800000,1063,0dB,invert=0 */
|
||
{0x1066, 0x3F800000}, /*3F800000,1066,0dB,invert=0 */
|
||
{0x1069, 0x3F800000}, /*3F800000,1069,0dB,invert=0 */
|
||
{0x106C, 0x3F800000}, /*3F800000,106C,0dB,invert=0 */
|
||
{0x1073, 0x00000000}, /*00000000,1073,Cutoff,invert=0 */
|
||
{0x1076, 0x3F800000}, /*3F800000,1076,0dB,invert=0 */
|
||
{0x1079, 0x3F800000}, /*3F800000,1079,0dB,invert=0 */
|
||
{0x107C, 0x3F800000}, /*3F800000,107C,0dB,invert=0 */
|
||
{0x1083, 0x38D1B700}, /*38D1B700,1083,-80dB,invert=0 */
|
||
{0x1086, 0x00000000}, /*00000000,1086,Cutoff,invert=0 */
|
||
{0x1089, 0x00000000}, /*00000000,1089,Cutoff,invert=0 */
|
||
{0x108C, 0x00000000}, /*00000000,108C,Cutoff,invert=0 */
|
||
{0x1093, 0x00000000}, /*00000000,1093,Cutoff,invert=0 */
|
||
{0x1098, 0x3F800000}, /*3F800000,1098,0dB,invert=0 */
|
||
{0x1099, 0x3F800000}, /*3F800000,1099,0dB,invert=0 */
|
||
{0x109A, 0x3F800000}, /*3F800000,109A,0dB,invert=0 */
|
||
{0x10A1, 0x3C58B440}, /*3C58B440,10A1,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A2, 0x3C58B440}, /*3C58B440,10A2,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A3, 0x3F793A40}, /*3F793A40,10A3,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A4, 0x3C58B440}, /*3C58B440,10A4,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A5, 0x3C58B440}, /*3C58B440,10A5,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A6, 0x3F793A40}, /*3F793A40,10A6,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x10A7, 0x3F800000}, /*3F800000,10A7,Through,0dB,fs/1,invert=0 */
|
||
{0x10A8, 0x00000000}, /*00000000,10A8,Through,0dB,fs/1,invert=0 */
|
||
{0x10A9, 0x00000000}, /*00000000,10A9,Through,0dB,fs/1,invert=0 */
|
||
{0x10AA, 0x00000000}, /*00000000,10AA,Cutoff,invert=0 */
|
||
{0x10AB, 0x3BDA2580}, /*3BDA2580,10AB,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x10AC, 0x3BDA2580}, /*3BDA2580,10AC,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x10AD, 0x3F7C9780}, /*3F7C9780,10AD,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x10B0, 0x3DD17800}, /*3DD17800,10B0,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x10B1, 0x3DD17800}, /*3DD17800,10B1,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x10B2, 0x3F4BA200}, /*3F4BA200,10B2,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x10B3, 0x3F800000}, /*3F800000,10B3,0dB,invert=0 */
|
||
{0x10B4, 0x00000000}, /*00000000,10B4,Cutoff,invert=0 */
|
||
{0x10B5, 0x00000000}, /*00000000,10B5,Cutoff,invert=0 */
|
||
{0x10B6, 0x3F353C00}, /*3F353C00,10B6,-3dB,invert=0 */
|
||
{0x10B8, 0x3F800000}, /*3F800000,10B8,0dB,invert=0 */
|
||
{0x10B9, 0x00000000}, /*00000000,10B9,Cutoff,invert=0 */
|
||
{0x10C0,
|
||
0x3F915680}, /*3F915680,10C0,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x10C1,
|
||
0xBF8E4100}, /*BF8E4100,10C1,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x10C2,
|
||
0x3F48E240}, /*3F48E240,10C2,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x10C3,
|
||
0x3FA04B40}, /*3FA04B40,10C3,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x10C4,
|
||
0xBF9C0DC0}, /*BF9C0DC0,10C4,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x10C5,
|
||
0x3F7691C0}, /*3F7691C0,10C5,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x10C6, 0x3D506F00}, /*3D506F00,10C6,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x10C7, 0x3D506F00}, /*3D506F00,10C7,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x10C8, 0x3F65F240}, /*3F65F240,10C8,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x10C9, 0x3C208400}, /*3C208400,10C9,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x10CA, 0x3C208400}, /*3C208400,10CA,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x10CB, 0x3F7FE940}, /*3F7FE940,10CB,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x10CC,
|
||
0x3E196280}, /*3E196280,10CC,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x10CD,
|
||
0xBE17BF80}, /*BE17BF80,10CD,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x10CE,
|
||
0x3F7EF900}, /*3F7EF900,10CE,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x10D0, 0x3FFF64C0}, /*3FFF64C0,10D0,6dB,invert=0 */
|
||
{0x10D1, 0x00000000}, /*00000000,10D1,Cutoff,invert=0 */
|
||
{0x10D2, 0x3F800000}, /*3F800000,10D2,0dB,invert=0 */
|
||
{0x10D3, 0x3F800000}, /*3F800000,10D3,0dB,invert=0 */
|
||
{0x10D4, 0x3F800000}, /*3F800000,10D4,0dB,invert=0 */
|
||
{0x10D5, 0x3F800000}, /*3F800000,10D5,0dB,invert=0 */
|
||
{0x10D7, 0x41618840}, /*41618840,10D7,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x10D8, 0x41618840}, /*41618840,10D8,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x10D9, 0x3DDE3840}, /*3DDE3840,10D9,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x10DA,
|
||
0x3F672280}, /*3F672280,10DA,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x10DB,
|
||
0xBFD3E1C0}, /*BFD3E1C0,10DB,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x10DC,
|
||
0x3FD3E1C0}, /*3FD3E1C0,10DC,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x10DD,
|
||
0x3F5022C0}, /*3F5022C0,10DD,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x10DE,
|
||
0xBF374540}, /*BF374540,10DE,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x10E0, 0x3F800000}, /*3F800000,10E0,Through,0dB,fs/1,invert=0 */
|
||
{0x10E1, 0x00000000}, /*00000000,10E1,Through,0dB,fs/1,invert=0 */
|
||
{0x10E2, 0x00000000}, /*00000000,10E2,Through,0dB,fs/1,invert=0 */
|
||
{0x10E3, 0x00000000}, /*00000000,10E3,Through,0dB,fs/1,invert=0 */
|
||
{0x10E4, 0x00000000}, /*00000000,10E4,Through,0dB,fs/1,invert=0 */
|
||
{0x10E5, 0x3F800000}, /*3F800000,10E5,0dB,invert=0 */
|
||
{0x10E8, 0x3F800000}, /*3F800000,10E8,0dB,invert=0 */
|
||
{0x10E9, 0x00000000}, /*00000000,10E9,Cutoff,invert=0 */
|
||
{0x10EA, 0x00000000}, /*00000000,10EA,Cutoff,invert=0 */
|
||
{0x10EB, 0x00000000}, /*00000000,10EB,Cutoff,invert=0 */
|
||
{0x10F0, 0x3F800000}, /*3F800000,10F0,Through,0dB,fs/1,invert=0 */
|
||
{0x10F1, 0x00000000}, /*00000000,10F1,Through,0dB,fs/1,invert=0 */
|
||
{0x10F2, 0x00000000}, /*00000000,10F2,Through,0dB,fs/1,invert=0 */
|
||
{0x10F3, 0x00000000}, /*00000000,10F3,Through,0dB,fs/1,invert=0 */
|
||
{0x10F4, 0x00000000}, /*00000000,10F4,Through,0dB,fs/1,invert=0 */
|
||
{0x10F5, 0x3F800000}, /*3F800000,10F5,Through,0dB,fs/1,invert=0 */
|
||
{0x10F6, 0x00000000}, /*00000000,10F6,Through,0dB,fs/1,invert=0 */
|
||
{0x10F7, 0x00000000}, /*00000000,10F7,Through,0dB,fs/1,invert=0 */
|
||
{0x10F8, 0x00000000}, /*00000000,10F8,Through,0dB,fs/1,invert=0 */
|
||
{0x10F9, 0x00000000}, /*00000000,10F9,Through,0dB,fs/1,invert=0 */
|
||
{0x1100, 0x3F800000}, /*3F800000,1100,0dB,invert=0 */
|
||
{0x1101, 0x3F800000}, /*3F800000,1101,0dB,invert=0 */
|
||
{0x1102, 0x00000000}, /*00000000,1102,Cutoff,invert=0 */
|
||
{0x1103, 0x3F800000}, /*3F800000,1103,0dB,invert=0 */
|
||
{0x1104, 0x3828A700}, /*3828A700,1104,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1105, 0x3828A700}, /*3828A700,1105,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1106, 0x3F7FFAC0}, /*3F7FFAC0,1106,LPF,0.3Hz,0dB,fs/1,invert=0 */
|
||
{0x1107, 0x3F800000}, /*3F800000,1107,0dB,invert=0 */
|
||
{0x1108, 0xBF800000}, /*BF800000,1108,0dB,invert=1 */
|
||
{0x1109, 0x3F800000}, /*3F800000,1109,0dB,invert=0 */
|
||
{0x110A, 0x3F800000}, /*3F800000,110A,0dB,invert=0 */
|
||
{0x110B, 0x3F800000}, /*3F800000,110B,0dB,invert=0 */
|
||
{0x110C, 0x3F800000}, /*3F800000,110C,0dB,invert=0 */
|
||
{0x110E, 0x3F800000}, /*3F800000,110E,0dB,invert=0 */
|
||
{0x1110, 0x3DA2AD80}, /*3DA2AD80,1110 */
|
||
{0x1111, 0x00000000}, /*00000000,1111,Free,fs/1,invert=0 */
|
||
{0x1112, 0x3F7FFE00}, /*3F7FFE00,1112,Free,fs/1,invert=0 */
|
||
{0x1113,
|
||
0x3FB26DC0}, /*3FB26DC0,1113,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1114,
|
||
0xBFB00DC0}, /*BFB00DC0,1114,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1115,
|
||
0x3F75E8C0}, /*3F75E8C0,1115,HBF,50Hz,150Hz,3dB,fs/1,invert=0 */
|
||
{0x1116,
|
||
0x3F1B2780}, /*3F1B2780,1116,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1117,
|
||
0xBF1B2400}, /*BF1B2400,1117,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1118,
|
||
0x3F7FFC80}, /*3F7FFC80,1118,LBF,0.2Hz,0.33Hz,0dB,fs/1,invert=0 */
|
||
{0x1119, 0x3F800000}, /*3F800000,1119,Through,0dB,fs/1,invert=0 */
|
||
{0x111A, 0x00000000}, /*00000000,111A,Through,0dB,fs/1,invert=0 */
|
||
{0x111B, 0x00000000}, /*00000000,111B,Through,0dB,fs/1,invert=0 */
|
||
{0x111C, 0x3F800000}, /*3F800000,111C,0dB,invert=0 */
|
||
{0x111D, 0x00000000}, /*00000000,111D,Cutoff,invert=0 */
|
||
{0x111E, 0x3F800000}, /*3F800000,111E,0dB,invert=0 */
|
||
{0x1120, 0x3F800000}, /*3F800000,1120,0dB,invert=0 */
|
||
{0x1121, 0x3F800000}, /*3F800000,1121,0dB,invert=0 */
|
||
{0x1122, 0x3F800000}, /*3F800000,1122,0dB,invert=0 */
|
||
{0x1123, 0x3F800000}, /*3F800000,1123,Through,0dB,fs/1,invert=0 */
|
||
{0x1124, 0x00000000}, /*00000000,1124,Through,0dB,fs/1,invert=0 */
|
||
{0x1125, 0x00000000}, /*00000000,1125,Through,0dB,fs/1,invert=0 */
|
||
{0x1126, 0x00000000}, /*00000000,1126,Through,0dB,fs/1,invert=0 */
|
||
{0x1127, 0x00000000}, /*00000000,1127,Through,0dB,fs/1,invert=0 */
|
||
{0x1130, 0x3F800000}, /*3F800000,1130,Through,0dB,fs/1,invert=0 */
|
||
{0x1131, 0x00000000}, /*00000000,1131,Through,0dB,fs/1,invert=0 */
|
||
{0x1132, 0x00000000}, /*00000000,1132,Through,0dB,fs/1,invert=0 */
|
||
{0x1133, 0x3F800000}, /*3F800000,1133,Through,0dB,fs/1,invert=0 */
|
||
{0x1134, 0x00000000}, /*00000000,1134,Through,0dB,fs/1,invert=0 */
|
||
{0x1135, 0x00000000}, /*00000000,1135,Through,0dB,fs/1,invert=0 */
|
||
{0x1136, 0x3F800000}, /*3F800000,1136,Through,0dB,fs/1,invert=0 */
|
||
{0x1137, 0x00000000}, /*00000000,1137,Through,0dB,fs/1,invert=0 */
|
||
{0x1138, 0x00000000}, /*00000000,1138,Through,0dB,fs/1,invert=0 */
|
||
{0x1139, 0x3F800000}, /*3F800000,1139,Through,0dB,fs/1,invert=0 */
|
||
{0x113A, 0x00000000}, /*00000000,113A,Through,0dB,fs/1,invert=0 */
|
||
{0x113B, 0x00000000}, /*00000000,113B,Through,0dB,fs/1,invert=0 */
|
||
{0x113C, 0x3F800000}, /*3F800000,113C,Through,0dB,fs/1,invert=0 */
|
||
{0x113D, 0x00000000}, /*00000000,113D,Through,0dB,fs/1,invert=0 */
|
||
{0x113E, 0x00000000}, /*00000000,113E,Through,0dB,fs/1,invert=0 */
|
||
{0x1143, 0x39D2BD40}, /*39D2BD40,1143,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1144, 0x39D2BD40}, /*39D2BD40,1144,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1145, 0x3F7FCB40}, /*3F7FCB40,1145,LPF,3Hz,0dB,fs/1,invert=0 */
|
||
{0x1146, 0x388C8A40}, /*388C8A40,1146,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1147, 0x388C8A40}, /*388C8A40,1147,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1148, 0x3F7FF740}, /*3F7FF740,1148,LPF,0.5Hz,0dB,fs/1,invert=0 */
|
||
{0x1149, 0x390C87C0}, /*390C87C0,1149,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x114A, 0x390C87C0}, /*390C87C0,114A,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x114B, 0x3F7FEE80}, /*3F7FEE80,114B,LPF,1Hz,0dB,fs/1,invert=0 */
|
||
{0x114C, 0x398C8300}, /*398C8300,114C,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x114D, 0x398C8300}, /*398C8300,114D,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x114E, 0x3F7FDCC0}, /*3F7FDCC0,114E,LPF,2Hz,0dB,fs/1,invert=0 */
|
||
{0x1153, 0x3F800000}, /*3F800000,1153,Through,0dB,fs/1,invert=0 */
|
||
{0x1154, 0x00000000}, /*00000000,1154,Through,0dB,fs/1,invert=0 */
|
||
{0x1155, 0x00000000}, /*00000000,1155,Through,0dB,fs/1,invert=0 */
|
||
{0x1156, 0x3F800000}, /*3F800000,1156,Through,0dB,fs/1,invert=0 */
|
||
{0x1157, 0x00000000}, /*00000000,1157,Through,0dB,fs/1,invert=0 */
|
||
{0x1158, 0x00000000}, /*00000000,1158,Through,0dB,fs/1,invert=0 */
|
||
{0x1159, 0x3F800000}, /*3F800000,1159,Through,0dB,fs/1,invert=0 */
|
||
{0x115A, 0x00000000}, /*00000000,115A,Through,0dB,fs/1,invert=0 */
|
||
{0x115B, 0x00000000}, /*00000000,115B,Through,0dB,fs/1,invert=0 */
|
||
{0x115C, 0x3F800000}, /*3F800000,115C,Through,0dB,fs/1,invert=0 */
|
||
{0x115D, 0x00000000}, /*00000000,115D,Through,0dB,fs/1,invert=0 */
|
||
{0x115E, 0x00000000}, /*00000000,115E,Through,0dB,fs/1,invert=0 */
|
||
{0x1163, 0x3F800000}, /*3F800000,1163,0dB,invert=0 */
|
||
{0x1166, 0x3F800000}, /*3F800000,1166,0dB,invert=0 */
|
||
{0x1169, 0x3F800000}, /*3F800000,1169,0dB,invert=0 */
|
||
{0x116C, 0x3F800000}, /*3F800000,116C,0dB,invert=0 */
|
||
{0x1173, 0x00000000}, /*00000000,1173,Cutoff,invert=0 */
|
||
{0x1176, 0x3F800000}, /*3F800000,1176,0dB,invert=0 */
|
||
{0x1179, 0x3F800000}, /*3F800000,1179,0dB,invert=0 */
|
||
{0x117C, 0x3F800000}, /*3F800000,117C,0dB,invert=0 */
|
||
{0x1183, 0x38D1B700}, /*38D1B700,1183,-80dB,invert=0 */
|
||
{0x1186, 0x00000000}, /*00000000,1186,Cutoff,invert=0 */
|
||
{0x1189, 0x00000000}, /*00000000,1189,Cutoff,invert=0 */
|
||
{0x118C, 0x00000000}, /*00000000,118C,Cutoff,invert=0 */
|
||
{0x1193, 0x00000000}, /*00000000,1193,Cutoff,invert=0 */
|
||
{0x1198, 0x3F800000}, /*3F800000,1198,0dB,invert=0 */
|
||
{0x1199, 0x3F800000}, /*3F800000,1199,0dB,invert=0 */
|
||
{0x119A, 0x3F800000}, /*3F800000,119A,0dB,invert=0 */
|
||
{0x11A1, 0x3C58B440}, /*3C58B440,11A1,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A2, 0x3C58B440}, /*3C58B440,11A2,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A3, 0x3F793A40}, /*3F793A40,11A3,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A4, 0x3C58B440}, /*3C58B440,11A4,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A5, 0x3C58B440}, /*3C58B440,11A5,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A6, 0x3F793A40}, /*3F793A40,11A6,LPF,100Hz,0dB,fs/1,invert=0 */
|
||
{0x11A7, 0x3F800000}, /*3F800000,11A7,Through,0dB,fs/1,invert=0 */
|
||
{0x11A8, 0x00000000}, /*00000000,11A8,Through,0dB,fs/1,invert=0 */
|
||
{0x11A9, 0x00000000}, /*00000000,11A9,Through,0dB,fs/1,invert=0 */
|
||
{0x11AA, 0x00000000}, /*00000000,11AA,Cutoff,invert=0 */
|
||
{0x11AB, 0x3BDA2580}, /*3BDA2580,11AB,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x11AC, 0x3BDA2580}, /*3BDA2580,11AC,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x11AD, 0x3F7C9780}, /*3F7C9780,11AD,LPF,50Hz,0dB,fs/1,invert=0 */
|
||
{0x11B0, 0x3DD17800}, /*3DD17800,11B0,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x11B1, 0x3DD17800}, /*3DD17800,11B1,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x11B2, 0x3F4BA200}, /*3F4BA200,11B2,LPF,850Hz,0dB,fs/1,invert=0 */
|
||
{0x11B3, 0x3F800000}, /*3F800000,11B3,0dB,invert=0 */
|
||
{0x11B4, 0x00000000}, /*00000000,11B4,Cutoff,invert=0 */
|
||
{0x11B5, 0x00000000}, /*00000000,11B5,Cutoff,invert=0 */
|
||
{0x11B6, 0x3F353C00}, /*3F353C00,11B6,-3dB,invert=0 */
|
||
{0x11B8, 0x3F800000}, /*3F800000,11B8,0dB,invert=0 */
|
||
{0x11B9, 0x00000000}, /*00000000,11B9,Cutoff,invert=0 */
|
||
{0x11C0,
|
||
0x3F915680}, /*3F915680,11C0,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x11C1,
|
||
0xBF8E4100}, /*BF8E4100,11C1,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x11C2,
|
||
0x3F48E240}, /*3F48E240,11C2,HBF,80Hz,900Hz,2dB,fs/1,invert=0 */
|
||
{0x11C3,
|
||
0x3FA04B40}, /*3FA04B40,11C3,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x11C4,
|
||
0xBF9C0DC0}, /*BF9C0DC0,11C4,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x11C5,
|
||
0x3F7691C0}, /*3F7691C0,11C5,HBF,100Hz,140Hz,2dB,fs/1,invert=0 */
|
||
{0x11C6, 0x3D506F00}, /*3D506F00,11C6,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x11C7, 0x3D506F00}, /*3D506F00,11C7,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x11C8, 0x3F65F240}, /*3F65F240,11C8,LPF,400Hz,0dB,fs/1,invert=0 */
|
||
{0x11C9, 0x3C208400}, /*3C208400,11C9,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x11CA, 0x3C208400}, /*3C208400,11CA,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x11CB, 0x3F7FE940}, /*3F7FE940,11CB,LPF,1.3Hz,35dB,fs/1,invert=0 */
|
||
{0x11CC,
|
||
0x3E196280}, /*3E196280,11CC,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x11CD,
|
||
0xBE17BF80}, /*BE17BF80,11CD,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x11CE,
|
||
0x3F7EF900}, /*3F7EF900,11CE,LBF,15Hz,40Hz,-8dB,fs/1,invert=0 */
|
||
{0x11D0, 0x3FFF64C0}, /*3FFF64C0,11D0,6dB,invert=0 */
|
||
{0x11D1, 0x00000000}, /*00000000,11D1,Cutoff,invert=0 */
|
||
{0x11D2, 0x3F800000}, /*3F800000,11D2,0dB,invert=0 */
|
||
{0x11D3, 0x3F800000}, /*3F800000,11D3,0dB,invert=0 */
|
||
{0x11D4, 0x3F800000}, /*3F800000,11D4,0dB,invert=0 */
|
||
{0x11D5, 0x3F800000}, /*3F800000,11D5,0dB,invert=0 */
|
||
{0x11D7, 0x41618840}, /*41618840,11D7,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x11D8, 0x41618840}, /*41618840,11D8,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x11D9, 0x3DDE3840}, /*3DDE3840,11D9,LPF,6000Hz,30dB,fs/1,invert=0 */
|
||
{0x11DA,
|
||
0x3F672280}, /*3F672280,11DA,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x11DB,
|
||
0xBFD3E1C0}, /*BFD3E1C0,11DB,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x11DC,
|
||
0x3FD3E1C0}, /*3FD3E1C0,11DC,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x11DD,
|
||
0x3F5022C0}, /*3F5022C0,11DD,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x11DE,
|
||
0xBF374540}, /*BF374540,11DE,PKF,1000Hz,-10dB,3,fs/1,invert=0 */
|
||
{0x11E0, 0x3F800000}, /*3F800000,11E0,Through,0dB,fs/1,invert=0 */
|
||
{0x11E1, 0x00000000}, /*00000000,11E1,Through,0dB,fs/1,invert=0 */
|
||
{0x11E2, 0x00000000}, /*00000000,11E2,Through,0dB,fs/1,invert=0 */
|
||
{0x11E3, 0x00000000}, /*00000000,11E3,Through,0dB,fs/1,invert=0 */
|
||
{0x11E4, 0x00000000}, /*00000000,11E4,Through,0dB,fs/1,invert=0 */
|
||
{0x11E5, 0x3F800000}, /*3F800000,11E5,0dB,invert=0 */
|
||
{0x11E8, 0x3F800000}, /*3F800000,11E8,0dB,invert=0 */
|
||
{0x11E9, 0x00000000}, /*00000000,11E9,Cutoff,invert=0 */
|
||
{0x11EA, 0x00000000}, /*00000000,11EA,Cutoff,invert=0 */
|
||
{0x11EB, 0x00000000}, /*00000000,11EB,Cutoff,invert=0 */
|
||
{0x11F0, 0x3F800000}, /*3F800000,11F0,Through,0dB,fs/1,invert=0 */
|
||
{0x11F1, 0x00000000}, /*00000000,11F1,Through,0dB,fs/1,invert=0 */
|
||
{0x11F2, 0x00000000}, /*00000000,11F2,Through,0dB,fs/1,invert=0 */
|
||
{0x11F3, 0x00000000}, /*00000000,11F3,Through,0dB,fs/1,invert=0 */
|
||
{0x11F4, 0x00000000}, /*00000000,11F4,Through,0dB,fs/1,invert=0 */
|
||
{0x11F5, 0x3F800000}, /*3F800000,11F5,Through,0dB,fs/1,invert=0 */
|
||
{0x11F6, 0x00000000}, /*00000000,11F6,Through,0dB,fs/1,invert=0 */
|
||
{0x11F7, 0x00000000}, /*00000000,11F7,Through,0dB,fs/1,invert=0 */
|
||
{0x11F8, 0x00000000}, /*00000000,11F8,Through,0dB,fs/1,invert=0 */
|
||
{0x11F9, 0x00000000}, /*00000000,11F9,Through,0dB,fs/1,invert=0 */
|
||
{0x1200, 0x00000000}, /*00000000,1200,Cutoff,invert=0 */
|
||
{0x1201, 0x3F800000}, /*3F800000,1201,0dB,invert=0 */
|
||
{0x1202, 0x3F800000}, /*3F800000,1202,0dB,invert=0 */
|
||
{0x1203, 0x3F800000}, /*3F800000,1203,0dB,invert=0 */
|
||
{0x1204, 0x3F800000}, /*3F800000,1204,Through,0dB,fs/1,invert=0 */
|
||
{0x1205, 0x00000000}, /*00000000,1205,Through,0dB,fs/1,invert=0 */
|
||
{0x1206, 0x00000000}, /*00000000,1206,Through,0dB,fs/1,invert=0 */
|
||
{0x1207, 0x3F800000}, /*3F800000,1207,Through,0dB,fs/1,invert=0 */
|
||
{0x1208, 0x00000000}, /*00000000,1208,Through,0dB,fs/1,invert=0 */
|
||
{0x1209, 0x00000000}, /*00000000,1209,Through,0dB,fs/1,invert=0 */
|
||
{0x120A, 0x3F800000}, /*3F800000,120A,Through,0dB,fs/1,invert=0 */
|
||
{0x120B, 0x00000000}, /*00000000,120B,Through,0dB,fs/1,invert=0 */
|
||
{0x120C, 0x00000000}, /*00000000,120C,Through,0dB,fs/1,invert=0 */
|
||
{0x120D, 0x3F800000}, /*3F800000,120D,Through,0dB,fs/1,invert=0 */
|
||
{0x120E, 0x00000000}, /*00000000,120E,Through,0dB,fs/1,invert=0 */
|
||
{0x120F, 0x00000000}, /*00000000,120F,Through,0dB,fs/1,invert=0 */
|
||
{0x1210, 0x3F800000}, /*3F800000,1210,Through,0dB,fs/1,invert=0 */
|
||
{0x1211, 0x00000000}, /*00000000,1211,Through,0dB,fs/1,invert=0 */
|
||
{0x1212, 0x00000000}, /*00000000,1212,Through,0dB,fs/1,invert=0 */
|
||
{0x1213, 0x3F800000}, /*3F800000,1213,0dB,invert=0 */
|
||
{0x1214, 0x3F800000}, /*3F800000,1214,0dB,invert=0 */
|
||
{0x1215, 0x3F800000}, /*3F800000,1215,0dB,invert=0 */
|
||
{0x1216, 0x3F800000}, /*3F800000,1216,0dB,invert=0 */
|
||
{0x1217, 0x3F800000}, /*3F800000,1217,0dB,invert=0 */
|
||
{0x1218, 0x00000000}, /*00000000,1218,Cutoff,fs/1,invert=0 */
|
||
{0x1219, 0x00000000}, /*00000000,1219,Cutoff,fs/1,invert=0 */
|
||
{0x121A, 0x00000000}, /*00000000,121A,Cutoff,fs/1,invert=0 */
|
||
{0x121B, 0x00000000}, /*00000000,121B,Cutoff,fs/1,invert=0 */
|
||
{0x121C, 0x00000000}, /*00000000,121C,Cutoff,fs/1,invert=0 */
|
||
{0x121D, 0x3F800000}, /*3F800000,121D,0dB,invert=0 */
|
||
{0x121E, 0x3F800000}, /*3F800000,121E,0dB,invert=0 */
|
||
{0x121F, 0x3F800000}, /*3F800000,121F,0dB,invert=0 */
|
||
{0x1235, 0x3F800000}, /*3F800000,1235,0dB,invert=0 */
|
||
{0x1236, 0x3F800000}, /*3F800000,1236,0dB,invert=0 */
|
||
{0x1237, 0x3F800000}, /*3F800000,1237,0dB,invert=0 */
|
||
{0x1238, 0x3F800000}, /*3F800000,1238,0dB,invert=0 */
|
||
{0xFFFF, 0xFFFFFFFF} };
|
||
|
||
void IniSet(void)
|
||
{
|
||
/* Command Execute Process Initial */
|
||
IniCmd();
|
||
/* Clock Setting */
|
||
IniClk();
|
||
/* I/O Port Initial Setting */
|
||
IniIop();
|
||
/* DigitalGyro Initial Setting */
|
||
IniDgy();
|
||
/* Monitor & Other Initial Setting */
|
||
IniMon();
|
||
/* Servo Initial Setting */
|
||
IniSrv();
|
||
/* Gyro Filter Initial Setting */
|
||
IniGyr();
|
||
/* Gyro Filter Initial Setting */
|
||
IniFil();
|
||
/* Adjust Fix Value Setting */
|
||
IniAdj();
|
||
}
|
||
|
||
void IniSetAf(void)
|
||
{
|
||
/* Command Execute Process Initial */
|
||
IniCmd();
|
||
/* Clock Setting */
|
||
IniClk();
|
||
/* AF Initial Setting */
|
||
IniAf();
|
||
}
|
||
|
||
void IniClk(void)
|
||
{
|
||
ChkCvr(); /* Read Cver */
|
||
|
||
/*OSC Enables */
|
||
UcOscAdjFlg = 0; /* Osc adj flag */
|
||
|
||
#ifdef DEF_SET
|
||
/*OSC ENABLE */
|
||
RegWriteA_LC898122AF(OSCSTOP, 0x00); /* 0x0256 */
|
||
RegWriteA_LC898122AF(OSCSET, 0x90); /* 0x0257 OSC ini */
|
||
RegWriteA_LC898122AF(OSCCNTEN, 0x00); /* 0x0258 OSC Cnt disable */
|
||
#endif
|
||
/*Clock Enables */
|
||
RegWriteA_LC898122AF(CLKON, 0x1F); /* 0x020B */
|
||
|
||
#ifdef USE_EXTCLK_ALL
|
||
RegWriteA_LC898122AF(CLKSEL, 0x07); /* 0x020C All */
|
||
#else
|
||
#ifdef USE_EXTCLK_PWM
|
||
RegWriteA_LC898122AF(CLKSEL, 0x01); /* 0x020C only PWM */
|
||
#else
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(CLKSEL, 0x00); /* 0x020C */
|
||
#endif
|
||
#endif
|
||
#endif
|
||
|
||
#ifdef USE_EXTCLK_ALL /* 24MHz */
|
||
RegWriteA_LC898122AF(PWMDIV, 0x00); /* 0x0210 24MHz/1 */
|
||
RegWriteA_LC898122AF(SRVDIV, 0x00); /* 0x0211 24MHz/1 */
|
||
RegWriteA_LC898122AF(GIFDIV, 0x02); /* 0x0212 24MHz/2 = 12MHz */
|
||
RegWriteA_LC898122AF(AFPWMDIV, 0x00); /* 0x0213 24MHz/1 = 24MHz */
|
||
RegWriteA_LC898122AF(OPAFDIV, 0x02); /* 0x0214 24MHz/2 = 12MHz */
|
||
#else
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(PWMDIV, 0x00); /* 0x0210 48MHz/1 */
|
||
RegWriteA_LC898122AF(SRVDIV, 0x00); /* 0x0211 48MHz/1 */
|
||
RegWriteA_LC898122AF(GIFDIV, 0x03); /* 0x0212 48MHz/3 = 16MHz */
|
||
#ifdef AF_PWMMODE
|
||
RegWriteA_LC898122AF(AFPWMDIV, 0x00); /* 0x0213 48MHz/1 */
|
||
#else
|
||
RegWriteA_LC898122AF(AFPWMDIV, 0x02); /* 0x0213 48MHz/2 = 24MHz */
|
||
#endif
|
||
RegWriteA_LC898122AF(OPAFDIV, 0x04); /* 0x0214 48MHz/4 = 12MHz */
|
||
#endif
|
||
#endif
|
||
}
|
||
|
||
void IniIop(void)
|
||
{
|
||
#ifdef DEF_SET
|
||
/*set IOP direction */
|
||
RegWriteA_LC898122AF(P0LEV, 0x00);
|
||
RegWriteA_LC898122AF(P0DIR, 0x00);
|
||
/*set pull up/down */
|
||
RegWriteA_LC898122AF(P0PON, 0x0F);
|
||
RegWriteA_LC898122AF(P0PUD, 0x0F);
|
||
#endif
|
||
/*select IOP signal */
|
||
#ifdef USE_3WIRE_DGYRO
|
||
RegWriteA_LC898122AF(IOP1SEL, 0x02); /* 0x0231 IOP1 : IOP1 */
|
||
#else
|
||
RegWriteA_LC898122AF(
|
||
IOP1SEL,
|
||
0x00); /* 0x0231 IOP1 : DGDATAIN (ATT:0236h[0]=1) */
|
||
#endif
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(IOP0SEL, 0x02); /* 0x0230 IOP0 : IOP0 */
|
||
RegWriteA_LC898122AF(IOP2SEL, 0x02); /* 0x0232 IOP2 : IOP2 */
|
||
RegWriteA_LC898122AF(IOP3SEL, 0x00); /* 0x0233 IOP3 : DGDATAOUT */
|
||
RegWriteA_LC898122AF(IOP4SEL, 0x00); /* 0x0234 IOP4 : DGSCLK */
|
||
RegWriteA_LC898122AF(IOP5SEL, 0x00); /* 0x0235 IOP5 : DGSSB */
|
||
RegWriteA_LC898122AF(DGINSEL,
|
||
0x00); /* 0x0236 DGDATAIN 0:IOP1 1:IOP2 */
|
||
RegWriteA_LC898122AF(I2CSEL,
|
||
0x00); /* 0x0248 I2C noise reduction ON */
|
||
RegWriteA_LC898122AF(DLMODE, 0x00); /* 0x0249 Download OFF */
|
||
#endif
|
||
}
|
||
|
||
void IniDgy(void)
|
||
{
|
||
#ifdef USE_INVENSENSE
|
||
unsigned char UcGrini;
|
||
#endif
|
||
|
||
/*************/
|
||
/*For ST gyro */
|
||
/*************/
|
||
|
||
/*Set SPI Type */
|
||
#ifdef USE_3WIRE_DGYRO
|
||
RegWriteA_LC898122AF(SPIM, 0x00);
|
||
#else
|
||
RegWriteA_LC898122AF(SPIM, 0x01);
|
||
#endif
|
||
/* DGSPI4 0: 3-wire SPI, 1: 4-wire SPI */
|
||
|
||
/*Set to Command Mode */
|
||
RegWriteA_LC898122AF(GRSEL, 0x01);
|
||
|
||
/*Digital Gyro Read settings */
|
||
RegWriteA_LC898122AF(GRINI, 0x80);
|
||
|
||
#ifdef USE_INVENSENSE
|
||
|
||
RegReadA_LC898122AF(GRINI, &UcGrini);
|
||
RegWriteA_LC898122AF(GRINI, (UcGrini | SLOWMODE));
|
||
|
||
RegWriteA_LC898122AF(GRADR0, 0x6A); /* 0x0283 Set I2C_DIS */
|
||
RegWriteA_LC898122AF(GSETDT, 0x10); /* 0x028A Set Write Data */
|
||
RegWriteA_LC898122AF(GRACC, 0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10);
|
||
|
||
RegWriteA_LC898122AF(GRADR0, 0x1B); /* 0x0283 Set GYRO_CONFIG */
|
||
RegWriteA_LC898122AF(GSETDT,
|
||
(FS_SEL << 3)); /* 0x028A Set Write Data */
|
||
RegWriteA_LC898122AF(GRACC, 0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
|
||
RegReadA_LC898122AF(GRINI, &UcGrini);
|
||
RegWriteA_LC898122AF(GRINI, (UcGrini & ~SLOWMODE));
|
||
/* 0x0281 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ - | SLOWMODE | - | - ] */
|
||
|
||
#endif
|
||
|
||
RegWriteA_LC898122AF(
|
||
RDSEL,
|
||
0x7C); /* 0x028B RDSEL(Data1 and 2 for continuos mode) */
|
||
|
||
GyOutSignal();
|
||
}
|
||
|
||
void IniMon(void)
|
||
{
|
||
RegWriteA_LC898122AF(PWMMONA, 0x00); /* 0x0030 0:off */
|
||
|
||
RegWriteA_LC898122AF(MONSELA, 0x5C); /* 0x0270 DLYMON1 */
|
||
RegWriteA_LC898122AF(MONSELB, 0x5D); /* 0x0271 DLYMON2 */
|
||
RegWriteA_LC898122AF(MONSELC, 0x00); /* 0x0272 */
|
||
RegWriteA_LC898122AF(MONSELD, 0x00); /* 0x0273 */
|
||
|
||
/* Monitor Circuit */
|
||
RegWriteA_LC898122AF(WC_PINMON1,
|
||
0x00); /* 0x01C0 Filter Monitor */
|
||
RegWriteA_LC898122AF(WC_PINMON2, 0x00); /* 0x01C1 */
|
||
RegWriteA_LC898122AF(WC_PINMON3, 0x00); /* 0x01C2 */
|
||
RegWriteA_LC898122AF(WC_PINMON4, 0x00); /* 0x01C3 */
|
||
/* Delay Monitor */
|
||
RegWriteA_LC898122AF(WC_DLYMON11, 0x04); /* 0x01C5 DlyMonAdd1[10:8] */
|
||
RegWriteA_LC898122AF(WC_DLYMON10, 0x40); /* 0x01C4 DlyMonAdd1[ 7:0] */
|
||
RegWriteA_LC898122AF(WC_DLYMON21, 0x04); /* 0x01C7 DlyMonAdd2[10:8] */
|
||
RegWriteA_LC898122AF(WC_DLYMON20, 0xC0); /* 0x01C6 DlyMonAdd2[ 7:0] */
|
||
RegWriteA_LC898122AF(WC_DLYMON31, 0x00); /* 0x01C9 DlyMonAdd3[10:8] */
|
||
RegWriteA_LC898122AF(WC_DLYMON30, 0x00); /* 0x01C8 DlyMonAdd3[ 7:0] */
|
||
RegWriteA_LC898122AF(WC_DLYMON41, 0x00); /* 0x01CB DlyMonAdd4[10:8] */
|
||
RegWriteA_LC898122AF(WC_DLYMON40, 0x00); /* 0x01CA DlyMonAdd4[ 7:0] */
|
||
|
||
/* Monitor */
|
||
RegWriteA_LC898122AF(PWMMONA, 0x80); /* 0x0030 1:on */
|
||
/**/}
|
||
|
||
|
||
void IniSrv(void)
|
||
{
|
||
unsigned char UcStbb0;
|
||
|
||
UcPwmMod = INIT_PWMMODE; /* Driver output mode */
|
||
|
||
RegWriteA_LC898122AF(WC_EQON,
|
||
0x00); /* 0x0101 Filter Calcu */
|
||
RegWriteA_LC898122AF(WC_RAMINITON, 0x00); /* 0x0102 */
|
||
ClrGyr(0x0000, CLR_ALL_RAM); /* All Clear */
|
||
|
||
RegWriteA_LC898122AF(WH_EQSWX, 0x02);
|
||
RegWriteA_LC898122AF(WH_EQSWY, 0x02);
|
||
|
||
RamAccFixMod(OFF); /* 32bit Float mode */
|
||
|
||
/* Monitor Gain */
|
||
RamWrite32A_LC898122AF(dm1g, 0x3F800000); /* 0x109A */
|
||
RamWrite32A_LC898122AF(dm2g, 0x3F800000); /* 0x109B */
|
||
RamWrite32A_LC898122AF(dm3g, 0x3F800000); /* 0x119A */
|
||
RamWrite32A_LC898122AF(dm4g, 0x3F800000); /* 0x119B */
|
||
|
||
/* Hall output limitter */
|
||
RamWrite32A_LC898122AF(
|
||
sxlmta1,
|
||
0x3F800000); /* 0x10E6 Hall X output Limit */
|
||
RamWrite32A_LC898122AF(
|
||
sylmta1,
|
||
0x3F800000); /* 0x11E6 Hall Y output Limit */
|
||
|
||
/* Emargency Stop */
|
||
RegWriteA_LC898122AF(
|
||
WH_EMGSTPON,
|
||
0x00); /* 0x0178 Emargency Stop OFF */
|
||
RegWriteA_LC898122AF(WH_EMGSTPTMR,
|
||
0xFF); /* 0x017A 255*(16/23.4375kHz)=174ms */
|
||
|
||
RamWrite32A_LC898122AF(sxemglev, 0x3F800000);
|
||
RamWrite32A_LC898122AF(syemglev, 0x3F800000);
|
||
|
||
/* Hall Servo smoothing */
|
||
RegWriteA_LC898122AF(WH_SMTSRVON,
|
||
0x00); /* 0x017C Smooth Servo OFF */
|
||
#ifdef USE_EXTCLK_ALL /* 24MHz */
|
||
RegWriteA_LC898122AF(WH_SMTSRVSMP,
|
||
0x03); /* 0x017D 2.7ms=2^03/11.718kHz */
|
||
RegWriteA_LC898122AF(WH_SMTTMR,
|
||
0x00); /* 0x017E 1.3ms=(0+1)*16/11.718kHz */
|
||
#else
|
||
RegWriteA_LC898122AF(WH_SMTSRVSMP,
|
||
0x06); /* 0x017D 2.7ms=2^06/23.4375kHz */
|
||
RegWriteA_LC898122AF(WH_SMTTMR,
|
||
0x01); /* 0x017E 1.3ms=(1+1)*16/23.4375kHz */
|
||
#endif
|
||
|
||
RamWrite32A_LC898122AF(sxsmtav, 0xBC800000);
|
||
RamWrite32A_LC898122AF(sysmtav, 0xBC800000);
|
||
RamWrite32A_LC898122AF(sxsmtstp, 0x3AE90466);
|
||
RamWrite32A_LC898122AF(sysmtstp, 0x3AE90466);
|
||
|
||
/* High-dimensional correction */
|
||
RegWriteA_LC898122AF(WH_HOFCON,
|
||
0x11); /* 0x0174 OUT 3x3 */
|
||
|
||
/* Front */
|
||
RamWrite32A_LC898122AF(sxiexp3, A3_IEXP3); /* 0x10BA */
|
||
RamWrite32A_LC898122AF(sxiexp2, 0x00000000); /* 0x10BB */
|
||
RamWrite32A_LC898122AF(sxiexp1, A1_IEXP1); /* 0x10BC */
|
||
RamWrite32A_LC898122AF(sxiexp0, 0x00000000); /* 0x10BD */
|
||
RamWrite32A_LC898122AF(sxiexp, 0x3F800000); /* 0x10BE */
|
||
|
||
RamWrite32A_LC898122AF(syiexp3, A3_IEXP3); /* 0x11BA */
|
||
RamWrite32A_LC898122AF(syiexp2, 0x00000000); /* 0x11BB */
|
||
RamWrite32A_LC898122AF(syiexp1, A1_IEXP1); /* 0x11BC */
|
||
RamWrite32A_LC898122AF(syiexp0, 0x00000000); /* 0x11BD */
|
||
RamWrite32A_LC898122AF(syiexp, 0x3F800000); /* 0x11BE */
|
||
|
||
/* Back */
|
||
RamWrite32A_LC898122AF(sxoexp3, A3_IEXP3); /* 0x10FA */
|
||
RamWrite32A_LC898122AF(sxoexp2, 0x00000000); /* 0x10FB */
|
||
RamWrite32A_LC898122AF(sxoexp1, A1_IEXP1); /* 0x10FC */
|
||
RamWrite32A_LC898122AF(sxoexp0, 0x00000000); /* 0x10FD */
|
||
RamWrite32A_LC898122AF(sxoexp, 0x3F800000); /* 0x10FE */
|
||
|
||
RamWrite32A_LC898122AF(syoexp3, A3_IEXP3); /* 0x11FA */
|
||
RamWrite32A_LC898122AF(syoexp2, 0x00000000); /* 0x11FB */
|
||
RamWrite32A_LC898122AF(syoexp1, A1_IEXP1); /* 0x11FC */
|
||
RamWrite32A_LC898122AF(syoexp0, 0x00000000); /* 0x11FD */
|
||
RamWrite32A_LC898122AF(syoexp, 0x3F800000); /* 0x11FE */
|
||
|
||
/* Sine wave */
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(WC_SINON,
|
||
0x00); /* 0x0180 Sin Wave off */
|
||
RegWriteA_LC898122AF(WC_SINFRQ0, 0x00); /* 0x0181 */
|
||
RegWriteA_LC898122AF(WC_SINFRQ1, 0x60); /* 0x0182 */
|
||
RegWriteA_LC898122AF(WC_SINPHSX, 0x00); /* 0x0183 */
|
||
RegWriteA_LC898122AF(WC_SINPHSY, 0x20); /* 0x0184 */
|
||
|
||
/* AD over sampling */
|
||
RegWriteA_LC898122AF(WC_ADMODE,
|
||
0x06); /* 0x0188 AD Over Sampling */
|
||
|
||
/* Measure mode */
|
||
RegWriteA_LC898122AF(WC_MESMODE, 0x00); /* 0x0190 Measurement Mode */
|
||
RegWriteA_LC898122AF(WC_MESSINMODE, 0x00); /* 0x0191 */
|
||
RegWriteA_LC898122AF(WC_MESLOOP0, 0x08); /* 0x0192 */
|
||
RegWriteA_LC898122AF(WC_MESLOOP1, 0x02); /* 0x0193 */
|
||
RegWriteA_LC898122AF(WC_MES1ADD0, 0x00); /* 0x0194 */
|
||
RegWriteA_LC898122AF(WC_MES1ADD1, 0x00); /* 0x0195 */
|
||
RegWriteA_LC898122AF(WC_MES2ADD0, 0x00); /* 0x0196 */
|
||
RegWriteA_LC898122AF(WC_MES2ADD1, 0x00); /* 0x0197 */
|
||
RegWriteA_LC898122AF(WC_MESABS, 0x00); /* 0x0198 */
|
||
RegWriteA_LC898122AF(WC_MESWAIT, 0x00); /* 0x0199 */
|
||
|
||
/* auto measure */
|
||
RegWriteA_LC898122AF(
|
||
WC_AMJMODE,
|
||
0x00); /* 0x01A0 Automatic measurement mode */
|
||
|
||
RegWriteA_LC898122AF(WC_AMJLOOP0, 0x08); /* 0x01A2 Self-Aadjustment */
|
||
RegWriteA_LC898122AF(WC_AMJLOOP1, 0x02); /* 0x01A3 */
|
||
RegWriteA_LC898122AF(WC_AMJIDL0, 0x02); /* 0x01A4 */
|
||
RegWriteA_LC898122AF(WC_AMJIDL1, 0x00); /* 0x01A5 */
|
||
RegWriteA_LC898122AF(WC_AMJ1ADD0, 0x00); /* 0x01A6 */
|
||
RegWriteA_LC898122AF(WC_AMJ1ADD1, 0x00); /* 0x01A7 */
|
||
RegWriteA_LC898122AF(WC_AMJ2ADD0, 0x00); /* 0x01A8 */
|
||
RegWriteA_LC898122AF(WC_AMJ2ADD1, 0x00); /* 0x01A9 */
|
||
|
||
/* Data Pass */
|
||
RegWriteA_LC898122AF(WC_DPI1ADD0,
|
||
0x00); /* 0x01B0 Data Pass */
|
||
RegWriteA_LC898122AF(WC_DPI1ADD1, 0x00); /* 0x01B1 */
|
||
RegWriteA_LC898122AF(WC_DPI2ADD0, 0x00); /* 0x01B2 */
|
||
RegWriteA_LC898122AF(WC_DPI2ADD1, 0x00); /* 0x01B3 */
|
||
RegWriteA_LC898122AF(WC_DPI3ADD0, 0x00); /* 0x01B4 */
|
||
RegWriteA_LC898122AF(WC_DPI3ADD1, 0x00); /* 0x01B5 */
|
||
RegWriteA_LC898122AF(WC_DPI4ADD0, 0x00); /* 0x01B6 */
|
||
RegWriteA_LC898122AF(WC_DPI4ADD1, 0x00); /* 0x01B7 */
|
||
RegWriteA_LC898122AF(WC_DPO1ADD0,
|
||
0x00); /* 0x01B8 Data Pass */
|
||
RegWriteA_LC898122AF(WC_DPO1ADD1, 0x00); /* 0x01B9 */
|
||
RegWriteA_LC898122AF(WC_DPO2ADD0, 0x00); /* 0x01BA */
|
||
RegWriteA_LC898122AF(WC_DPO2ADD1, 0x00); /* 0x01BB */
|
||
RegWriteA_LC898122AF(WC_DPO3ADD0, 0x00); /* 0x01BC */
|
||
RegWriteA_LC898122AF(WC_DPO3ADD1, 0x00); /* 0x01BD */
|
||
RegWriteA_LC898122AF(WC_DPO4ADD0, 0x00); /* 0x01BE */
|
||
RegWriteA_LC898122AF(WC_DPO4ADD1, 0x00); /* 0x01BF */
|
||
RegWriteA_LC898122AF(WC_DPON,
|
||
0x00); /* 0x0105 Data pass OFF */
|
||
|
||
/* Interrupt Flag */
|
||
RegWriteA_LC898122AF(WC_INTMSK,
|
||
0xFF); /* 0x01CE All Mask */
|
||
|
||
#endif
|
||
|
||
/* Ram Access */
|
||
RamAccFixMod(OFF); /* 32bit float mode */
|
||
|
||
/* PWM Signal Generate */
|
||
DrvSw(OFF); /* 0x0070 Drvier Block Ena=0 */
|
||
RegWriteA_LC898122AF(
|
||
DRVFC2, 0x90); /* 0x0002 Slope 3, Dead Time = 30 ns */
|
||
RegWriteA_LC898122AF(
|
||
DRVSELX,
|
||
0xFF); /* 0x0003 PWM X drv max current DRVSELX[7:0] */
|
||
RegWriteA_LC898122AF(
|
||
DRVSELY,
|
||
0xFF); /* 0x0004 PWM Y drv max current DRVSELY[7:0] */
|
||
|
||
#ifdef PWM_BREAK
|
||
#ifdef PWM_CAREER_TEST
|
||
RegWriteA_LC898122AF(PWMFC, 0x7C);
|
||
#else /* PWM_CAREER_TEST */
|
||
if (UcCvrCod == CVER122)
|
||
RegWriteA_LC898122AF(PWMFC, 0x2D);
|
||
else
|
||
RegWriteA_LC898122AF(PWMFC, 0x3D);
|
||
#endif /* PWM_CAREER_TEST */
|
||
#else
|
||
RegWriteA_LC898122AF(PWMFC, 0x21);
|
||
#endif
|
||
|
||
#ifdef USE_VH_SYNC
|
||
RegWriteA_LC898122AF(STROBEFC,
|
||
0x80); /* 0x001C <20>O?<3F><><EFBFBD><EFBFBD>Strobe<62>M?<3F>̗L<CC97><4C> */
|
||
RegWriteA_LC898122AF(STROBEDLYX, 0x00); /* 0x001D Delay */
|
||
RegWriteA_LC898122AF(STROBEDLYY, 0x00); /* 0x001E Delay */
|
||
#endif /* USE_VH_SYNC */
|
||
|
||
RegWriteA_LC898122AF(PWMA, 0x00); /* 0x0010 PWM X/Y standby */
|
||
RegWriteA_LC898122AF(PWMDLYX,
|
||
0x04); /* 0x0012 X Phase Delay Setting */
|
||
RegWriteA_LC898122AF(PWMDLYY,
|
||
0x04); /* 0x0013 Y Phase Delay Setting */
|
||
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(DRVCH1SEL,
|
||
0x00); /* 0x0005 OUT1/OUT2 X axis */
|
||
RegWriteA_LC898122AF(DRVCH2SEL,
|
||
0x00); /* 0x0006 OUT3/OUT4 Y axis */
|
||
|
||
RegWriteA_LC898122AF(PWMDLYTIMX,
|
||
0x00); /* 0x0014 PWM Timing */
|
||
RegWriteA_LC898122AF(PWMDLYTIMY,
|
||
0x00); /* 0x0015 PWM Timing */
|
||
#endif
|
||
|
||
if (UcCvrCod == CVER122) {
|
||
#ifdef PWM_CAREER_TEST
|
||
RegWriteA_LC898122AF(PWMPERIODY, 0xD0);
|
||
RegWriteA_LC898122AF(PWMPERIODY2, 0xD0);
|
||
#else /* PWM_CAREER_TEST */
|
||
RegWriteA_LC898122AF(PWMPERIODY, 0x00);
|
||
RegWriteA_LC898122AF(PWMPERIODY2, 0x00);
|
||
#endif
|
||
} else {
|
||
#ifdef PWM_CAREER_TEST
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODX,
|
||
0xF2); /* 0x0018 PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODX2,
|
||
0x00); /* 0x0019 PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODY,
|
||
0xF2); /* 0x001A PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODY2,
|
||
0x00); /* 0x001B PWM Carrier Freq */
|
||
#else /* PWM_CAREER_TEST */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODX,
|
||
0x00); /* 0x0018 PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODX2,
|
||
0x00); /* 0x0019 PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODY,
|
||
0x00); /* 0x001A PWM Carrier Freq */
|
||
RegWriteA_LC898122AF(
|
||
PWMPERIODY2,
|
||
0x00); /* 0x001B PWM Carrier Freq */
|
||
#endif
|
||
}
|
||
|
||
/* Linear PWM circuit setting */
|
||
RegWriteA_LC898122AF(CVA,
|
||
0xC0); /* 0x0020 Linear PWM mode enable */
|
||
|
||
if (UcCvrCod == CVER122)
|
||
RegWriteA_LC898122AF(CVFC, 0x22); /* 0x0021 */
|
||
|
||
RegWriteA_LC898122AF(CVFC2, 0x80); /* 0x0022 */
|
||
if (UcCvrCod == CVER122) {
|
||
RegWriteA_LC898122AF(CVSMTHX,
|
||
0x00); /* 0x0023 smooth off */
|
||
RegWriteA_LC898122AF(CVSMTHY,
|
||
0x00); /* 0x0024 smooth off */
|
||
}
|
||
|
||
RegReadA_LC898122AF(STBB0, &UcStbb0);
|
||
|
||
UcStbb0 &= 0x80;
|
||
RegWriteA_LC898122AF(STBB0, UcStbb0); /* 0x0250 OIS standby */
|
||
}
|
||
|
||
#ifdef GAIN_CONT
|
||
#define TRI_LEVEL 0x3A031280 /* 0.0005 */
|
||
#define TIMELOW 0x50 /* */
|
||
#define TIMEHGH 0x05 /* */
|
||
#ifdef USE_EXTCLK_ALL /* 24MHz */
|
||
#define TIMEBSE 0x2F /* 4.0ms */
|
||
#else
|
||
#define TIMEBSE 0x5D /* 3.96ms */
|
||
#endif
|
||
#define MONADR GXXFZ
|
||
#define GANADR gxadj
|
||
#define XMINGAIN 0x00000000
|
||
#define XMAXGAIN 0x3F800000
|
||
#define YMINGAIN 0x00000000
|
||
#define YMAXGAIN 0x3F800000
|
||
#define XSTEPUP 0x38D1B717 /* 0.0001 */
|
||
#define XSTEPDN 0xBD4CCCCD /* -0.05 */
|
||
#define YSTEPUP 0x38D1B717 /* 0.0001 */
|
||
#define YSTEPDN 0xBD4CCCCD /* -0.05 */
|
||
#endif
|
||
|
||
void IniGyr(void)
|
||
{
|
||
|
||
/*Gyro Filter Setting */
|
||
RegWriteA_LC898122AF(WG_EQSW, 0x03);
|
||
|
||
/*Gyro Filter Down Sampling */
|
||
|
||
RegWriteA_LC898122AF(WG_SHTON, 0x10);
|
||
/* CmShtOpe[1:0] 00: <20>V?<3F>b<EFBFBD>^<5E>[OFF, 01: <20>V?<3F>b<EFBFBD>^<5E>[ON, 1x:<3A>O?<3F><><EFBFBD><EFBFBD> */
|
||
|
||
#ifdef DEF_SET
|
||
RegWriteA_LC898122AF(WG_SHTDLYTMR, 0x00); /* 0x0117 Shutter Delay */
|
||
RegWriteA_LC898122AF(WG_GADSMP,
|
||
0x00); /* 0x011C Sampling timing */
|
||
RegWriteA_LC898122AF(WG_HCHR, 0x00);
|
||
RegWriteA_LC898122AF(WG_LMT3MOD, 0x00);
|
||
/* CmLmt3Mod 0: <20>ʏ<EFBFBD>?<3F>~<7E>b<EFBFBD>^<5E>[<5B><><EFBFBD><EFBFBD>, 1: <20>~<7E>̔<EFBFBD><CC94>a?<3F>~<7E>b<EFBFBD>^<5E>[<5B><><EFBFBD><EFBFBD> */
|
||
RegWriteA_LC898122AF(WG_VREFADD, 0x12);
|
||
#endif
|
||
RegWriteA_LC898122AF(WG_SHTMOD, 0x06);
|
||
|
||
/* Limiter */
|
||
RamWrite32A_LC898122AF(gxlmt1H, GYRLMT1H); /* 0x1028 */
|
||
RamWrite32A_LC898122AF(gylmt1H, GYRLMT1H); /* 0x1128 */
|
||
|
||
RamWrite32A_LC898122AF(gxlmt3HS0, GYRLMT3_S1); /* 0x1029 */
|
||
RamWrite32A_LC898122AF(gylmt3HS0, GYRLMT3_S1); /* 0x1129 */
|
||
|
||
RamWrite32A_LC898122AF(gxlmt3HS1, GYRLMT3_S2); /* 0x102A */
|
||
RamWrite32A_LC898122AF(gylmt3HS1, GYRLMT3_S2); /* 0x112A */
|
||
|
||
RamWrite32A_LC898122AF(
|
||
gylmt4HS0,
|
||
GYRLMT4_S1); /* 0x112B Y<><59>Limiter4 High?<3F>l0 */
|
||
RamWrite32A_LC898122AF(
|
||
gxlmt4HS0,
|
||
GYRLMT4_S1); /* 0x102B X<><58>Limiter4 High?<3F>l0 */
|
||
|
||
RamWrite32A_LC898122AF(
|
||
gxlmt4HS1,
|
||
GYRLMT4_S2); /* 0x102C X<><58>Limiter4 High?<3F>l1 */
|
||
RamWrite32A_LC898122AF(
|
||
gylmt4HS1,
|
||
GYRLMT4_S2); /* 0x112C Y<><59>Limiter4 High?<3F>l1 */
|
||
|
||
/* Pan/Tilt parameter */
|
||
RegWriteA_LC898122AF(WG_PANADDA,
|
||
0x12); /* 0x0130 GXH1Z2/GYH1Z2 Select */
|
||
RegWriteA_LC898122AF(WG_PANADDB,
|
||
0x09); /* 0x0131 GXIZ/GYIZ Select */
|
||
|
||
/* Threshold */
|
||
RamWrite32A_LC898122AF(SttxHis, 0x00000000); /* 0x1226 */
|
||
RamWrite32A_LC898122AF(SttxaL, 0x00000000); /* 0x109D */
|
||
RamWrite32A_LC898122AF(SttxbL, 0x00000000); /* 0x109E */
|
||
RamWrite32A_LC898122AF(Sttx12aM, GYRA12_MID); /* 0x104F */
|
||
RamWrite32A_LC898122AF(Sttx12aH, GYRA12_HGH); /* 0x105F */
|
||
RamWrite32A_LC898122AF(Sttx12bM, GYRB12_MID); /* 0x106F */
|
||
RamWrite32A_LC898122AF(Sttx12bH, GYRB12_HGH); /* 0x107F */
|
||
RamWrite32A_LC898122AF(Sttx34aM, GYRA34_MID); /* 0x108F */
|
||
RamWrite32A_LC898122AF(Sttx34aH, GYRA34_HGH); /* 0x109F */
|
||
RamWrite32A_LC898122AF(Sttx34bM, GYRB34_MID); /* 0x10AF */
|
||
RamWrite32A_LC898122AF(Sttx34bH, GYRB34_HGH); /* 0x10BF */
|
||
RamWrite32A_LC898122AF(SttyaL, 0x00000000); /* 0x119D */
|
||
RamWrite32A_LC898122AF(SttybL, 0x00000000); /* 0x119E */
|
||
RamWrite32A_LC898122AF(Stty12aM, GYRA12_MID); /* 0x114F */
|
||
RamWrite32A_LC898122AF(Stty12aH, GYRA12_HGH); /* 0x115F */
|
||
RamWrite32A_LC898122AF(Stty12bM, GYRB12_MID); /* 0x116F */
|
||
RamWrite32A_LC898122AF(Stty12bH, GYRB12_HGH); /* 0x117F */
|
||
RamWrite32A_LC898122AF(Stty34aM, GYRA34_MID); /* 0x118F */
|
||
RamWrite32A_LC898122AF(Stty34aH, GYRA34_HGH); /* 0x119F */
|
||
RamWrite32A_LC898122AF(Stty34bM, GYRB34_MID); /* 0x11AF */
|
||
RamWrite32A_LC898122AF(Stty34bH, GYRB34_HGH); /* 0x11BF */
|
||
|
||
/* Pan level */
|
||
RegWriteA_LC898122AF(WG_PANLEVABS, 0x00); /* 0x0133 */
|
||
|
||
/* Average parameter are set IniAdj */
|
||
|
||
/* Phase Transition Setting */
|
||
/* State 2 -> 1 */
|
||
RegWriteA_LC898122AF(WG_PANSTT21JUG0, 0x00); /* 0x0140 */
|
||
RegWriteA_LC898122AF(WG_PANSTT21JUG1, 0x00); /* 0x0141 */
|
||
/* State 3 -> 1 */
|
||
RegWriteA_LC898122AF(WG_PANSTT31JUG0, 0x00); /* 0x0142 */
|
||
RegWriteA_LC898122AF(WG_PANSTT31JUG1, 0x00); /* 0x0143 */
|
||
/* State 4 -> 1 */
|
||
RegWriteA_LC898122AF(WG_PANSTT41JUG0, 0x01); /* 0x0144 */
|
||
RegWriteA_LC898122AF(WG_PANSTT41JUG1, 0x00); /* 0x0145 */
|
||
/* State 1 -> 2 */
|
||
RegWriteA_LC898122AF(WG_PANSTT12JUG0, 0x00); /* 0x0146 */
|
||
RegWriteA_LC898122AF(WG_PANSTT12JUG1, 0x07); /* 0x0147 */
|
||
/* State 1 -> 3 */
|
||
RegWriteA_LC898122AF(WG_PANSTT13JUG0, 0x00); /* 0x0148 */
|
||
RegWriteA_LC898122AF(WG_PANSTT13JUG1, 0x00); /* 0x0149 */
|
||
/* State 2 -> 3 */
|
||
RegWriteA_LC898122AF(WG_PANSTT23JUG0, 0x11); /* 0x014A */
|
||
RegWriteA_LC898122AF(WG_PANSTT23JUG1, 0x00); /* 0x014B */
|
||
/* State 4 -> 3 */
|
||
RegWriteA_LC898122AF(WG_PANSTT43JUG0, 0x00); /* 0x014C */
|
||
RegWriteA_LC898122AF(WG_PANSTT43JUG1, 0x00); /* 0x014D */
|
||
/* State 3 -> 4 */
|
||
RegWriteA_LC898122AF(WG_PANSTT34JUG0, 0x01); /* 0x014E */
|
||
RegWriteA_LC898122AF(WG_PANSTT34JUG1, 0x00); /* 0x014F */
|
||
/* State 2 -> 4 */
|
||
RegWriteA_LC898122AF(WG_PANSTT24JUG0, 0x00); /* 0x0150 */
|
||
RegWriteA_LC898122AF(WG_PANSTT24JUG1, 0x00); /* 0x0151 */
|
||
/* State 4 -> 2 */
|
||
RegWriteA_LC898122AF(WG_PANSTT42JUG0, 0x44); /* 0x0152 */
|
||
RegWriteA_LC898122AF(WG_PANSTT42JUG1, 0x04); /* 0x0153 */
|
||
|
||
/* State Timer */
|
||
RegWriteA_LC898122AF(WG_PANSTT1LEVTMR, 0x00); /* 0x015B */
|
||
RegWriteA_LC898122AF(WG_PANSTT2LEVTMR, 0x00); /* 0x015C */
|
||
RegWriteA_LC898122AF(WG_PANSTT3LEVTMR, 0x00); /* 0x015D */
|
||
RegWriteA_LC898122AF(WG_PANSTT4LEVTMR, 0x03); /* 0x015E */
|
||
|
||
/* Control filter */
|
||
RegWriteA_LC898122AF(WG_PANTRSON0,
|
||
0x11); /* 0x0132 USE I12/iSTP/Gain-Filter */
|
||
|
||
/* State Setting */
|
||
IniPtMovMod(OFF); /* Pan/Tilt setting (Still) */
|
||
|
||
/* Hold */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETILHLD, 0x00); /* 0x015F */
|
||
|
||
/* State2,4 Step Time Setting */
|
||
RegWriteA_LC898122AF(WG_PANSTT2TMR0, 0x01); /* 0x013C */
|
||
RegWriteA_LC898122AF(WG_PANSTT2TMR1, 0x00); /* 0x013D */
|
||
RegWriteA_LC898122AF(WG_PANSTT4TMR0, 0x02); /* 0x013E */
|
||
RegWriteA_LC898122AF(WG_PANSTT4TMR1, 0x07); /* 0x013F */
|
||
|
||
RegWriteA_LC898122AF(WG_PANSTTXXXTH, 0x00); /* 0x015A */
|
||
|
||
#ifdef GAIN_CONT
|
||
RamWrite32A_LC898122AF(gxlevlow, TRI_LEVEL); /* 0x10AE Low Th */
|
||
RamWrite32A_LC898122AF(gylevlow, TRI_LEVEL); /* 0x11AE Low Th */
|
||
RamWrite32A_LC898122AF(gxadjmin, XMINGAIN); /* 0x1094 Low gain */
|
||
RamWrite32A_LC898122AF(gxadjmax, XMAXGAIN); /* 0x1095 Hgh gain */
|
||
RamWrite32A_LC898122AF(gxadjdn, XSTEPDN); /* 0x1096 -step */
|
||
RamWrite32A_LC898122AF(gxadjup, XSTEPUP); /* 0x1097 +step */
|
||
RamWrite32A_LC898122AF(gyadjmin, YMINGAIN); /* 0x1194 Low gain */
|
||
RamWrite32A_LC898122AF(gyadjmax, YMAXGAIN); /* 0x1195 Hgh gain */
|
||
RamWrite32A_LC898122AF(gyadjdn, YSTEPDN); /* 0x1196 -step */
|
||
RamWrite32A_LC898122AF(gyadjup, YSTEPUP); /* 0x1197 +step */
|
||
|
||
RegWriteA_LC898122AF(
|
||
WG_LEVADD,
|
||
(unsigned char)MONADR); /* 0x0120 Input signal */
|
||
RegWriteA_LC898122AF(WG_LEVTMR, TIMEBSE); /* 0x0123 Base Time */
|
||
RegWriteA_LC898122AF(WG_LEVTMRLOW,
|
||
TIMELOW); /* 0x0121 X Low Time */
|
||
RegWriteA_LC898122AF(WG_LEVTMRHGH,
|
||
TIMEHGH); /* 0x0122 X Hgh Time */
|
||
RegWriteA_LC898122AF(
|
||
WG_ADJGANADD,
|
||
(unsigned char)GANADR); /* 0x0128 control address */
|
||
RegWriteA_LC898122AF(WG_ADJGANGO, 0x00); /* 0x0108 manual off */
|
||
|
||
/* exe function */
|
||
/* AutoGainControlSw( OFF ) ; */ /* Auto Gain Control Mode OFF */
|
||
AutoGainControlSw(ON); /* Auto Gain Control Mode ON */
|
||
#endif
|
||
}
|
||
|
||
void IniFil(void)
|
||
{
|
||
unsigned short UsAryId;
|
||
|
||
/* Filter Registor Parameter Setting */
|
||
UsAryId = 0;
|
||
while (CsFilReg[UsAryId].UsRegAdd != 0xFFFF) {
|
||
RegWriteA_LC898122AF(CsFilReg[UsAryId].UsRegAdd,
|
||
CsFilReg[UsAryId].UcRegDat);
|
||
UsAryId++;
|
||
}
|
||
|
||
/* Filter Ram Parameter Setting */
|
||
UsAryId = 0;
|
||
while (CsFilRam[UsAryId].UsRamAdd != 0xFFFF) {
|
||
RamWrite32A_LC898122AF(CsFilRam[UsAryId].UsRamAdd,
|
||
CsFilRam[UsAryId].UlRamDat);
|
||
UsAryId++;
|
||
}
|
||
}
|
||
|
||
void IniAdj(void)
|
||
{
|
||
RegWriteA_LC898122AF(WC_RAMACCXY,
|
||
0x00); /* 0x018D Filter copy off */
|
||
|
||
IniPtAve(); /* Average setting */
|
||
|
||
/* OIS */
|
||
RegWriteA_LC898122AF(CMSDAC0,
|
||
BIAS_CUR_OIS); /* 0x0251 Hall Dac<61>d<EFBFBD><64> */
|
||
RegWriteA_LC898122AF(OPGSEL0,
|
||
AMP_GAIN_X); /* 0x0253 Hall amp Gain X */
|
||
RegWriteA_LC898122AF(OPGSEL1,
|
||
AMP_GAIN_Y); /* 0x0254 Hall amp Gain Y */
|
||
/* AF */
|
||
RegWriteA_LC898122AF(CMSDAC1,
|
||
BIAS_CUR_AF); /* 0x0252 Hall Dac<61>d<EFBFBD><64> */
|
||
RegWriteA_LC898122AF(OPGSEL2,
|
||
AMP_GAIN_AF); /* 0x0255 Hall amp Gain AF */
|
||
|
||
RegWriteA_LC898122AF(OSCSET, OSC_INI); /* 0x0257 OSC ini */
|
||
|
||
/* adjusted value */
|
||
RegWriteA_LC898122AF(
|
||
IZAH,
|
||
DGYRO_OFST_XH); /* 0x02A0 Set Offset High byte */
|
||
RegWriteA_LC898122AF(
|
||
IZAL,
|
||
DGYRO_OFST_XL); /* 0x02A1 Set Offset Low byte */
|
||
RegWriteA_LC898122AF(
|
||
IZBH,
|
||
DGYRO_OFST_YH); /* 0x02A2 Set Offset High byte */
|
||
RegWriteA_LC898122AF(
|
||
IZBL,
|
||
DGYRO_OFST_YL); /* 0x02A3 Set Offset Low byte */
|
||
|
||
/* Ram Access */
|
||
RamAccFixMod(ON); /* 16bit Fix mode */
|
||
|
||
/* OIS adjusted parameter */
|
||
RamWriteA_LC898122AF(DAXHLO, DAHLXO_INI); /* 0x1479 */
|
||
RamWriteA_LC898122AF(DAXHLB, DAHLXB_INI); /* 0x147A */
|
||
RamWriteA_LC898122AF(DAYHLO, DAHLYO_INI); /* 0x14F9 */
|
||
RamWriteA_LC898122AF(DAYHLB, DAHLYB_INI); /* 0x14FA */
|
||
RamWriteA_LC898122AF(OFF0Z, HXOFF0Z_INI); /* 0x1450 */
|
||
RamWriteA_LC898122AF(OFF1Z, HYOFF1Z_INI); /* 0x14D0 */
|
||
RamWriteA_LC898122AF(sxg, SXGAIN_INI); /* 0x10D3 */
|
||
RamWriteA_LC898122AF(syg, SYGAIN_INI); /* 0x11D3 */
|
||
/* UsCntXof = OPTCEN_X ; */ /* Clear Optical center X value */
|
||
/* UsCntYof = OPTCEN_Y ; */ /* Clear Optical center Y value */
|
||
/* RamWriteA_LC898122AF( SXOFFZ1, UsCntXof ) ; // 0x1461 */
|
||
/* RamWriteA_LC898122AF( SYOFFZ1, UsCntYof ) ; // 0x14E1 */
|
||
|
||
/* AF adjusted parameter */
|
||
RamWriteA_LC898122AF(DAZHLO, DAHLZO_INI); /* 0x1529 */
|
||
RamWriteA_LC898122AF(DAZHLB, DAHLZB_INI); /* 0x152A */
|
||
|
||
/* Ram Access */
|
||
RamAccFixMod(OFF); /* 32bit Float mode */
|
||
|
||
RamWrite32A_LC898122AF(
|
||
gxzoom,
|
||
GXGAIN_INI); /* 0x1020 Gyro X axis Gain adjusted value */
|
||
RamWrite32A_LC898122AF(
|
||
gyzoom,
|
||
GYGAIN_INI); /* 0x1120 Gyro Y axis Gain adjusted value */
|
||
|
||
RamWrite32A_LC898122AF(sxq, SXQ_INI);
|
||
RamWrite32A_LC898122AF(syq, SYQ_INI);
|
||
|
||
if (GXHY_GYHX) { /* GX -> HY , GY -> HX */
|
||
RamWrite32A_LC898122AF(sxgx, 0x00000000); /* 0x10B8 */
|
||
RamWrite32A_LC898122AF(sxgy, 0x3F800000); /* 0x10B9 */
|
||
|
||
RamWrite32A_LC898122AF(sygy, 0x00000000); /* 0x11B8 */
|
||
RamWrite32A_LC898122AF(sygx, 0x3F800000); /* 0x11B9 */
|
||
}
|
||
|
||
SetZsp(0); /* Zoom coefficient Initial Setting */
|
||
|
||
RegWriteA_LC898122AF(PWMA, 0xC0); /* 0x0010 PWM enable */
|
||
|
||
RegWriteA_LC898122AF(STBB0, 0xDF);
|
||
|
||
RegWriteA_LC898122AF(WC_EQSW, 0x02); /* 0x01E0 */
|
||
RegWriteA_LC898122AF(WC_MESLOOP1, 0x02); /* 0x0193 */
|
||
RegWriteA_LC898122AF(WC_MESLOOP0, 0x00); /* 0x0192 */
|
||
RegWriteA_LC898122AF(WC_AMJLOOP1, 0x02); /* 0x01A3 */
|
||
RegWriteA_LC898122AF(WC_AMJLOOP0, 0x00); /* 0x01A2 */
|
||
|
||
SetPanTiltMode(OFF); /* Pan/Tilt OFF */
|
||
|
||
SetGcf(0); /* DI initial value */
|
||
#ifdef H1COEF_CHANGER
|
||
SetH1cMod(ACTMODE); /* Lvl Change Active mode */
|
||
#endif
|
||
|
||
DrvSw(ON); /* 0x0001 Driver Mode setting */
|
||
|
||
RegWriteA_LC898122AF(WC_EQON, 0x01); /* 0x0101 Filter ON */
|
||
}
|
||
|
||
void IniCmd(void)
|
||
{
|
||
|
||
MemClr((unsigned char *)&StAdjPar,
|
||
sizeof(struct stAdjPar)); /* Adjust Parameter Clear */
|
||
}
|
||
|
||
void BsyWit(unsigned short UsTrgAdr, unsigned char UcTrgDat)
|
||
{
|
||
unsigned char UcFlgVal;
|
||
|
||
RegWriteA_LC898122AF(UsTrgAdr,
|
||
UcTrgDat); /* Trigger Register Setting */
|
||
|
||
UcFlgVal = 1;
|
||
|
||
while (UcFlgVal) {
|
||
|
||
RegReadA_LC898122AF(UsTrgAdr, &UcFlgVal);
|
||
UcFlgVal &= (UcTrgDat & 0x0F);
|
||
};
|
||
}
|
||
|
||
void MemClr(unsigned char *NcTgtPtr, unsigned short UsClrSiz)
|
||
{
|
||
unsigned short UsClrIdx;
|
||
|
||
for (UsClrIdx = 0; UsClrIdx < UsClrSiz; UsClrIdx++) {
|
||
*NcTgtPtr = 0;
|
||
NcTgtPtr++;
|
||
}
|
||
}
|
||
|
||
void GyOutSignal(void)
|
||
{
|
||
|
||
RegWriteA_LC898122AF(GRADR0, GYROX_INI);
|
||
RegWriteA_LC898122AF(GRADR1, GYROY_INI);
|
||
|
||
/*Start OIS Reading */
|
||
RegWriteA_LC898122AF(GRSEL, 0x02);
|
||
}
|
||
|
||
void GyOutSignalCont(void)
|
||
{
|
||
|
||
/*Start OIS Reading */
|
||
RegWriteA_LC898122AF(GRSEL, 0x04);
|
||
}
|
||
|
||
#ifdef STANDBY_MODE
|
||
|
||
void AccWit(unsigned char UcTrgDat)
|
||
{
|
||
unsigned char UcFlgVal;
|
||
|
||
UcFlgVal = 1;
|
||
|
||
while (UcFlgVal) {
|
||
RegReadA_LC898122AF(GRACC, &UcFlgVal); /* 0x0282 */
|
||
UcFlgVal &= UcTrgDat;
|
||
};
|
||
}
|
||
|
||
void SelectGySleep(unsigned char UcSelMode)
|
||
{
|
||
#ifdef USE_INVENSENSE
|
||
unsigned char UcRamIni;
|
||
unsigned char UcGrini;
|
||
|
||
if (UcSelMode == ON) {
|
||
RegWriteA_LC898122AF(WC_EQON,
|
||
0x00); /* 0x0101 Equalizer OFF */
|
||
RegWriteA_LC898122AF(GRSEL,
|
||
0x01); /* 0x0280 Set Command Mode */
|
||
|
||
RegReadA_LC898122AF(GRINI, &UcGrini);
|
||
|
||
RegWriteA_LC898122AF(GRINI, (UcGrini | SLOWMODE));
|
||
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x6B); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(GRACC, 0x01);
|
||
AccWit(0x01); /* Digital Gyro busy wait */
|
||
RegReadA_LC898122AF(GRDAT0H, &UcRamIni); /* 0x0290 */
|
||
|
||
UcRamIni |= 0x40; /* Set Sleep bit */
|
||
#ifdef GYROSTBY
|
||
UcRamIni &= ~0x01; /* Clear PLL bit(internal oscillator */
|
||
#endif
|
||
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x6B); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
UcRamIni); /* 0x028A Set Write Data(Sleep ON) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
|
||
#ifdef GYROSTBY
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x6C); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
0x07); /* 0x028A Set Write Data(STBY ON) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
#endif
|
||
} else {
|
||
#ifdef GYROSTBY
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x6C); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
0x00); /* 0x028A Set Write Data(STBY OFF) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
#endif
|
||
RegWriteA_LC898122AF(GRADR0, 0x6B);
|
||
RegWriteA_LC898122AF(GRACC, 0x01);
|
||
AccWit(0x01); /* Digital Gyro busy wait */
|
||
RegReadA_LC898122AF(GRDAT0H, &UcRamIni); /* 0x0290 */
|
||
|
||
UcRamIni &= ~0x40; /* Clear Sleep bit */
|
||
#ifdef GYROSTBY
|
||
UcRamIni |= 0x01; /* Set PLL bit */
|
||
#endif
|
||
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
UcRamIni); /* 0x028A Set Write Data(Sleep OFF) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
|
||
RegReadA_LC898122AF(GRINI, &UcGrini);
|
||
|
||
RegWriteA_LC898122AF(GRINI, (UcGrini & ~SLOWMODE));
|
||
|
||
GyOutSignal(); /* Select Gyro output signal */
|
||
|
||
WitTim_LC898122AF(50); /* 50ms wait */
|
||
|
||
RegWriteA_LC898122AF(
|
||
WC_EQON, 0x01); /* 0x0101 GYRO Equalizer ON */
|
||
|
||
ClrGyr(0x007F, CLR_FRAM1); /* Gyro Delay RAM Clear */
|
||
}
|
||
#else /* Panasonic */
|
||
|
||
/* unsigned char UcRamIni ; */
|
||
|
||
if (UcSelMode == ON) {
|
||
RegWriteA_LC898122AF(
|
||
WC_EQON, 0x00); /* 0x0101 GYRO Equalizer OFF */
|
||
RegWriteA_LC898122AF(GRSEL,
|
||
0x01); /* 0x0280 Set Command Mode */
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x4C); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
0x02); /* 0x028A Set Write Data(Sleep ON) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
} else {
|
||
RegWriteA_LC898122AF(
|
||
GRADR0, 0x4C); /* 0x0283 Set Write Command */
|
||
RegWriteA_LC898122AF(
|
||
GSETDT,
|
||
0x00); /* 0x028A Set Write Data(Sleep OFF) */
|
||
RegWriteA_LC898122AF(GRACC,
|
||
0x10); /* 0x0282 Set Trigger ON */
|
||
AccWit(0x10); /* Digital Gyro busy wait */
|
||
GyOutSignal(); /* Select Gyro output signal */
|
||
|
||
WitTim_LC898122AF(50); /* 50ms wait */
|
||
|
||
RegWriteA_LC898122AF(
|
||
WC_EQON, 0x01); /* 0x0101 GYRO Equalizer ON */
|
||
ClrGyr(0x007F, CLR_FRAM1); /* Gyro Delay RAM Clear */
|
||
}
|
||
#endif
|
||
}
|
||
#endif
|
||
|
||
#ifdef GAIN_CONT
|
||
|
||
void AutoGainControlSw(unsigned char UcModeSw)
|
||
{
|
||
|
||
if (UcModeSw == OFF) {
|
||
RegWriteA_LC898122AF(WG_ADJGANGXATO,
|
||
0xA0); /* 0x0129 X exe off */
|
||
RegWriteA_LC898122AF(WG_ADJGANGYATO,
|
||
0xA0); /* 0x012A Y exe off */
|
||
RamWrite32A_LC898122AF(GANADR, XMAXGAIN); /* Gain Through */
|
||
RamWrite32A_LC898122AF(GANADR | 0x0100,
|
||
YMAXGAIN); /* Gain Through */
|
||
} else {
|
||
RegWriteA_LC898122AF(WG_ADJGANGXATO,
|
||
0xA3); /* 0x0129 X exe on */
|
||
RegWriteA_LC898122AF(WG_ADJGANGYATO,
|
||
0xA3); /* 0x012A Y exe on */
|
||
}
|
||
}
|
||
#endif
|
||
|
||
void ClrGyr(unsigned short UsClrFil, unsigned char UcClrMod)
|
||
{
|
||
unsigned char UcRamClr;
|
||
|
||
/*Select Filter to clear */
|
||
RegWriteA_LC898122AF(WC_RAMDLYMOD1, (unsigned char)(UsClrFil >> 8));
|
||
RegWriteA_LC898122AF(
|
||
WC_RAMDLYMOD0, (unsigned char)UsClrFil);
|
||
|
||
/*Enable Clear */
|
||
RegWriteA_LC898122AF(WC_RAMINITON,
|
||
UcClrMod);
|
||
|
||
/*Check RAM Clear complete */
|
||
do {
|
||
RegReadA_LC898122AF(WC_RAMINITON, &UcRamClr);
|
||
UcRamClr &= UcClrMod;
|
||
} while (UcRamClr != 0x00);
|
||
}
|
||
|
||
void DrvSw(unsigned char UcDrvSw)
|
||
{
|
||
if (UcDrvSw == ON) {
|
||
if (UcPwmMod == PWMMOD_CVL) {
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0xF0);
|
||
} else {
|
||
#ifdef PWM_BREAK
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0x00);
|
||
#else
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0xC0);
|
||
#endif
|
||
}
|
||
} else {
|
||
if (UcPwmMod == PWMMOD_CVL) {
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0x30); /* 0x0001 Drvier Block Ena=0 */
|
||
} else {
|
||
#ifdef PWM_BREAK
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0x00); /* 0x0001 Drv.MODE=0,Drv.BLK=0,MODE0B */
|
||
#else
|
||
RegWriteA_LC898122AF(
|
||
DRVFC,
|
||
0x00); /* 0x0001 Drvier Block Ena=0 */
|
||
#endif
|
||
}
|
||
}
|
||
}
|
||
|
||
void AfDrvSw(unsigned char UcDrvSw)
|
||
{
|
||
if (UcDrvSw == ON) {
|
||
#ifdef AF_PWMMODE
|
||
RegWriteA_LC898122AF(DRVFCAF, 0x00);
|
||
#else
|
||
RegWriteA_LC898122AF(DRVFCAF, 0x20);
|
||
#endif
|
||
RegWriteA_LC898122AF(CCAAF, 0x80);
|
||
} else {
|
||
RegWriteA_LC898122AF(CCAAF, 0x00);
|
||
}
|
||
}
|
||
|
||
void RamAccFixMod(unsigned char UcAccMod)
|
||
{
|
||
switch (UcAccMod) {
|
||
case OFF:
|
||
RegWriteA_LC898122AF(WC_RAMACCMOD, 0x00);
|
||
break;
|
||
case ON:
|
||
RegWriteA_LC898122AF(WC_RAMACCMOD, 0x31);
|
||
break;
|
||
}
|
||
}
|
||
|
||
void IniAf(void)
|
||
{
|
||
unsigned char UcStbb0;
|
||
|
||
AfDrvSw(OFF); /* AF Drvier Block Ena=0 */
|
||
#ifdef AF_PWMMODE
|
||
RegWriteA_LC898122AF(DRVFCAF, 0x00);
|
||
#else
|
||
RegWriteA_LC898122AF(DRVFCAF, 0x20);
|
||
#endif
|
||
RegWriteA_LC898122AF(DRVFC3AF, 0x00);
|
||
RegWriteA_LC898122AF(DRVFC4AF, 0x80); /* 0x0084 DOFSTDAF */
|
||
RegWriteA_LC898122AF(PWMAAF, 0x00); /* 0x0090 AF PWM standby */
|
||
RegWriteA_LC898122AF(AFFC, 0x80); /* 0x0088 OpenAF/-/- */
|
||
#ifdef AF_PWMMODE
|
||
RegWriteA_LC898122AF(DRVFC2AF, 0x82); /* 0x0082 AF slope3 */
|
||
RegWriteA_LC898122AF(DRVCH3SEL,
|
||
0x02); /* 0x0085 AF only IN1 control */
|
||
RegWriteA_LC898122AF(PWMFCAF,
|
||
0x89); /* 0x0091 AF GND , Carrier , MODE1 */
|
||
RegWriteA_LC898122AF(PWMPERIODAF,
|
||
0xA0); /* 0x0099 AF none-synchronism */
|
||
#else
|
||
RegWriteA_LC898122AF(DRVFC2AF, 0x00); /* 0x0082 AF slope0 */
|
||
RegWriteA_LC898122AF(DRVCH3SEL,
|
||
0x00); /* 0x0085 AF H bridge control */
|
||
RegWriteA_LC898122AF(
|
||
PWMFCAF, 0x01); /* 0x0091 AF VREF , Carrier , MODE1 */
|
||
RegWriteA_LC898122AF(PWMPERIODAF,
|
||
0x20); /* 0x0099 AF none-synchronism */
|
||
#endif
|
||
RegWriteA_LC898122AF(CCFCAF, 0x40); /* 0x00A1 GND/- */
|
||
|
||
RegReadA_LC898122AF(STBB0, &UcStbb0);
|
||
|
||
UcStbb0 &= 0x7F;
|
||
RegWriteA_LC898122AF(STBB0, UcStbb0); /* 0x0250 OIS standby */
|
||
RegWriteA_LC898122AF(STBB1, 0x00); /* 0x0264 All standby */
|
||
|
||
/* AF Initial setting */
|
||
RegWriteA_LC898122AF(FSTMODE, FSTMODE_AF); /* 0x0302 */
|
||
RamWriteA_LC898122AF(
|
||
RWEXD1_L,
|
||
RWEXD1_L_AF); /* 0x0396 - 0x0397 (Register continuos write) */
|
||
RamWriteA_LC898122AF(
|
||
RWEXD2_L,
|
||
RWEXD2_L_AF); /* 0x0398 - 0x0399 (Register continuos write) */
|
||
RamWriteA_LC898122AF(
|
||
RWEXD3_L,
|
||
RWEXD3_L_AF); /* 0x039A - 0x039B (Register continuos write) */
|
||
RegWriteA_LC898122AF(FSTCTIME, FSTCTIME_AF); /* 0x0303 */
|
||
RamWriteA_LC898122AF(
|
||
TCODEH,
|
||
0x0000); /* 0x0304 - 0x0305 (Register continuos write) */
|
||
|
||
#ifdef AF_PWMMODE
|
||
RegWriteA_LC898122AF(PWMAAF, 0x80); /* 0x0090 AF PWM enable */
|
||
#endif
|
||
|
||
UcStbb0 |= 0x80;
|
||
RegWriteA_LC898122AF(STBB0, UcStbb0); /* 0x0250 */
|
||
RegWriteA_LC898122AF(STBB1,
|
||
0x05);
|
||
AfDrvSw(ON); /* AF Drvier Block Ena=1 */
|
||
}
|
||
|
||
void IniPtAve(void)
|
||
{
|
||
RegWriteA_LC898122AF(WG_PANSTT1DWNSMP0, 0x00); /* 0x0134 */
|
||
RegWriteA_LC898122AF(WG_PANSTT1DWNSMP1, 0x00); /* 0x0135 */
|
||
RegWriteA_LC898122AF(WG_PANSTT2DWNSMP0, 0x90); /* 0x0136 400 */
|
||
RegWriteA_LC898122AF(WG_PANSTT2DWNSMP1, 0x01); /* 0x0137 */
|
||
RegWriteA_LC898122AF(WG_PANSTT3DWNSMP0, 0x64); /* 0x0138 100 */
|
||
RegWriteA_LC898122AF(WG_PANSTT3DWNSMP1, 0x00); /* 0x0139 */
|
||
RegWriteA_LC898122AF(WG_PANSTT4DWNSMP0, 0x00); /* 0x013A */
|
||
RegWriteA_LC898122AF(WG_PANSTT4DWNSMP1, 0x00); /* 0x013B */
|
||
|
||
RamWrite32A_LC898122AF(st1mean, 0x3f800000); /* 0x1235 */
|
||
RamWrite32A_LC898122AF(st2mean, 0x3B23D700); /* 0x1236 1/400 */
|
||
RamWrite32A_LC898122AF(st3mean, 0x3C23D700); /* 0x1237 1/100 */
|
||
RamWrite32A_LC898122AF(st4mean, 0x3f800000); /* 0x1238 */
|
||
}
|
||
|
||
void IniPtMovMod(unsigned char UcPtMod)
|
||
{
|
||
switch (UcPtMod) {
|
||
case OFF:
|
||
RegWriteA_LC898122AF(WG_PANSTTSETGYRO, 0x00); /* 0x0154 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETGAIN, 0x54); /* 0x0155 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETISTP, 0x14); /* 0x0156 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETIFTR, 0x94); /* 0x0157 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETLFTR, 0x00); /* 0x0158 */
|
||
|
||
break;
|
||
case ON:
|
||
RegWriteA_LC898122AF(WG_PANSTTSETGYRO, 0x00); /* 0x0154 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETGAIN, 0x00); /* 0x0155 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETISTP, 0x14); /* 0x0156 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETIFTR, 0x94); /* 0x0157 */
|
||
RegWriteA_LC898122AF(WG_PANSTTSETLFTR, 0x00); /* 0x0158 */
|
||
break;
|
||
}
|
||
}
|
||
|
||
void ChkCvr(void)
|
||
{
|
||
RegReadA_LC898122AF(CVER, &UcCvrCod); /* 0x027E */
|
||
RegWriteA_LC898122AF(MDLREG, MDL_VER); /* 0x00FF Model */
|
||
RegWriteA_LC898122AF(VRREG, FW_VER); /* 0x02D0 Version */
|
||
}
|