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;
+}
+
+