summaryrefslogtreecommitdiffstats
path: root/libsensors/akmdfs/AKFS_APIs.c
blob: ace9bc1a1a1f06e6d7a624f53c1e00929d88e5f2 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
/******************************************************************************
 *
 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 ******************************************************************************/
#include "AKFS_Common.h"
#include "AKFS_Disp.h"
#include "AKFS_FileIO.h"
#include "AKFS_APIs.h"

#ifdef WIN32
#include "AK8975_LinuxDriver.h"
#endif

static AK8975PRMS g_prms;

/*!
  Initialize library. At first, 0 is set to all parameters.  After that, some
  parameters, which should not be 0, are set to specific value. Some of initial
  values can be customized by editing the file \c "AKFS_CSpec.h".
  @return The return value is #AKM_SUCCESS.
  @param[in] hpat Specify a layout pattern number.  The number is determined
  according to the mount orientation of the magnetometer.
  @param[in] regs[3] Specify the ASA values which are read out from
  fuse ROM.  regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ.
 */
int16 AKFS_Init(
	const	AKFS_PATNO	hpat,
	const	uint8		regs[]
)
{
	AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n",
		__FUNCTION__, hpat, regs[0], regs[1], regs[2]);

	/* Set 0 to the AK8975 structure. */
	memset(&g_prms, 0, sizeof(AK8975PRMS));

	/* Sensitivity */
	g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT;
	g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT;
	g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT;

	/* Initialize variables that initial value is not 0. */
	g_prms.mi_hnaveV = CSPEC_HNAVE_V;
	g_prms.mi_hnaveD = CSPEC_HNAVE_D;
	g_prms.mi_anaveV = CSPEC_ANAVE_V;
	g_prms.mi_anaveD = CSPEC_ANAVE_D;

	/* Copy ASA values */
	g_prms.mi_asa.u.x = regs[0];
	g_prms.mi_asa.u.y = regs[1];
	g_prms.mi_asa.u.z = regs[2];

	/* Copy layout pattern */
	g_prms.m_hpat = hpat;

	return AKM_SUCCESS;
}

/*!
  Release resources. This function is for future expansion.
  @return The return value is #AKM_SUCCESS.
 */
int16 AKFS_Release(void)
{
	return AKM_SUCCESS;
}

/*
  This function is called just before a measurement sequence starts.
  This function reads parameters from file, then initializes algorithm
  parameters.
  @return The return value is #AKM_SUCCESS.
  @param[in] path Specify a path to the settings file.
 */
int16 AKFS_Start(
	const char* path
)
{
	AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);

	/* Read setting files from a file */
	if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) {
		AKMERROR_STR("AKFS_Load");
	}

	/* Initialize buffer */
	AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata);
	AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf);
	AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata);
	AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf);

	/* Initialize for AOC */
	AKFS_InitAOC(&g_prms.m_aocv);
	/* Initialize magnetic status */
	g_prms.mi_hstatus = 0;

	return AKM_SUCCESS;
}

/*!
  This function is called when a measurement sequence is done.
  This fucntion writes parameters to file.
  @return The return value is #AKM_SUCCESS.
  @param[in] path Specify a path to the settings file.
 */
int16 AKFS_Stop(
	const char* path
)
{
	AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);

	/* Write setting files to a file */
	if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) {
		AKMERROR_STR("AKFS_Save");
	}

	return AKM_SUCCESS;
}

/*!
  This function is called when new magnetometer data is available.  The output
  vector format and coordination system follow the Android definition.
  @return The return value is #AKM_SUCCESS.
   Otherwise the return value is #AKM_FAIL.
  @param[in] mag A set of measurement data from magnetometer.  X axis value
   should be in mag[0], Y axis value should be in mag[1], Z axis value should be 
   in mag[2].
  @param[in] status A status of magnetometer.  This status indicates the result
   of measurement data, i.e. overflow, success or fail, etc.
  @param[out] vx X axis value of magnetic field vector.
  @param[out] vy Y axis value of magnetic field vector.
  @param[out] vz Z axis value of magnetic field vector.
  @param[out] accuracy Accuracy of magnetic field vector.
 */
