summaryrefslogtreecommitdiffstats
path: root/libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c')
-rw-r--r--libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c333
1 files changed, 0 insertions, 333 deletions
diff --git a/libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c b/libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c
deleted file mode 100644
index 62b2361..0000000
--- a/libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c
+++ /dev/null
@@ -1,333 +0,0 @@
-/******************************************************************************
- *
- * 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;
-}
-
generated by cgit v1.1 at 2024-04-19 13:38:50 +0000