From 381a05756bdbe474ab76242a4e2934d81b8975e9 Mon Sep 17 00:00:00 2001 From: Simon Shields Date: Sat, 5 Mar 2016 20:36:08 +1100 Subject: i9300: rewrite libsensors in c++ * based off t0lte sensors * AkmSensor kanged from i9100 (added sensor status support) * has a working compass! Change-Id: Iaed0463c33089ca0b636be4ba3f966f2e25f34f9 --- libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c | 333 ---------------------------- 1 file changed, 333 deletions(-) delete mode 100644 libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c (limited to 'libsensors/akmdfs/AKFS_APIs_8975/AKFS_AOC.c') 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; -} - -- cgit v1.1