1 /******************************************************************************
2 * $Id: AKFS_APIs.c 580 2012-03-29 09:56:21Z yamada.rj $
3 ******************************************************************************
4 *
5 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 */
19 #include "AKFS_Common.h"
20 #include "AKFS_Disp.h"
21 #include "AKFS_FileIO.h"
22 #include "AKFS_APIs.h"
23
24 #ifdef WIN32
25 #include "AK8975_LinuxDriver.h"
26 #endif
27
28 static AK8975PRMS g_prms;
29
30 /*!
31 Initialize library. At first, 0 is set to all parameters. After that, some
32 parameters, which should not be 0, are set to specific value. Some of initial
33 values can be customized by editing the file \c "AKFS_CSpec.h".
34 @return The return value is #AKM_SUCCESS.
35 @param[in] hpat Specify a layout pattern number. The number is determined
36 according to the mount orientation of the magnetometer.
37 @param[in] regs[3] Specify the ASA values which are read out from
38 fuse ROM. regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ.
39 */
AKFS_Init(const AKFS_PATNO hpat,const uint8 regs[])40 int16 AKFS_Init(
41 const AKFS_PATNO hpat,
42 const uint8 regs[]
43 )
44 {
45 AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n",
46 __FUNCTION__, hpat, regs[0], regs[1], regs[2]);
47
48 /* Set 0 to the AK8975 structure. */
49 memset(&g_prms, 0, sizeof(AK8975PRMS));
50
51 /* Sensitivity */
52 g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT;
53 g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT;
54 g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT;
55 g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT;
56 g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT;
57 g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT;
58
59 /* Initialize variables that initial value is not 0. */
60 g_prms.mi_hnaveV = CSPEC_HNAVE_V;
61 g_prms.mi_hnaveD = CSPEC_HNAVE_D;
62 g_prms.mi_anaveV = CSPEC_ANAVE_V;
63 g_prms.mi_anaveD = CSPEC_ANAVE_D;
64
65 /* Copy ASA values */
66 g_prms.mi_asa.u.x = regs[0];
67 g_prms.mi_asa.u.y = regs[1];
68 g_prms.mi_asa.u.z = regs[2];
69
70 /* Copy layout pattern */
71 g_prms.m_hpat = hpat;
72
73 return AKM_SUCCESS;
74 }
75
76 /*!
77 Release resources. This function is for future expansion.
78 @return The return value is #AKM_SUCCESS.
79 */
AKFS_Release(void)80 int16 AKFS_Release(void)
81 {
82 return AKM_SUCCESS;
83 }
84
85 /*
86 This function is called just before a measurement sequence starts.
87 This function reads parameters from file, then initializes algorithm
88 parameters.
89 @return The return value is #AKM_SUCCESS.
90 @param[in] path Specify a path to the settings file.
91 */
AKFS_Start(const char * path)92 int16 AKFS_Start(
93 const char* path
94 )
95 {
96 AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);
97
98 /* Read setting files from a file */
99 if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) {
100 AKMERROR_STR("AKFS_Load");
101 }
102
103 /* Initialize buffer */
104 AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata);
105 AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf);
106 AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata);
107 AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf);
108
109 /* Initialize for AOC */
110 AKFS_InitAOC(&g_prms.m_aocv);
111 /* Initialize magnetic status */
112 g_prms.mi_hstatus = 0;
113
114 return AKM_SUCCESS;
115 }
116
117 /*!
118 This function is called when a measurement sequence is done.
119 This fucntion writes parameters to file.
120 @return The return value is #AKM_SUCCESS.
121 @param[in] path Specify a path to the settings file.
122 */
AKFS_Stop(const char * path)123 int16 AKFS_Stop(
124 const char* path
125 )
126 {
127 AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);
128
129 /* Write setting files to a file */
130 if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) {
131 AKMERROR_STR("AKFS_Save");
132 }
133
134 return AKM_SUCCESS;
135 }
136
137 /*!
138 This function is called when new magnetometer data is available. The output
139 vector format and coordination system follow the Android definition.
140 @return The return value is #AKM_SUCCESS.
141 Otherwise the return value is #AKM_FAIL.
142 @param[in] mag A set of measurement data from magnetometer. X axis value
143 should be in mag[0], Y axis value should be in mag[1], Z axis value should be
144 in mag[2].
145 @param[in] status A status of magnetometer. This status indicates the result
146 of measurement data, i.e. overflow, success or fail, etc.
147 @param[out] vx X axis value of magnetic field vector.
148 @param[out] vy Y axis value of magnetic field vector.
149 @param[out] vz Z axis value of magnetic field vector.
150 @param[out] accuracy Accuracy of magnetic field vector.
151 */
AKFS_Get_MAGNETIC_FIELD(const int16 mag[3],const int16 status,AKFLOAT * vx,AKFLOAT * vy,AKFLOAT * vz,int16 * accuracy)152 int16 AKFS_Get_MAGNETIC_FIELD(
153 const int16 mag[3],
154 const int16 status,
155 AKFLOAT* vx,
156 AKFLOAT* vy,
157 AKFLOAT* vz,
158 int16* accuracy
159 )
160 {
161 int16 akret;
162 int16 aocret;
163 AKFLOAT radius;
164
165 AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n",
166 __FUNCTION__, mag[0], mag[1], mag[2], status);
167
168 /* Decomposition */
169 /* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */
170 akret = AKFS_DecompAK8975(
171 mag,
172 status,
173 &g_prms.mi_asa,
174 AKFS_HDATA_SIZE,
175 g_prms.mfv_hdata
176 );
177 if(akret == AKFS_ERROR) {
178 AKMERROR;
179 return AKM_FAIL;
180 }
181
182 /* Adjust coordination */
183 akret = AKFS_Rotate(
184 g_prms.m_hpat,
185 &g_prms.mfv_hdata[0]
186 );
187 if (akret == AKFS_ERROR) {
188 AKMERROR;
189 return AKM_FAIL;
190 }
191
192 /* AOC for magnetometer */
193 /* Offset estimation is done in this function */
194 aocret = AKFS_AOC(
195 &g_prms.m_aocv,
196 g_prms.mfv_hdata,
197 &g_prms.mfv_ho
198 );
199
200 /* Subtract offset */
201 /* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */
202 akret = AKFS_VbNorm(
203 AKFS_HDATA_SIZE,
204 g_prms.mfv_hdata,
205 1,
206 &g_prms.mfv_ho,
207 &g_prms.mfv_hs,
208 AK8975_HSENSE_TARGET,
209 AKFS_HDATA_SIZE,
210 g_prms.mfv_hvbuf
211 );
212 if(akret == AKFS_ERROR) {
213 AKMERROR;
214 return AKM_FAIL;
215 }
216
217 /* Averaging */
218 akret = AKFS_VbAve(
219 AKFS_HDATA_SIZE,
220 g_prms.mfv_hvbuf,
221 CSPEC_HNAVE_V,
222 &g_prms.mfv_hvec
223 );
224 if (akret == AKFS_ERROR) {
225 AKMERROR;
226 return AKM_FAIL;
227 }
228
229 /* Check the size of magnetic vector */
230 radius = AKFS_SQRT(
231 (g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) +
232 (g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) +
233 (g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z));
234
235 if (radius > AKFS_GEOMAG_MAX) {
236 g_prms.mi_hstatus = 0;
237 } else {
238 if (aocret) {
239 g_prms.mi_hstatus = 3;
240 }
241 }
242
243 *vx = g_prms.mfv_hvec.u.x;
244 *vy = g_prms.mfv_hvec.u.y;
245 *vz = g_prms.mfv_hvec.u.z;
246 *accuracy = g_prms.mi_hstatus;
247
248 /* Debug output */
249 AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n",
250 *accuracy, *vx, *vy, *vz);
251
252 return AKM_SUCCESS;
253 }
254
255 /*!
256 This function is called when new accelerometer data is available. The output
257 vector format and coordination system follow the Android definition.
258 @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
259 the return value is #AKM_FAIL.
260 @param[in] acc A set of measurement data from accelerometer. X axis value
261 should be in acc[0], Y axis value should be in acc[1], Z axis value should be
262 in acc[2].
263 @param[in] status A status of accelerometer. This status indicates the result
264 of acceleration data, i.e. overflow, success or fail, etc.
265 @param[out] vx X axis value of acceleration vector.
266 @param[out] vy Y axis value of acceleration vector.
267 @param[out] vz Z axis value of acceleration vector.
268 @param[out] accuracy Accuracy of acceleration vector.
269 This value is always 3.
270 */
AKFS_Get_ACCELEROMETER(const int16 acc[3],const int16 status,AKFLOAT * vx,AKFLOAT * vy,AKFLOAT * vz,int16 * accuracy)271 int16 AKFS_Get_ACCELEROMETER(
272 const int16 acc[3],
273 const int16 status,
274 AKFLOAT* vx,
275 AKFLOAT* vy,
276 AKFLOAT* vz,
277 int16* accuracy
278 )
279 {
280 int16 akret;
281
282 AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n",
283 __FUNCTION__, acc[0], acc[1], acc[2], status);
284
285 /* Save data to buffer */
286 AKFS_BufShift(
287 AKFS_ADATA_SIZE,
288 1,
289 g_prms.mfv_adata
290 );
291 g_prms.mfv_adata[0].u.x = acc[0];
292 g_prms.mfv_adata[0].u.y = acc[1];
293 g_prms.mfv_adata[0].u.z = acc[2];
294
295 /* Subtract offset, adjust sensitivity */
296 /* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */
297 akret = AKFS_VbNorm(
298 AKFS_ADATA_SIZE,
299 g_prms.mfv_adata,
300 1,
301 &g_prms.mfv_ao,
302 &g_prms.mfv_as,
303 AK8975_ASENSE_TARGET,
304 AKFS_ADATA_SIZE,
305 g_prms.mfv_avbuf
306 );
307 if(akret == AKFS_ERROR) {
308 AKMERROR;
309 return AKM_FAIL;
310 }
311
312 /* Averaging */
313 akret = AKFS_VbAve(
314 AKFS_ADATA_SIZE,
315 g_prms.mfv_avbuf,
316 CSPEC_ANAVE_V,
317 &g_prms.mfv_avec
318 );
319 if (akret == AKFS_ERROR) {
320 AKMERROR;
321 return AKM_FAIL;
322 }
323
324 /* Adjust coordination */
325 /* It is not needed. Because, the data from AK8975 driver is already
326 follows Android coordinate system. */
327
328 *vx = g_prms.mfv_avec.u.x;
329 *vy = g_prms.mfv_avec.u.y;
330 *vz = g_prms.mfv_avec.u.z;
331 *accuracy = 3;
332
333 /* Debug output */
334 AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n",
335 *accuracy, *vx, *vy, *vz);
336
337 return AKM_SUCCESS;
338 }
339
340 /*!
341 Get orientation sensor's elements. The vector format and coordination system
342 follow the Android definition. Before this function is called, magnetic
343 field vector and acceleration vector should be stored in the buffer by
344 calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER.
345 @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
346 the return value is #AKM_FAIL.
347 @param[out] azimuth Azimuthal angle in degree.
348 @param[out] pitch Pitch angle in degree.
349 @param[out] roll Roll angle in degree.
350 @param[out] accuracy Accuracy of orientation sensor.
351 */
AKFS_Get_ORIENTATION(AKFLOAT * azimuth,AKFLOAT * pitch,AKFLOAT * roll,int16 * accuracy)352 int16 AKFS_Get_ORIENTATION(
353 AKFLOAT* azimuth,
354 AKFLOAT* pitch,
355 AKFLOAT* roll,
356 int16* accuracy
357 )
358 {
359 int16 akret;
360
361 /* Azimuth calculation */
362 /* Coordination system follows the Android coordination. */
363 akret = AKFS_Direction(
364 AKFS_HDATA_SIZE,
365 g_prms.mfv_hvbuf,
366 CSPEC_HNAVE_D,
367 AKFS_ADATA_SIZE,
368 g_prms.mfv_avbuf,
369 CSPEC_ANAVE_D,
370 &g_prms.mf_azimuth,
371 &g_prms.mf_pitch,
372 &g_prms.mf_roll
373 );
374
375 if(akret == AKFS_ERROR) {
376 AKMERROR;
377 return AKM_FAIL;
378 }
379 *azimuth = g_prms.mf_azimuth;
380 *pitch = g_prms.mf_pitch;
381 *roll = g_prms.mf_roll;
382 *accuracy = g_prms.mi_hstatus;
383
384 /* Debug output */
385 AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n",
386 *accuracy, *azimuth, *pitch, *roll);
387
388 return AKM_SUCCESS;
389 }
390
391