int16 AKFS_Get_MAGNETIC_FIELD(
	const	int16		mag[3],
	const	int16		status,
			AKFLOAT*	vx,
			AKFLOAT*	vy,
			AKFLOAT*	vz,
			int16*		accuracy
)
{
	int16 akret;
	int16 aocret;
	AKFLOAT radius;

	AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n",
		__FUNCTION__, mag[0], mag[1], mag[2], status);

	/* Decomposition */
	/* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */
	akret = AKFS_DecompAK8975(
		mag,
		status,
		&g_prms.mi_asa,
		AKFS_HDATA_SIZE,
		g_prms.mfv_hdata
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Adjust coordination */
	akret = AKFS_Rotate(
		g_prms.m_hpat,
		&g_prms.mfv_hdata[0]
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* AOC for magnetometer */
	/* Offset estimation is done in this function */
	aocret = AKFS_AOC(
		&g_prms.m_aocv,
		g_prms.mfv_hdata,
		&g_prms.mfv_ho
	);

	/* Subtract offset */
	/* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */
	akret = AKFS_VbNorm(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hdata,
		1,
		&g_prms.mfv_ho,
		&g_prms.mfv_hs,
		AK8975_HSENSE_TARGET,
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Averaging */
	akret = AKFS_VbAve(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf,
		CSPEC_HNAVE_V,
		&g_prms.mfv_hvec
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Check the size of magnetic vector */
	radius = AKFS_SQRT(
			(g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) +
			(g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) +
			(g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z));

	if (radius > AKFS_GEOMAG_MAX) {
		g_prms.mi_hstatus = 0;
	} else {
		if (aocret) {
			g_prms.mi_hstatus = 3;
		}
	}

	*vx = g_prms.mfv_hvec.u.x;
	*vy = g_prms.mfv_hvec.u.y;
	*vz = g_prms.mfv_hvec.u.z;
	*accuracy = g_prms.mi_hstatus;

	/* Debug output */
	AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *vx, *vy, *vz);

	return AKM_SUCCESS;
}

/*!
  This function is called when new accelerometer data is available.  The output
  vector format and coordination system follow the Android definition.
  @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
   the return value is #AKM_FAIL.
  @param[in] acc A set of measurement data from accelerometer.  X axis value
   should be in acc[0], Y axis value should be in acc[1], Z axis value should be 
   in acc[2].
  @param[in] status A status of accelerometer.  This status indicates the result
   of acceleration data, i.e. overflow, success or fail, etc.
  @param[out] vx X axis value of acceleration vector.
  @param[out] vy Y axis value of acceleration vector.
  @param[out] vz Z axis value of acceleration vector.
  @param[out] accuracy Accuracy of acceleration vector.
  This value is always 3.
 */
int16 AKFS_Get_ACCELEROMETER(
	const   int16		acc[3],
	const	int16		status,
			AKFLOAT*	vx,
			AKFLOAT*	vy,
			AKFLOAT*	vz,
			int16*		accuracy
)
{
	int16 akret;

	AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n",
		__FUNCTION__, acc[0], acc[1], acc[2], status);

	/* Save data to buffer */
	AKFS_BufShift(
		AKFS_ADATA_SIZE,
		1,
		g_prms.mfv_adata
	);
	g_prms.mfv_adata[0].u.x = acc[0];
	g_prms.mfv_adata[0].u.y = acc[1];
	g_prms.mfv_adata[0].u.z = acc[2];

	/* Subtract offset, adjust sensitivity */
	/* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */
	akret = AKFS_VbNorm(
		AKFS_ADATA_SIZE,
		g_prms.mfv_adata,
		1,
		&g_prms.mfv_ao,
		&g_prms.mfv_as,
		AK8975_ASENSE_TARGET,
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Averaging */
	akret = AKFS_VbAve(
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf,
		CSPEC_ANAVE_V,
		&g_prms.mfv_avec
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Adjust coordination */
	/* It is not needed. Because, the data from AK8975 driver is already
	   follows Android coordinate system. */

	*vx = g_prms.mfv_avec.u.x;
	*vy = g_prms.mfv_avec.u.y;
	*vz = g_prms.mfv_avec.u.z;
	*accuracy = 3;

	/* Debug output */
	AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *vx, *vy, *vz);

	return AKM_SUCCESS;
}

/*!
  Get orientation sensor's elements. The vector format and coordination system
   follow the Android definition.  Before this function is called, magnetic
   field vector and acceleration vector should be stored in the buffer by 
   calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER.
  @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
   the return value is #AKM_FAIL.
  @param[out] azimuth Azimuthal angle in degree.
  @param[out] pitch Pitch angle in degree.
  @param[out] roll Roll angle in degree.
  @param[out] accuracy Accuracy of orientation sensor.
 */
int16 AKFS_Get_ORIENTATION(
			AKFLOAT*	azimuth,
			AKFLOAT*	pitch,
			AKFLOAT*	roll,
			int16*		accuracy
)
{
	int16 akret;

	/* Azimuth calculation */
	/* Coordination system follows the Android coordination. */
	akret = AKFS_Direction(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf,
		CSPEC_HNAVE_D,
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf,
		CSPEC_ANAVE_D,
		&g_prms.mf_azimuth,
		&g_prms.mf_pitch,
		&g_prms.mf_roll
	);

	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}
	*azimuth  = g_prms.mf_azimuth;
	*pitch    = g_prms.mf_pitch;
	*roll     = g_prms.mf_roll;
	*accuracy = g_prms.mi_hstatus;

	/* Debug output */
	AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *azimuth, *pitch, *roll);

	return AKM_SUCCESS;
}