summaryrefslogtreecommitdiffstats
path: root/sensors/akmdfs/AKFS_APIs_8975
diff options
context:
space:
mode:
Diffstat (limited to 'sensors/akmdfs/AKFS_APIs_8975')
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c44
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h50
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c333
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h53
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h37
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c110
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h107
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c133
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h39
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h47
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c107
-rw-r--r--sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h46
12 files changed, 1106 insertions, 0 deletions
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c
new file mode 100644
index 0000000..7bac9a1
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.c
@@ -0,0 +1,44 @@
+/******************************************************************************
+ *
+ * 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_AK8975.h"
+#include "AKFS_Device.h"
+
+/*!
+ */
+int16 AKFS_DecompAK8975(
+ const int16 mag[3],
+ const int16 status,
+ const uint8vec* asa,
+ const int16 nhdata,
+ AKFVEC hdata[]
+)
+{
+ /* put st1 and st2 value */
+ if (AK8975_ST_ERROR(status)) {
+ return AKFS_ERROR;
+ }
+
+ /* magnetic */
+ AKFS_BufShift(nhdata, 1, hdata);
+ hdata[0].u.x = mag[0] * (((asa->u.x)/256.0f) + 0.5f);
+ hdata[0].u.y = mag[1] * (((asa->u.y)/256.0f) + 0.5f);
+ hdata[0].u.z = mag[2] * (((asa->u.z)/256.0f) + 0.5f);
+
+ return AKFS_SUCCESS;
+}
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h
new file mode 100644
index 0000000..25459e3
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AK8975.h
@@ -0,0 +1,50 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_AK8975_H
+#define AKFS_INC_AK8975_H
+
+#include "AKFS_Device.h"
+
+/***** Constant definition ****************************************************/
+#define AK8975_BDATA_SIZE 8
+
+#define AK8975_HSENSE_DEFAULT 1
+#define AK8975_HSENSE_TARGET 0.3f
+#define AK8975_ASENSE_DEFAULT 720
+#define AK8975_ASENSE_TARGET 9.80665f
+
+#define AK8975_HDATA_CONVERTER(hi, low, asa) \
+ (AKFLOAT)((int16)((((uint16)(hi))<<8)+(uint16)(low))*(((asa)/256.0f) + 0.5f))
+
+#define AK8975_ST_ERROR(st) (((st)&0x09) != 0x01)
+
+/***** Type declaration *******************************************************/
+
+/***** Prototype of function **************************************************/
+AKLIB_C_API_START
+int16 AKFS_DecompAK8975(
+ const int16 mag[3],
+ const int16 status,
+ const uint8vec* asa,
+ const int16 nhdata,
+ AKFVEC hdata[]
+);
+AKLIB_C_API_END
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c
new file mode 100644
index 0000000..62b2361
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c
@@ -0,0 +1,333 @@
+/******************************************************************************
+ *
+ * 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_AOC.h"
+#include "AKFS_Math.h"
+
+/*
+ * CalcR
+ */
+static AKFLOAT CalcR(
+ const AKFVEC* x,
+ const AKFVEC* y
+){
+ int16 i;
+ AKFLOAT r;
+
+ r = 0.0;
+ for(i = 0; i < 3; i++){
+ r += (x->v[i]-y->v[i]) * (x->v[i]-y->v[i]);
+ }
+ r = sqrt(r);
+
+ return r;
+}
+
+/*
+ * From4Points2Sphere()
+ */
+static int16 From4Points2Sphere(
+ const AKFVEC points[], /*! (i/o) : input vectors */
+ AKFVEC* center, /*! (o) : center of sphere */
+ AKFLOAT* r /*! (i) : add/subtract value */
+){
+ AKFLOAT dif[3][3];
+ AKFLOAT r2[3];
+
+ AKFLOAT A;
+ AKFLOAT B;
+ AKFLOAT C;
+ AKFLOAT D;
+ AKFLOAT E;
+ AKFLOAT F;
+ AKFLOAT G;
+
+ AKFLOAT OU;
+ AKFLOAT OD;
+
+ int16 i, j;
+
+ for(i = 0; i < 3; i++){
+ r2[i] = 0.0;
+ for(j = 0; j < 3; j++){
+ dif[i][j] = points[i].v[j] - points[3].v[j];
+ r2[i] += (points[i].v[j]*points[i].v[j]
+ - points[3].v[j]*points[3].v[j]);
+ }
+ r2[i] *= 0.5;
+ }
+
+ A = dif[0][0]*dif[2][2] - dif[0][2]*dif[2][0];
+ B = dif[0][1]*dif[2][0] - dif[0][0]*dif[2][1];
+ C = dif[0][0]*dif[2][1] - dif[0][1]*dif[2][0];
+ D = dif[0][0]*r2[2] - dif[2][0]*r2[0];
+ E = dif[0][0]*dif[1][1] - dif[0][1]*dif[1][0];
+ F = dif[1][0]*dif[0][2] - dif[0][0]*dif[1][2];
+ G = dif[0][0]*r2[1] - dif[1][0]*r2[0];
+
+ OU = D*E + B*G;
+ OD = C*F + A*E;
+
+ if(fabs(OD) < AKFS_EPSILON){
+ return -1;
+ }
+
+ center->v[2] = OU / OD;
+
+ OU = F*center->v[2] + G;
+ OD = E;
+
+ if(fabs(OD) < AKFS_EPSILON){
+ return -1;
+ }
+
+ center->v[1] = OU / OD;
+
+ OU = r2[0] - dif[0][1]*center->v[1] - dif[0][2]*center->v[2];
+ OD = dif[0][0];
+
+ if(fabs(OD) < AKFS_EPSILON){
+ return -1;
+ }
+
+ center->v[0] = OU / OD;
+
+ *r = CalcR(&points[0], center);
+
+ return 0;
+
+}
+
+/*
+ * MeanVar
+ */
+static void MeanVar(
+ const AKFVEC v[], /*!< (i) : input vectors */
+ const int16 n, /*!< (i) : number of vectors */
+ AKFVEC* mean, /*!< (o) : (max+min)/2 */
+ AKFVEC* var /*!< (o) : variation in vectors */
+){
+ int16 i;
+ int16 j;
+ AKFVEC max;
+ AKFVEC min;
+
+ for(j = 0; j < 3; j++){
+ min.v[j] = v[0].v[j];
+ max.v[j] = v[0].v[j];
+ for(i = 1; i < n; i++){
+ if(v[i].v[j] < min.v[j]){
+ min.v[j] = v[i].v[j];
+ }
+ if(v[i].v[j] > max.v[j]){
+ max.v[j] = v[i].v[j];
+ }
+ }
+ mean->v[j] = (max.v[j] + min.v[j]) / 2.0; /*mean */
+ var->v[j] = max.v[j] - min.v[j]; /*var */
+ }
+}
+
+/*
+ * Get4points
+ */
+static void Get4points(
+ const AKFVEC v[], /*!< (i) : input vectors */
+ const int16 n, /*!< (i) : number of vectors */
+ AKFVEC out[] /*!< (o) : */
+){
+ int16 i, j;
+ AKFLOAT temp;
+ AKFLOAT d;
+
+ AKFVEC dv[AKFS_HBUF_SIZE];
+ AKFVEC cross;
+ AKFVEC tempv;
+
+ /* out 0 */
+ out[0] = v[0];
+
+ /* out 1 */
+ d = 0.0;
+ for(i = 1; i < n; i++){
+ temp = CalcR(&v[i], &out[0]);
+ if(d < temp){
+ d = temp;
+ out[1] = v[i];
+ }
+ }
+
+ /* out 2 */
+ d = 0.0;
+ for(j = 0; j < 3; j++){
+ dv[0].v[j] = out[1].v[j] - out[0].v[j];
+ }
+ for(i = 1; i < n; i++){
+ for(j = 0; j < 3; j++){
+ dv[i].v[j] = v[i].v[j] - out[0].v[j];
+ }
+ tempv.v[0] = dv[0].v[1]*dv[i].v[2] - dv[0].v[2]*dv[i].v[1];
+ tempv.v[1] = dv[0].v[2]*dv[i].v[0] - dv[0].v[0]*dv[i].v[2];
+ tempv.v[2] = dv[0].v[0]*dv[i].v[1] - dv[0].v[1]*dv[i].v[0];
+ temp = tempv.u.x * tempv.u.x
+ + tempv.u.y * tempv.u.y
+ + tempv.u.z * tempv.u.z;
+ if(d < temp){
+ d = temp;
+ out[2] = v[i];
+ cross = tempv;
+ }
+ }
+
+ /* out 3 */
+ d = 0.0;
+ for(i = 1; i < n; i++){
+ temp = dv[i].u.x * cross.u.x
+ + dv[i].u.y * cross.u.y
+ + dv[i].u.z * cross.u.z;
+ temp = fabs(temp);
+ if(d < temp){
+ d = temp;
+ out[3] = v[i];
+ }
+ }
+}
+
+/*
+ * CheckInitFvec
+ */
+static int16 CheckInitFvec(
+ const AKFVEC *v /*!< [in] vector */
+){
+ int16 i;
+
+ for(i = 0; i < 3; i++){
+ if(AKFS_FMAX <= v->v[i]){
+ return 1; /* initvalue */
+ }
+ }
+
+ return 0; /* not initvalue */
+}
+
+/*
+ * AKFS_AOC
+ */
+int16 AKFS_AOC( /*!< (o) : calibration success(1), failure(0) */
+ AKFS_AOC_VAR* haocv, /*!< (i/o) : a set of variables */
+ const AKFVEC* hdata, /*!< (i) : vectors of data */
+ AKFVEC* ho /*!< (i/o) : offset */
+){
+ int16 i, j;
+ int16 num;
+ AKFLOAT tempf;
+ AKFVEC tempho;
+
+ AKFVEC fourpoints[4];
+
+ AKFVEC var;
+ AKFVEC mean;
+
+ /* buffer new data */
+ for(i = 1; i < AKFS_HBUF_SIZE; i++){
+ haocv->hbuf[AKFS_HBUF_SIZE-i] = haocv->hbuf[AKFS_HBUF_SIZE-i-1];
+ }
+ haocv->hbuf[0] = *hdata;
+
+ /* Check Init */
+ num = 0;
+ for(i = AKFS_HBUF_SIZE; 3 < i; i--){
+ if(CheckInitFvec(&haocv->hbuf[i-1]) == 0){
+ num = i;
+ break;
+ }
+ }
+ if(num < 4){
+ return AKFS_ERROR;
+ }
+
+ /* get 4 points */
+ Get4points(haocv->hbuf, num, fourpoints);
+
+ /* estimate offset */
+ if(0 != From4Points2Sphere(fourpoints, &tempho, &haocv->hraoc)){
+ return AKFS_ERROR;
+ }
+
+ /* check distance */
+ for(i = 0; i < 4; i++){
+ for(j = (i+1); j < 4; j++){
+ tempf = CalcR(&fourpoints[i], &fourpoints[j]);
+ if((tempf < haocv->hraoc)||(tempf < AKFS_HR_TH)){
+ return AKFS_ERROR;
+ }
+ }
+ }
+
+ /* update offset buffer */
+ for(i = 1; i < AKFS_HOBUF_SIZE; i++){
+ haocv->hobuf[AKFS_HOBUF_SIZE-i] = haocv->hobuf[AKFS_HOBUF_SIZE-i-1];
+ }
+ haocv->hobuf[0] = tempho;
+
+ /* clear hbuf */
+ for(i = (AKFS_HBUF_SIZE>>1); i < AKFS_HBUF_SIZE; i++) {
+ for(j = 0; j < 3; j++) {
+ haocv->hbuf[i].v[j]= AKFS_FMAX;
+ }
+ }
+
+ /* Check Init */
+ if(CheckInitFvec(&haocv->hobuf[AKFS_HOBUF_SIZE-1]) == 1){
+ return AKFS_ERROR;
+ }
+
+ /* Check ovar */
+ tempf = haocv->hraoc * AKFS_HO_TH;
+ MeanVar(haocv->hobuf, AKFS_HOBUF_SIZE, &mean, &var);
+ if ((var.u.x >= tempf) || (var.u.y >= tempf) || (var.u.z >= tempf)){
+ return AKFS_ERROR;
+ }
+
+ *ho = mean;
+
+ return AKFS_SUCCESS;
+}
+
+/*
+ * AKFS_InitAOC
+ */
+void AKFS_InitAOC(
+ AKFS_AOC_VAR* haocv
+){
+ int16 i, j;
+
+ /* Initialize buffer */
+ for(i = 0; i < AKFS_HBUF_SIZE; i++) {
+ for(j = 0; j < 3; j++) {
+ haocv->hbuf[i].v[j]= AKFS_FMAX;
+ }
+ }
+ for(i = 0; i < AKFS_HOBUF_SIZE; i++) {
+ for(j = 0; j < 3; j++) {
+ haocv->hobuf[i].v[j]= AKFS_FMAX;
+ }
+ }
+
+ haocv->hraoc = 0.0;
+}
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h
new file mode 100644
index 0000000..ffaaa88
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.h
@@ -0,0 +1,53 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_AOC_H
+#define AKFS_INC_AOC_H
+
+#include "AKFS_Device.h"
+
+/***** Constant definition ****************************************************/
+#define AKFS_HBUF_SIZE 20
+#define AKFS_HOBUF_SIZE 4
+#define AKFS_HR_TH 10
+#define AKFS_HO_TH 0.15
+
+/***** Macro definition *******************************************************/
+
+/***** Type declaration *******************************************************/
+typedef struct _AKFS_AOC_VAR{
+ AKFVEC hbuf[AKFS_HBUF_SIZE];
+ AKFVEC hobuf[AKFS_HOBUF_SIZE];
+ AKFLOAT hraoc;
+} AKFS_AOC_VAR;
+
+/***** Prototype of function **************************************************/
+AKLIB_C_API_START
+int16 AKFS_AOC(
+ AKFS_AOC_VAR* haocv,
+ const AKFVEC* hdata,
+ AKFVEC* ho
+);
+
+void AKFS_InitAOC(
+ AKFS_AOC_VAR* haocv
+);
+
+AKLIB_C_API_END
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h
new file mode 100644
index 0000000..1f80f48
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Configure.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_CONFIG_H
+#define AKFS_INC_CONFIG_H
+
+/***** Language configuration *************************************************/
+#if defined(__cplusplus)
+#define AKLIB_C_API_START extern "C" {
+#define AKLIB_C_API_END }
+#else
+#define AKLIB_C_API_START
+#define AKLIB_C_API_END
+#endif
+
+/*! If following line is commented in, double type is used for floating point
+ calculation */
+/*
+#define AKFS_PRECISION_DOUBLE
+*/
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c
new file mode 100644
index 0000000..3d99ab1
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.c
@@ -0,0 +1,110 @@
+/******************************************************************************
+ *
+ * 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_Device.h"
+
+int16 AKFS_InitBuffer(
+ const int16 ndata, /*!< Size of vector buffer */
+ AKFVEC vdata[] /*!< Vector buffer */
+)
+{
+ int i;
+
+ /* size check */
+ if (ndata <= 0) {
+ return AKFS_ERROR;
+ }
+
+ for (i=0; i<ndata; i++) {
+ vdata[i].u.x = AKFS_INIT_VALUE_F;
+ vdata[i].u.y = AKFS_INIT_VALUE_F;
+ vdata[i].u.z = AKFS_INIT_VALUE_F;
+ }
+
+ return AKFS_SUCCESS;
+}
+
+int16 AKFS_BufShift(
+ const int16 len, /*!< size of buffer */
+ const int16 shift, /*!< shift size */
+ AKFVEC v[] /*!< buffer */
+)
+{
+ int16 i;
+
+ if((shift < 1) || (len < shift)) {
+ return AKFS_ERROR;
+ }
+ for (i = len-1; i >= shift; i--) {
+ v[i] = v[i-shift];
+ }
+ return AKFS_SUCCESS;
+}
+
+int16 AKFS_Rotate(
+ const AKFS_PATNO pat,
+ AKFVEC* vec
+)
+{
+ AKFLOAT tmp;
+ switch(pat){
+ /* Obverse */
+ case PAT1:
+ /* This is Android default */
+ break;
+ case PAT2:
+ tmp = vec->u.x;
+ vec->u.x = vec->u.y;
+ vec->u.y = -tmp;
+ break;
+ case PAT3:
+ vec->u.x = -(vec->u.x);
+ vec->u.y = -(vec->u.y);
+ break;
+ case PAT4:
+ tmp = vec->u.x;
+ vec->u.x = -(vec->u.y);
+ vec->u.y = tmp;
+ break;
+ /* Reverse */
+ case PAT5:
+ vec->u.x = -(vec->u.x);
+ vec->u.z = -(vec->u.z);
+ break;
+ case PAT6:
+ tmp = vec->u.x;
+ vec->u.x = vec->u.y;
+ vec->u.y = tmp;
+ vec->u.z = -(vec->u.z);
+ break;
+ case PAT7:
+ vec->u.y = -(vec->u.y);
+ vec->u.z = -(vec->u.z);
+ break;
+ case PAT8:
+ tmp = vec->u.x;
+ vec->u.x = -(vec->u.y);
+ vec->u.y = -tmp;
+ vec->u.z = -(vec->u.z);
+ break;
+ default:
+ return AKFS_ERROR;
+ }
+
+ return AKFS_SUCCESS;
+}
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h
new file mode 100644
index 0000000..0292d54
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Device.h
@@ -0,0 +1,107 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_DEVICE_H
+#define AKFS_INC_DEVICE_H
+
+#include <float.h>
+#include "AKFS_Configure.h"
+
+/***** Constant definition ****************************************************/
+#define AKFS_ERROR 0
+#define AKFS_SUCCESS 1
+
+#define AKFS_HDATA_SIZE 32
+#define AKFS_ADATA_SIZE 32
+
+/***** Type declaration *******************************************************/
+typedef signed char int8;
+typedef signed short int16;
+typedef unsigned char uint8;
+typedef unsigned short uint16;
+
+
+#ifdef AKFS_PRECISION_DOUBLE
+typedef double AKFLOAT;
+#define AKFS_EPSILON DBL_EPSILON
+#define AKFS_FMAX DBL_MAX
+#define AKFS_FMIN DBL_MIN
+
+#else
+typedef float AKFLOAT;
+#define AKFS_EPSILON FLT_EPSILON
+#define AKFS_FMAX FLT_MAX
+#define AKFS_FMIN FLT_MIN
+
+#endif
+
+/* Treat maximum value as initial value */
+#define AKFS_INIT_VALUE_F AKFS_FMAX
+
+/***** Vector *****/
+typedef union _uint8vec{
+ struct {
+ uint8 x;
+ uint8 y;
+ uint8 z;
+ }u;
+ uint8 v[3];
+} uint8vec;
+
+typedef union _AKFVEC{
+ struct {
+ AKFLOAT x;
+ AKFLOAT y;
+ AKFLOAT z;
+ }u;
+ AKFLOAT v[3];
+} AKFVEC;
+
+/***** Layout pattern *****/
+typedef enum _AKFS_PATNO {
+ PAT_INVALID = 0,
+ PAT1, /* obverse: 1st pin is right down */
+ PAT2, /* obverse: 1st pin is left down */
+ PAT3, /* obverse: 1st pin is left top */
+ PAT4, /* obverse: 1st pin is right top */
+ PAT5, /* reverse: 1st pin is left down (from top view) */
+ PAT6, /* reverse: 1st pin is left top (from top view) */
+ PAT7, /* reverse: 1st pin is right top (from top view) */
+ PAT8 /* reverse: 1st pin is right down (from top view) */
+} AKFS_PATNO;
+
+/***** Prototype of function **************************************************/
+AKLIB_C_API_START
+int16 AKFS_InitBuffer(
+ const int16 ndata, /*!< Size of raw vector buffer */
+ AKFVEC vdata[] /*!< Raw vector buffer */
+);
+
+int16 AKFS_BufShift(
+ const int16 len,
+ const int16 shift,
+ AKFVEC v[]
+);
+
+int16 AKFS_Rotate(
+ const AKFS_PATNO pat,
+ AKFVEC* vec
+);
+AKLIB_C_API_END
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c
new file mode 100644
index 0000000..f47e930
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c
@@ -0,0 +1,133 @@
+/******************************************************************************
+ *
+ * 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_Direction.h"
+#include "AKFS_VNorm.h"
+#include "AKFS_Math.h"
+
+/*
+ Coordinate system is right-handed.
+ X-Axis: from left to right.
+ Y-Axis: from bottom to top.
+ Z-Axis: from reverse to obverse.
+
+ azimuth: Rotaion around Z axis, with positive values
+ when y-axis moves toward the x-axis.
+ pitch: Rotation around X axis, with positive values
+ when z-axis moves toward the y-axis.
+ roll: Rotation around Y axis, with positive values
+ when x-axis moves toward the z-axis.
+*/
+
+/*
+ This function is used internaly, so output is RADIAN!
+ */
+static void AKFS_Angle(
+ const AKFVEC* avec,
+ AKFLOAT* pitch, /* radian */
+ AKFLOAT* roll /* radian */
+)
+{
+ AKFLOAT av; /* Size of vector */
+
+ av = AKFS_SQRT((avec->u.x)*(avec->u.x) + (avec->u.y)*(avec->u.y) + (avec->u.z)*(avec->u.z));
+
+ *pitch = AKFS_ASIN(-(avec->u.y) / av);
+ *roll = AKFS_ASIN((avec->u.x) / av);
+}
+
+/*
+ This function is used internaly, so output is RADIAN!
+ */
+static void AKFS_Azimuth(
+ const AKFVEC* hvec,
+ const AKFLOAT pitch, /* radian */
+ const AKFLOAT roll, /* radian */
+ AKFLOAT* azimuth /* radian */
+)
+{
+ AKFLOAT sinP; /* sin value of pitch angle */
+ AKFLOAT cosP; /* cos value of pitch angle */
+ AKFLOAT sinR; /* sin value of roll angle */
+ AKFLOAT cosR; /* cos value of roll angle */
+ AKFLOAT Xh; /* X axis element of vector which is projected to horizontal plane */
+ AKFLOAT Yh; /* Y axis element of vector which is projected to horizontal plane */
+
+ sinP = AKFS_SIN(pitch);
+ cosP = AKFS_COS(pitch);
+ sinR = AKFS_SIN(roll);
+ cosR = AKFS_COS(roll);
+
+ Yh = -(hvec->u.x)*cosR + (hvec->u.z)*sinR;
+ Xh = (hvec->u.x)*sinP*sinR + (hvec->u.y)*cosP + (hvec->u.z)*sinP*cosR;
+
+ /* atan2(y, x) -> divisor and dividend is opposite from mathematical equation. */
+ *azimuth = AKFS_ATAN2(Yh, Xh);
+}
+
+int16 AKFS_Direction(
+ const int16 nhvec,
+ const AKFVEC hvec[],
+ const int16 hnave,
+ const int16 navec,
+ const AKFVEC avec[],
+ const int16 anave,
+ AKFLOAT* azimuth,
+ AKFLOAT* pitch,
+ AKFLOAT* roll
+)
+{
+ AKFVEC have, aave;
+ AKFLOAT azimuthRad;
+ AKFLOAT pitchRad;
+ AKFLOAT rollRad;
+
+ /* arguments check */
+ if ((nhvec <= 0) || (navec <= 0) || (hnave <= 0) || (anave <= 0)) {
+ return AKFS_ERROR;
+ }
+ if ((nhvec < hnave) || (navec < anave)) {
+ return AKFS_ERROR;
+ }
+
+ /* average */
+ if (AKFS_VbAve(nhvec, hvec, hnave, &have) != AKFS_SUCCESS) {
+ return AKFS_ERROR;
+ }
+ if (AKFS_VbAve(navec, avec, anave, &aave) != AKFS_SUCCESS) {
+ return AKFS_ERROR;
+ }
+
+ /* calculate pitch and roll */
+ AKFS_Angle(&aave, &pitchRad, &rollRad);
+
+ /* calculate azimuth */
+ AKFS_Azimuth(&have, pitchRad, rollRad, &azimuthRad);
+
+ *azimuth = RAD2DEG(azimuthRad);
+ *pitch = RAD2DEG(pitchRad);
+ *roll = RAD2DEG(rollRad);
+
+ /* Adjust range of azimuth */
+ if (*azimuth < 0) {
+ *azimuth += 360.0f;
+ }
+
+ return AKFS_SUCCESS;
+}
+
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h
new file mode 100644
index 0000000..c08338d
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_DIRECTION_H
+#define AKFS_INC_DIRECTION_H
+
+#include "AKFS_Device.h"
+
+/***** Prototype of function **************************************************/
+AKLIB_C_API_START
+int16 AKFS_Direction(
+ const int16 nhvec,
+ const AKFVEC hvec[],
+ const int16 hnave,
+ const int16 navec,
+ const AKFVEC avec[],
+ const int16 anave,
+ AKFLOAT* azimuth,
+ AKFLOAT* pitch,
+ AKFLOAT* roll
+);
+AKLIB_C_API_END
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h
new file mode 100644
index 0000000..dfe48b3
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_Math.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_MATH_H
+#define AKFS_INC_MATH_H
+
+#include <math.h>
+#include "AKFS_Configure.h"
+
+/***** Constant definition ****************************************************/
+#define AKFS_PI 3.141592654f
+#define RAD2DEG(rad) ((rad)*180.0f/AKFS_PI)
+
+/***** Macro definition *******************************************************/
+
+#ifdef AKFS_PRECISION_DOUBLE
+#define AKFS_SIN(x) sin(x)
+#define AKFS_COS(x) cos(x)
+#define AKFS_ASIN(x) asin(x)
+#define AKFS_ACOS(x) acos(x)
+#define AKFS_ATAN2(y, x) atan2((y), (x))
+#define AKFS_SQRT(x) sqrt(x)
+#else
+#define AKFS_SIN(x) sinf(x)
+#define AKFS_COS(x) cosf(x)
+#define AKFS_ASIN(x) asinf(x)
+#define AKFS_ACOS(x) acosf(x)
+#define AKFS_ATAN2(y, x) atan2f((y), (x))
+#define AKFS_SQRT(x) sqrtf(x)
+#endif
+
+#endif
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c
new file mode 100644
index 0000000..ffa934a
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.c
@@ -0,0 +1,107 @@
+/******************************************************************************
+ *
+ * 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_VNorm.h"
+#include "AKFS_Device.h"
+
+/*!
+ */
+int16 AKFS_VbNorm(
+ const int16 ndata, /*!< Size of raw vector buffer */
+ const AKFVEC vdata[], /*!< Raw vector buffer */
+ const int16 nbuf, /*!< Size of data to be buffered */
+ const AKFVEC* o, /*!< Offset */
+ const AKFVEC* s, /*!< Sensitivity */
+ const AKFLOAT tgt, /*!< Target sensitivity */
+ const int16 nvec, /*!< Size of normalized vector buffer */
+ AKFVEC vvec[] /*!< Normalized vector buffer */
+)
+{
+ int i;
+
+ /* size check */
+ if ((ndata <= 0) || (nvec <= 0) || (nbuf <= 0)) {
+ return AKFS_ERROR;
+ }
+ /* dependency check */
+ if ((nbuf < 1) || (ndata < nbuf) || (nvec < nbuf)) {
+ return AKFS_ERROR;
+ }
+ /* sensitivity check */
+ if ((s->u.x <= AKFS_EPSILON) ||
+ (s->u.y <= AKFS_EPSILON) ||
+ (s->u.z <= AKFS_EPSILON) ||
+ (tgt <= 0)) {
+ return AKFS_ERROR;
+ }
+
+ /* calculate and store data to buffer */
+ if (AKFS_BufShift(nvec, nbuf, vvec) != AKFS_SUCCESS) {
+ return AKFS_ERROR;
+ }
+ for (i=0; i<nbuf; i++) {
+ vvec[i].u.x = ((vdata[i].u.x - o->u.x) / (s->u.x) * (AKFLOAT)tgt);
+ vvec[i].u.y = ((vdata[i].u.y - o->u.y) / (s->u.y) * (AKFLOAT)tgt);
+ vvec[i].u.z = ((vdata[i].u.z - o->u.z) / (s->u.z) * (AKFLOAT)tgt);
+ }
+
+ return AKFS_SUCCESS;
+}
+
+/*!
+ */
+int16 AKFS_VbAve(
+ const int16 nvec, /*!< Size of normalized vector buffer */
+ const AKFVEC vvec[], /*!< Normalized vector buffer */
+ const int16 nave, /*!< Number of averaeg */
+ AKFVEC* vave /*!< Averaged vector */
+)
+{
+ int i;
+
+ /* arguments check */
+ if ((nave <= 0) || (nvec <= 0) || (nvec < nave)) {
+ return AKFS_ERROR;
+ }
+
+ /* calculate average */
+ vave->u.x = 0;
+ vave->u.y = 0;
+ vave->u.z = 0;
+ for (i=0; i<nave; i++) {
+ if ((vvec[i].u.x == AKFS_INIT_VALUE_F) ||
+ (vvec[i].u.y == AKFS_INIT_VALUE_F) ||
+ (vvec[i].u.z == AKFS_INIT_VALUE_F)) {
+ break;
+ }
+ vave->u.x += vvec[i].u.x;
+ vave->u.y += vvec[i].u.y;
+ vave->u.z += vvec[i].u.z;
+ }
+ if (i == 0) {
+ vave->u.x = 0;
+ vave->u.y = 0;
+ vave->u.z = 0;
+ } else {
+ vave->u.x /= i;
+ vave->u.y /= i;
+ vave->u.z /= i;
+ }
+ return AKFS_SUCCESS;
+}
+
+
diff --git a/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h
new file mode 100644
index 0000000..c3c9bed
--- /dev/null
+++ b/sensors/akmdfs/AKFS_APIs_8975/AKFS_VNorm.h
@@ -0,0 +1,46 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef AKFS_INC_VNORM_H
+#define AKFS_INC_VNORM_H
+
+#include "AKFS_Device.h"
+
+/***** Prototype of function **************************************************/
+AKLIB_C_API_START
+int16 AKFS_VbNorm(
+ const int16 ndata, /*!< Size of raw vector buffer */
+ const AKFVEC vdata[], /*!< Raw vector buffer */
+ const int16 nbuf, /*!< Size of data to be buffered */
+ const AKFVEC* o, /*!< Offset */
+ const AKFVEC* s, /*!< Sensitivity */
+ const AKFLOAT tgt, /*!< Target sensitivity */
+ const int16 nvec, /*!< Size of normalized vector buffer */
+ AKFVEC vvec[] /*!< Normalized vector buffer */
+);
+
+int16 AKFS_VbAve(
+ const int16 nvec, /*!< Size of normalized vector buffer */
+ const AKFVEC vvec[], /*!< Normalized vector buffer */
+ const int16 nave, /*!< Number of averaeg */
+ AKFVEC* vave /*!< Averaged vector */
+);
+
+AKLIB_C_API_END
+
+#endif
+
generated by cgit v1.1 at 2024-04-27 13:00:20 +0000