summaryrefslogtreecommitdiffstats
path: root/libsensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c')
-rw-r--r--libsensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c133
1 files changed, 133 insertions, 0 deletions
diff --git a/libsensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c b/libsensors/akmdfs/AKFS_APIs_8975/AKFS_Direction.c
new file mode 100644
index 0000000..f47e930
--- /dev/null
+++ b/libsensors/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;
+}
+
+