1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
4 *
5 * Author: Kay Guo <kay.guo@rock-chips.com>
6 */
7
8 #include <linux/kernel.h>
9 #include "da215s_core.h"
10
11 #define DA215S_OFFSET_THRESHOLD 20
12 #define PEAK_LVL 800
13 #define STICK_LSB 2000
14 #define AIX_HISTORY_SIZE 20
15
16
17 static int z_offset;
18
19 #define TEMP_CALIBRATE_STATIC_THRESHOLD 60
20 #define TEMP_CALIBRATE_STATIC_COUNT 12
21
squareRoot(int val)22 static int squareRoot(int val)
23 {
24 int r = 0, x;
25 int shift;
26
27 if (val < 0)
28 return 0;
29
30 for (shift = 0; shift < 32; shift += 2) {
31 x = 0x40000000l >> shift;
32 if ((x+r) <= val) {
33 val -= x + r;
34 r = (r >> 1) | x;
35 } else {
36 r = r >> 1;
37 }
38 }
39
40 return r;
41 }
42
da215s_temp_calibrate_detect_static(short x,short y,short z)43 static int da215s_temp_calibrate_detect_static(short x, short y, short z)
44 {
45 static int count_static;
46 static short temp_x[TEMP_CALIBRATE_STATIC_COUNT];
47 static short temp_y[TEMP_CALIBRATE_STATIC_COUNT];
48 static short temp_z[TEMP_CALIBRATE_STATIC_COUNT];
49 static short max_x, max_y, max_z;
50 static short min_x, min_y, min_z;
51 static char is_first = 1;
52 int i, delta_sum = 0;
53
54 count_static++;
55
56 if (is_first) {
57 temp_x[0] = x;
58 temp_y[0] = y;
59 temp_z[0] = z;
60 for (i = 1; i < TEMP_CALIBRATE_STATIC_COUNT; i++) {
61 temp_x[i] = temp_x[0];
62 temp_y[i] = temp_y[0];
63 temp_z[i] = temp_z[0];
64 }
65 is_first = 0;
66 } else {
67 max_x = min_x = temp_x[1];
68 max_y = min_y = temp_y[1];
69 max_z = min_z = temp_z[1];
70
71 for (i = 0; i < TEMP_CALIBRATE_STATIC_COUNT; i++) {
72 if (i == (TEMP_CALIBRATE_STATIC_COUNT-1)) {
73 temp_x[i] = x;
74 temp_y[i] = y;
75 temp_z[i] = z;
76 } else {
77 temp_x[i] = temp_x[i+1];
78 temp_y[i] = temp_y[i+1];
79 temp_z[i] = temp_z[i+1];
80 }
81 max_x = (max_x > temp_x[i]) ? max_x:temp_x[i];
82 max_y = (max_y > temp_y[i]) ? max_y:temp_y[i];
83 max_z = (max_z > temp_z[i]) ? max_z:temp_z[i];
84
85 min_x = (min_x < temp_x[i]) ? min_x:temp_x[i];
86 min_y = (min_y < temp_y[i]) ? min_y:temp_y[i];
87 min_z = (min_z < temp_z[i]) ? min_z:temp_z[i];
88 }
89 }
90
91 if (count_static > TEMP_CALIBRATE_STATIC_COUNT) {
92 count_static = TEMP_CALIBRATE_STATIC_COUNT;
93 delta_sum = abs(max_x - min_x) + abs(max_y - min_y) + abs(max_z - min_z);
94
95 if (delta_sum < TEMP_CALIBRATE_STATIC_THRESHOLD)
96 return 1;
97 }
98
99 return 0;
100 }
101
da215s_temp_calibrate(int * x,int * y,int * z)102 int da215s_temp_calibrate(int *x, int *y, int *z)
103 {
104 int tem_z = 0;
105 int cus = MIR3DA_OFFSET_MAX-MIR3DA_OFFSET_CUS;
106 int is_static = 0;
107 short lz_offset;
108
109 *z = *z + z_offset;
110 lz_offset = (*z) % 10;
111
112 if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs(*y) < MIR3DA_OFFSET_MAX)) {
113 is_static = da215s_temp_calibrate_detect_static(*x, *y, *z-z_offset);
114 tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN -
115 (*x)*(*x) - (*y)*(*y)) + lz_offset;
116 if (z_offset == 0) {
117 if (is_static == 1)
118 z_offset = (*z >= 0) ? (tem_z-*z) : (-tem_z-*z);
119 *z = ((*z >= 0) ? (1) : (-1))*tem_z;
120 } else if (is_static) {
121 if (abs(abs(*z) - MIR3DA_OFFSET_SEN) > MIR3DA_OFFSET_CUS) {
122 *z = ((*z >= 0) ? (1) : (-1))*tem_z;
123 z_offset = 0;
124 }
125 }
126
127 *x = (*x) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
128 *y = (*y) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
129
130 } else if ((abs((abs(*x)-MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
131 (abs(*y) < MIR3DA_OFFSET_MAX) && (z_offset)) {
132 if (abs(*x) > MIR3DA_OFFSET_SEN) {
133 *x = (*x > 0) ?
134 (*x - (abs(*x) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX) :
135 (*x + (abs(*x) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX);
136 } else {
137 *x = (*x > 0) ?
138 (*x + (MIR3DA_OFFSET_SEN-abs(*x)) * cus/MIR3DA_OFFSET_MAX) :
139 (*x - (MIR3DA_OFFSET_SEN-abs(*x)) * cus/MIR3DA_OFFSET_MAX);
140 }
141 *y = (*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
142 } else if ((abs((abs(*y) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
143 (abs(*x) < MIR3DA_OFFSET_MAX) && (z_offset)) {
144 if (abs(*y) > MIR3DA_OFFSET_SEN) {
145 *y = (*y > 0) ?
146 (*y - (abs(*y) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX) :
147 (*y + (abs(*y) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX);
148 } else {
149 *y = (*y > 0) ?
150 (*y + (MIR3DA_OFFSET_SEN-abs(*y)) * cus/MIR3DA_OFFSET_MAX) :
151 (*y - (MIR3DA_OFFSET_SEN-abs(*y)) * cus/MIR3DA_OFFSET_MAX);
152 }
153 *x = (*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
154 } else if (z_offset == 0) {
155 if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs((*y > 0) ?
156 (MIR3DA_OFFSET_SEN - *y) : (MIR3DA_OFFSET_SEN + *y)) < MIR3DA_OFFSET_MAX)) {
157 *z = ((*z >= 0) ?
158 (1) : (-1)) * abs(*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
159 } else if ((abs(*y) < MIR3DA_OFFSET_MAX) && (abs((*x > 0) ?
160 (MIR3DA_OFFSET_SEN - *x) :
161 (MIR3DA_OFFSET_SEN + *x)) < MIR3DA_OFFSET_MAX)) {
162 *z = ((*z >= 0) ?
163 (1) : (-1)) * abs(*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
164 } else {
165 tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN -
166 (*x) * (*x) - (*y) * (*y)) + lz_offset;
167 *z = ((*z >= 0) ? (1) : (-1)) * tem_z;
168 }
169 }
170
171 if (z_offset)
172 return 0;
173 else
174 return -1;
175 }
176
177
178
179