INSTALL(FILES include/sensor.h include/sensor-internal.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor)
INSTALL(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
-ADD_SUBDIRECTORY(tests/stress_test)
-ADD_SUBDIRECTORY(tests/orientation_test)
+ADD_SUBDIRECTORY(tests)
ADD_SUBDIRECTORY(tools)
%manifest packaging/capi-system-sensor.manifest
%defattr(-,root,root,-)
%{_bindir}/sensor-test
-%{_bindir}/sensor-orientation_test
%files tools
%{_bindir}/sensor-tool
--- /dev/null
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+PROJECT(sensor-test C)
+
+SET(PKG_MODULES
+ glib-2.0)
+
+INCLUDE(FindPkgConfig)
+PKG_CHECK_MODULES(PKGS REQUIRED ${PKG_MODULES})
+
+INCLUDE_DIRECTORIES(${PKGS_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
+
+FOREACH(flag ${REQUIRED_PKGS_CFLAGS})
+ SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}")
+ENDFOREACH(flag)
+
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS} -fPIE")
+SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
+
+# Installing files
+FILE(GLOB_RECURSE SRCS main.c spec_test.c stress_test.c)
+ADD_EXECUTABLE(${PROJECT_NAME} ${SRCS})
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE C)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PKGS_LDFLAGS} capi-system-sensor)
+INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR})
--- /dev/null
+/*
+ * sensor-test
+ *
+ * Copyright (c) 2022 Samsung Electronics Co., Ltd.
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+
+#include <sensor.h>
+
+#include "spec_test.h"
+#include "stress_test.h"
+#include "sensor_info.h"
+
+static void usage(void)
+{
+ static bool printed = false;
+
+ if (!printed) {
+ printf("Usage : sensor-test [OPTION]\n");
+ printf("Example : sensor-test -t stress -s 1\n");
+ printf("[-h|--help] : Show this help\n");
+ printf("[-t|--test_type] : spec, stress\n");
+ printf("\tspec : verifying measured value of range\n");
+ printf("\tstress : giving stress to sensord\n");
+ printf("[-s|--sensor_type] : (default is all in spec, accelerometer in stress)\n");
+ printf("\t0 : %s (It is valid in only spec test, not stress test)\n", sensor_infos[0].name);
+ for (int i = 1; i < SENSOR_NUM; ++i) {
+ printf("\t%d : %s\n", i, sensor_infos[i].name);
+ }
+ printed = true;
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ if (argc < 2) {
+ usage();
+ return 0;
+ }
+
+ static const struct option long_options[] = {
+ {"test_type", required_argument, NULL, 't'},
+ {"sensor_type", required_argument, NULL, 's'},
+ {"help", no_argument, NULL, 'h'},
+ {0, 0, 0, 0},
+ };
+
+ int mode = 0;
+ int sensor_type = 0;
+
+ for (;;) {
+ int opt = getopt_long(argc, argv, "t:s:h", long_options, NULL);
+ if (opt == -1)
+ break;
+
+ switch (opt) {
+ case 't':
+ if (!strcmp(optarg, "spec"))
+ mode = 1;
+ else if (!strcmp(optarg, "stress"))
+ mode = 2;
+ break;
+ case 's':
+ sensor_type = atoi(optarg);
+ if (sensor_type < 0 || sensor_type >= SENSOR_NUM) {
+ printf("Wrong sensor number. Sensor number range is 0 ~ %d.\n", SENSOR_NUM - 1);
+ return 0;
+ }
+ break;
+ case 'h':
+ usage();
+ break;
+ default:
+ break;
+ }
+ }
+
+ if (sensor_type) {
+ bool supported;
+ int ret;
+ ret = sensor_is_supported(sensor_infos[sensor_type].type, &supported);
+ if (ret < 0 || !supported) {
+ printf("%s is not supported in this device.\n", sensor_infos[sensor_type].name);
+ return 0;
+ }
+ }
+
+ else if (mode == 2) {
+ sensor_type = 1;
+ }
+
+ if (mode == 1)
+ spec_test(sensor_type);
+ else if (mode == 2)
+ stress_test(sensor_type);
+ else {
+ printf("Enter the test type (spec or stress).\n");
+ usage();
+ }
+
+ return 0;
+}
+++ /dev/null
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-PROJECT(sensor-orientation_test)
-
-SET(PKG_MODULES
- glib-2.0)
-
-INCLUDE(FindPkgConfig)
-PKG_CHECK_MODULES(pkgs REQUIRED ${PKG_MODULES})
-
-INCLUDE_DIRECTORIES(${pkgs_INCLUDE_DIRS} ${CMAKE_SOURCE_DIR}/include)
-
-FOREACH(flag ${REQUIRED_PKGS_CFLAGS})
- SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}")
-ENDFOREACH(flag)
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS} -fPIE")
-SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
-
-FILE(GLOB SRCS *.cpp *.h)
-ADD_EXECUTABLE(${PROJECT_NAME} ${SRCS})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PKGS_LDFLAGS} capi-system-sensor)
-INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR})
+++ /dev/null
-/*
- * Copyright (C) 2007 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_ERRORS_H
-#define ANDROID_ERRORS_H
-#include <sys/types.h>
-#include <errno.h>
-namespace android {
-// use this type to return error codes
-#ifdef HAVE_MS_C_RUNTIME
-typedef int status_t;
-#else
-typedef int32_t status_t;
-#endif
-/* the MS C runtime lacks a few error codes */
-/*
- * Error codes.
- * All error codes are negative values.
- */
-// Win32 #defines NO_ERROR as well. It has the same value, so there's no
-// real conflict, though it's a bit awkward.
-#ifdef _WIN32
-# undef NO_ERROR
-#endif
-
-enum {
- OK = 0, // Everything's swell.
- NO_ERROR = 0, // No errors.
-
- UNKNOWN_ERROR = 0x80000000,
- NO_MEMORY = -ENOMEM,
- INVALID_OPERATION = -ENOSYS,
- BAD_VALUE = -EINVAL,
- BAD_TYPE = 0x80000001,
- NAME_NOT_FOUND = -ENOENT,
- PERMISSION_DENIED = -EPERM,
- NO_INIT = -ENODEV,
- ALREADY_EXISTS = -EEXIST,
- DEAD_OBJECT = -EPIPE,
- FAILED_TRANSACTION = 0x80000002,
- JPARKS_BROKE_IT = -EPIPE,
-#if !defined(HAVE_MS_C_RUNTIME)
- BAD_INDEX = -EOVERFLOW,
- NOT_ENOUGH_DATA = -ENODATA,
- WOULD_BLOCK = -EWOULDBLOCK,
- TIMED_OUT = -ETIMEDOUT,
- UNKNOWN_TRANSACTION = -EBADMSG,
-#else
- BAD_INDEX = -E2BIG,
- NOT_ENOUGH_DATA = 0x80000003,
- WOULD_BLOCK = 0x80000004,
- TIMED_OUT = 0x80000005,
- UNKNOWN_TRANSACTION = 0x80000006,
-#endif
- FDS_NOT_ALLOWED = 0x80000007,
-};
-// Restore define; enumeration is in "android" namespace, so the value defined
-// there won't work for Win32 code in a different namespace.
-#ifdef _WIN32
-# define NO_ERROR 0L
-#endif
-}; // namespace android
-
-// ---------------------------------------------------------------------------
-
-#endif // ANDROID_ERRORS_H
\ No newline at end of file
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#include <stdio.h>
-#include "fusion.h"
-
-namespace android {
-
-// -----------------------------------------------------------------------
-
-/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
-
-/* Note:
- * If a platform uses software fusion, it is necessary to tune the following
- * parameters to fit the hardware sensors prior to release.
- *
- * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
- * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
- */
-
-/*
- * GYRO_VAR gives the measured variance of the gyro's output per
- * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
- * which is independent of the sampling frequency.
- *
- * The variance of gyro's output at a given sampling period can be
- * calculated as:
- * variance(T) = GYRO_VAR / T
- *
- * The variance of the INTEGRATED OUTPUT at a given sampling period can be
- * calculated as:
- * variance_integrate_output(T) = GYRO_VAR * T
- */
-static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
-static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
-static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
-static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
-
-/*
- * Standard deviations of accelerometer and magnetometer
- */
-static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
-static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
-
-
-/* ====================== END FUSION SENSOR PARAMETER ========================*/
-
-static const float SYMMETRY_TOLERANCE = 1e-10f;
-
-/*
- * Accelerometer updates will not be performed near free fall to avoid
- * ill-conditioning and div by zeros.
- * Threshhold: 10% of g, in m/s^2
- */
-static const float NOMINAL_GRAVITY = 9.81f;
-static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
-
-/*
- * The geomagnetic-field should be between 30uT and 60uT.
- * Fields strengths greater than this likely indicate a local magnetic
- * disturbance which we do not want to update into the fused frame.
- */
-static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
-static const float MAX_VALID_MAGNETIC_FIELD_SQ =
- MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;
-
-/*
- * Values of the field smaller than this should be ignored in fusion to avoid
- * ill-conditioning. This state can happen with anomalous local magnetic
- * disturbances canceling the Earth field.
- */
-static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
-static const float MIN_VALID_MAGNETIC_FIELD_SQ =
- MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;
-
-/*
- * If the cross product of two vectors has magnitude squared less than this,
- * we reject it as invalid due to alignment of the vectors.
- * This threshold is used to check for the case where the magnetic field sample
- * is parallel to the gravity field, which can happen in certain places due
- * to magnetic field disturbances.
- */
-static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
-static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
- MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
-
-static const float SQRT_3 = 1.732f;
-static const float WVEC_EPS = 1e-4f/SQRT_3;
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t C, size_t R>
-static mat<TYPE, R, R> scaleCovariance(
- const mat<TYPE, C, R>& A,
- const mat<TYPE, C, C>& P) {
- // A*P*transpose(A);
- mat<TYPE, R, R> APAt;
- for (size_t r=0 ; r<R ; r++) {
- for (size_t j=r ; j<R ; j++) {
- double apat(0);
- for (size_t c=0 ; c<C ; c++) {
- double v(A[c][r]*P[c][c]*0.5);
- for (size_t k=c+1 ; k<C ; k++)
- v += A[k][r] * P[c][k];
- apat += 2 * v * A[c][j];
- }
- APAt[j][r] = apat;
- APAt[r][j] = apat;
- }
- }
- return APAt;
-}
-
-template <typename TYPE, typename OTHER_TYPE>
-static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
- mat<TYPE, 3, 3> r;
- r[0][0] = diag;
- r[1][1] = diag;
- r[2][2] = diag;
- r[0][1] = p.z;
- r[1][0] =-p.z;
- r[0][2] =-p.y;
- r[2][0] = p.y;
- r[1][2] = p.x;
- r[2][1] =-p.x;
- return r;
-}
-
-
-template<typename TYPE, size_t SIZE>
-class Covariance {
- mat<TYPE, SIZE, SIZE> mSumXX;
- vec<TYPE, SIZE> mSumX;
- size_t mN;
-public:
- Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
- void update(const vec<TYPE, SIZE>& x) {
- mSumXX += x*transpose(x);
- mSumX += x;
- mN++;
- }
- mat<TYPE, SIZE, SIZE> operator()() const {
- const float N = 1.0f / mN;
- return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
- }
- void reset() {
- mN = 0;
- mSumXX = 0;
- mSumX = 0;
- }
- size_t getCount() const {
- return mN;
- }
-};
-
-// -----------------------------------------------------------------------
-
-Fusion::Fusion() {
- Phi[0][1] = 0;
- Phi[1][1] = 1;
-
- Ba.x = 0;
- Ba.y = 0;
- Ba.z = 1;
-
- Bm.x = 0;
- Bm.y = 1;
- Bm.z = 0;
-
- x0 = 0;
- x1 = 0;
-
- init();
-}
-
-void Fusion::init(int mode) {
- mInitState = 0;
-
- mGyroRate = 0;
-
- mCount[0] = 0;
- mCount[1] = 0;
- mCount[2] = 0;
-
- mData = 0;
- mMode = mode;
-
- if (mMode != FUSION_NOGYRO) { //normal or game rotation
- mParam.gyroVar = DEFAULT_GYRO_VAR;
- mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
- mParam.accStdev = DEFAULT_ACC_STDEV;
- mParam.magStdev = DEFAULT_MAG_STDEV;
- } else {
- mParam.gyroVar = GEOMAG_GYRO_VAR;
- mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
- mParam.accStdev = GEOMAG_ACC_STDEV;
- mParam.magStdev = GEOMAG_MAG_STDEV;
- }
-}
-
-void Fusion::initFusion(const vec4_t& q, float dT)
-{
- // initial estimate: E{ x(t0) }
- x0 = q;
- x1 = 0;
-
- // process noise covariance matrix: G.Q.Gt, with
- //
- // G = | -1 0 | Q = | q00 q10 |
- // | 0 1 | | q01 q11 |
- //
- // q00 = sv^2.dt + 1/3.su^2.dt^3
- // q10 = q01 = 1/2.su^2.dt^2
- // q11 = su^2.dt
- //
-
- const float dT2 = dT*dT;
- const float dT3 = dT2*dT;
-
- // variance of integrated output at 1/dT Hz (random drift)
- const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
-
- // variance of drift rate ramp
- const float q11 = mParam.gyroBiasVar * dT;
- const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
- const float q01 = q10;
-
- GQGt[0][0] = q00; // rad^2
- GQGt[1][0] = -q10;
- GQGt[0][1] = -q01;
- GQGt[1][1] = q11; // (rad/s)^2
-
- // initial covariance: Var{ x(t0) }
- // TODO: initialize P correctly
- P = 0;
-}
-
-bool Fusion::hasEstimate() const {
- return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
- ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
- (mInitState & ACC);
-}
-
-bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
- if (hasEstimate())
- return true;
-
- if (what == ACC) {
- mData[0] += d * (1/length(d));
- mCount[0]++;
- mInitState |= ACC;
- if (mMode == FUSION_NOGYRO ) {
- mGyroRate = dT;
- }
- } else if (what == MAG) {
- mData[1] += d * (1/length(d));
- mCount[1]++;
- mInitState |= MAG;
- } else if (what == GYRO) {
- mGyroRate = dT;
- mData[2] += d*dT;
- mCount[2]++;
- mInitState |= GYRO;
- }
-
- if (hasEstimate()) {
- // Average all the values we collected so far
- mData[0] *= 1.0f/mCount[0];
- if (mMode != FUSION_NOMAG) {
- mData[1] *= 1.0f/mCount[1];
- }
- mData[2] *= 1.0f/mCount[2];
-
- // calculate the MRPs from the data collection, this gives us
- // a rough estimate of our initial state
- mat33_t R;
- vec3_t up(mData[0]);
- vec3_t east;
-
- if (mMode != FUSION_NOMAG) {
- east = normalize(cross_product(mData[1], up));
- } else {
- east = getOrthogonal(up);
- }
-
- vec3_t north(cross_product(up, east));
- R << east << north << up;
- const vec4_t q = matrixToQuat(R);
-
- initFusion(q, mGyroRate);
- }
-
- return false;
-}
-
-void Fusion::handleGyro(const vec3_t& w, float dT) {
- if (!checkInitComplete(GYRO, w, dT))
- return;
-
- predict(w, dT);
-}
-
-status_t Fusion::handleAcc(const vec3_t& a, float dT) {
- if (!checkInitComplete(ACC, a, dT))
- return BAD_VALUE;
-
- // ignore acceleration data if we're close to free-fall
- const float l = length(a);
- if (l < FREE_FALL_THRESHOLD) {
- return BAD_VALUE;
- }
-
- const float l_inv = 1.0f/l;
-
- if ( mMode == FUSION_NOGYRO ) {
- //geo mag
- vec3_t w_dummy;
- w_dummy = x1; //bias
- predict(w_dummy, dT);
- }
-
- if ( mMode == FUSION_NOMAG) {
- vec3_t m;
- m = getRotationMatrix()*Bm;
- update(m, Bm, mParam.magStdev);
- }
-
- vec3_t unityA = a * l_inv;
- const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
- const float p = l_inv * mParam.accStdev*expf(d);
-
- update(unityA, Ba, p);
- return NO_ERROR;
-}
-
-status_t Fusion::handleMag(const vec3_t& m) {
- if (!checkInitComplete(MAG, m))
- return BAD_VALUE;
-
- // the geomagnetic-field should be between 30uT and 60uT
- // reject if too large to avoid spurious magnetic sources
- const float magFieldSq = length_squared(m);
- if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
- return BAD_VALUE;
- } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
- // Also reject if too small since we will get ill-defined (zero mag)
- // cross-products below
- return BAD_VALUE;
- }
-
- // Orthogonalize the magnetic field to the gravity field, mapping it into
- // tangent to Earth.
- const vec3_t up( getRotationMatrix() * Ba );
- const vec3_t east( cross_product(m, up) );
-
- // If the m and up vectors align, the cross product magnitude will
- // approach 0.
- // Reject this case as well to avoid div by zero problems and
- // ill-conditioning below.
- if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
- return BAD_VALUE;
- }
-
- // If we have created an orthogonal magnetic field successfully,
- // then pass it in as the update.
- vec3_t north( cross_product(up, east) );
-
- const float l_inv = 1 / length(north);
- north *= l_inv;
-
- update(north, Bm, mParam.magStdev*l_inv);
- return NO_ERROR;
-}
-
-void Fusion::checkState() {
- // P needs to stay positive semidefinite or the fusion diverges. When we
- // detect divergence, we reset the fusion.
- // TODO(braun): Instead, find the reason for the divergence and fix it.
-
- if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
- !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
- P = 0;
- }
-}
-
-vec4_t Fusion::getAttitude() const {
- return x0;
-}
-
-vec3_t Fusion::getBias() const {
- return x1;
-}
-
-mat33_t Fusion::getRotationMatrix() const {
- return quatToMatrix(x0);
-}
-
-mat34_t Fusion::getF(const vec4_t& q) {
- mat34_t F;
-
- // This is used to compute the derivative of q
- // F = | [q.xyz]x |
- // | -q.xyz |
-
- F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
- F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
- F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
- F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
- return F;
-}
-
-void Fusion::predict(const vec3_t& w, float dT) {
- const vec4_t q = x0;
- const vec3_t b = x1;
- vec3_t we = w - b;
-
- if (length(we) < WVEC_EPS) {
- we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
- }
- // q(k+1) = O(we)*q(k)
- // --------------------
- //
- // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
- // | -psi' cos(0.5*||w||*dT) |
- //
- // psi = sin(0.5*||w||*dT)*w / ||w||
- //
- //
- // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
- // ----------------------------------------
- //
- // G = | -I33 0 |
- // | 0 I33 |
- //
- // Phi = | Phi00 Phi10 |
- // | 0 1 |
- //
- // Phi00 = I33
- // - [w]x * sin(||w||*dt)/||w||
- // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
- //
- // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2
- // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
- // - I33*dT
-
- const mat33_t I33(1);
- const mat33_t I33dT(dT);
- const mat33_t wx(crossMatrix(we, 0));
- const mat33_t wx2(wx*wx);
- const float lwedT = length(we)*dT;
- const float hlwedT = 0.5f*lwedT;
- const float ilwe = 1.f/length(we);
- const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
- const float k1 = sinf(lwedT);
- const float k2 = cosf(hlwedT);
- const vec3_t psi(sinf(hlwedT)*ilwe*we);
- const mat33_t O33(crossMatrix(-psi, k2));
- mat44_t O;
- O[0].xyz = O33[0]; O[0].w = -psi.x;
- O[1].xyz = O33[1]; O[1].w = -psi.y;
- O[2].xyz = O33[2]; O[2].w = -psi.z;
- O[3].xyz = psi; O[3].w = k2;
-
- Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
- Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
-
- x0 = O*q;
-
- if (x0.w < 0)
- x0 = -x0;
-
- P = Phi*P*transpose(Phi) + GQGt;
-
- checkState();
-}
-
-void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
- vec4_t q(x0);
- // measured vector in body space: h(p) = A(p)*Bi
- const mat33_t A(quatToMatrix(q));
- const vec3_t Bb(A*Bi);
-
- // Sensitivity matrix H = dh(p)/dp
- // H = [ L 0 ]
- const mat33_t L(crossMatrix(Bb, 0));
-
- // gain...
- // K = P*Ht / [H*P*Ht + R]
- vec<mat33_t, 2> K;
- const mat33_t R(sigma*sigma);
- const mat33_t S(scaleCovariance(L, P[0][0]) + R);
- const mat33_t Si(invert(S));
- const mat33_t LtSi(transpose(L)*Si);
- K[0] = P[0][0] * LtSi;
- K[1] = transpose(P[1][0])*LtSi;
-
- // update...
- // P = (I-K*H) * P
- // P -= K*H*P
- // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 |
- // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 |
- // Note: the Joseph form is numerically more stable and given by:
- // P = (I-KH) * P * (I-KH)' + K*R*R'
- const mat33_t K0L(K[0] * L);
- const mat33_t K1L(K[1] * L);
- P[0][0] -= K0L*P[0][0];
- P[1][1] -= K1L*P[1][0];
- P[1][0] -= K0L*P[1][0];
- P[0][1] = transpose(P[1][0]);
-
- const vec3_t e(z - Bb);
- const vec3_t dq(K[0]*e);
-
- q += getF(q)*(0.5f*dq);
- x0 = normalize_quat(q);
-
- if (mMode != FUSION_NOMAG) {
- const vec3_t db(K[1]*e);
- x1 += db;
- }
-
- checkState();
-}
-
-vec3_t Fusion::getOrthogonal(const vec3_t &v) {
- vec3_t w;
- if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
- w[0]=0.f;
- w[1] = v[2];
- w[2] = -v[1];
- } else if (fabsf(v[1]) <= fabsf(v[2])) {
- w[0] = v[2];
- w[1] = 0.f;
- w[2] = -v[0];
- }else {
- w[0] = v[1];
- w[1] = -v[0];
- w[2] = 0.f;
- }
- return normalize(w);
-}
-
-
-// -----------------------------------------------------------------------
-
-}; // namespace android
-
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_FUSION_H
-#define ANDROID_FUSION_H
-
-#include "quat.h"
-#include "mat.h"
-#include "vec.h"
-#include "errors.h"
-
-namespace android {
-
-typedef mat<float, 3, 4> mat34_t;
-
-enum FUSION_MODE{
- FUSION_9AXIS, // use accel gyro mag
- FUSION_NOMAG, // use accel gyro (game rotation, gravity)
- FUSION_NOGYRO, // use accel mag (geomag rotation)
- NUM_FUSION_MODE
-};
-
-class Fusion {
- /*
- * the state vector is made of two sub-vector containing respectively:
- * - modified Rodrigues parameters
- * - the estimated gyro bias
- */
-// quat_t x0;
- vec3_t x1;
-
- /*
- * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is
- * semi-definite positive.
- *
- * P = | P00 P10 | = | P00 P10 |
- * | P01 P11 | | P10t P11 |
- *
- * Since P01 = transpose(P10), the code below never calculates or
- * stores P01.
- */
- mat<mat33_t, 2, 2> P;
-
- /*
- * the process noise covariance matrix
- */
- mat<mat33_t, 2, 2> GQGt;
-
-public:
- quat_t x0;
- Fusion();
- void init(int mode = FUSION_9AXIS);
- void handleGyro(const vec3_t& w, float dT);
- status_t handleAcc(const vec3_t& a, float dT);
- status_t handleMag(const vec3_t& m);
- vec4_t getAttitude() const;
- vec3_t getBias() const;
- mat33_t getRotationMatrix() const;
- bool hasEstimate() const;
-
-private:
- struct Parameter {
- float gyroVar;
- float gyroBiasVar;
- float accStdev;
- float magStdev;
- } mParam;
-
- mat<mat33_t, 2, 2> Phi;
- vec3_t Ba, Bm;
- uint32_t mInitState;
- float mGyroRate;
- vec<vec3_t, 3> mData;
- size_t mCount[3];
- int mMode;
-
- enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
- bool checkInitComplete(int, const vec3_t& w, float d = 0);
- void initFusion(const vec4_t& q0, float dT);
- void checkState();
- void predict(const vec3_t& w, float dT);
- void update(const vec3_t& z, const vec3_t& Bi, float sigma);
- static mat34_t getF(const vec4_t& p);
- static vec3_t getOrthogonal(const vec3_t &v);
-};
-
-}; // namespace android
-
-#endif // ANDROID_FUSION_H
+++ /dev/null
-#include <stdio.h>
-#include <sensor.h>
-#include <glib.h>
-#include <math.h>
-
-#include <vector>
-#include <string>
-
-#include "tizen_orientation.h"
-#include "fusion.h"
-
-const float TIMEUNIT = 1000000.0f;
-
-std::vector<sensor_type_e> ev;
-std::vector<std::string> sv;
-
-android::Fusion Android_Fusion; // the class that calculates Android rotation vector to Android orientation
-android::Fusion Android_Fusion2; // the class that calculates TIZEN rotation vector to Android orientation
-
-static unsigned long long timestamp[3]; // timestamps of accelerometer, gyroscope and magnetometer
-
-static float TIZEN_rv[4]; // TIZEN rotation vector
-static float Android_rv[4]; // Android rotation vector
-
-static float TIZEN_ori[3]; // TIZEN rotation vector to TIZEN orientation
-static float Android_ori[3]; // Android rotation vector to Android orientation
-
-static float TA_ori[3]; // TIZEN rotation vector to Android orientation
-static float AT_ori[3]; // Android rotation vector to TIZEN orientation
-
-static void sensor_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
-{
- sensor_type_e type;
- sensor_get_type(sensor, &type);
-
- if (type == SENSOR_ROTATION_VECTOR) {
- TIZEN_rv[0] = events[0].values[0];
- TIZEN_rv[1] = events[0].values[1];
- TIZEN_rv[2] = events[0].values[2];
- TIZEN_rv[3] = events[0].values[3];
-
- Android_Fusion2.x0 = android::vec3_t(TIZEN_rv);
- android::mat33_t m(Android_Fusion2.getRotationMatrix());
-
- android::vec3_t g;
- g[0] = atan2f(-m[1][0], m[0][0]) * RAD2DEG;
- g[1] = atan2f(-m[2][1], m[2][2]) * RAD2DEG;
- g[2] = asinf ( m[2][0]) * RAD2DEG;
- if (g[0] < 0)
- g[0] += 360;
-
- TA_ori[0] = g.x;
- TA_ori[1] = g.y;
- TA_ori[2] = g.z;
- }
-
- /* 6-axis rotation vector
- else if (type == SENSOR_GYROSCOPE_ROTATION_VECTOR) {
- T6_rv_f = true;
- T6_rv[0] = events[0].values[0];
- T6_rv[1] = events[0].values[1];
- T6_rv[2] = events[0].values[2];
- T6_rv[3] = events[0].values[3];
- }
- */
-
- else if (type == SENSOR_ORIENTATION) {
- TIZEN_ori[0] = events[0].values[0];
- TIZEN_ori[1] = events[0].values[1];
- TIZEN_ori[2] = events[0].values[2];
- }
-
- else {
- if (type == SENSOR_ACCELEROMETER) {
- android::vec3_t v(events[0].values);
- float dT = (events[0].timestamp - timestamp[0]) / TIMEUNIT;
- timestamp[0] = events[0].timestamp;
- Android_Fusion.handleAcc(v, dT);
- }
-
- else if (type == SENSOR_GYROSCOPE) {
- events[0].values[0] /= RAD2DEG;
- events[0].values[1] /= RAD2DEG;
- events[0].values[2] /= RAD2DEG;
-
- android::vec3_t v(events[0].values);
- float dT = (events[0].timestamp - timestamp[1]) / TIMEUNIT;
- timestamp[1] = events[0].timestamp;
-
- Android_Fusion.handleGyro(v, dT);
- }
-
- else if (type == SENSOR_MAGNETIC) {
- android::vec3_t v(events[0].values);
- Android_Fusion.handleMag(v);
- }
-
- android::quat_t v(Android_Fusion.getAttitude());
-
- Android_rv[0] = v[0];
- Android_rv[1] = v[1];
- Android_rv[2] = v[2];
- Android_rv[3] = v[3];
-
- quat_to_orientation(Android_rv, AT_ori[0], AT_ori[1], AT_ori[2]);
-
- android::mat33_t m(Android_Fusion.getRotationMatrix());
- android::vec3_t g;
- g[0] = atan2f(-m[1][0], m[0][0]) * RAD2DEG;
- g[1] = atan2f(-m[2][1], m[2][2]) * RAD2DEG;
- g[2] = asinf ( m[2][0]) * RAD2DEG;
- if (g[0] < 0)
- g[0] += 360;
-
- Android_ori[0] = g.x;
- Android_ori[1] = g.y;
- Android_ori[2] = g.z;
-
- /* 6-axis rotation vector
- A6_rv_f = true;
- android::quat_t v6(F6.getAttitude());
-
- A6_rv[0] = v6[0];
- A6_rv[1] = v6[1];
- A6_rv[2] = v6[2];
- A6_rv[3] = v6[3];
- */
- }
-
- printf("\n\n");
- printf("(Tizen / Android) Rotation vector : \n");
- printf("T : %f %f %f %f\n", TIZEN_rv[0], TIZEN_rv[1], TIZEN_rv[2], TIZEN_rv[3]);
- printf("A : %f %f %f %f\n\n", Android_rv[0], Android_rv[1], Android_rv[2], Android_rv[3]);
-
- printf("(Tizen / Android) Rotation vector -> (Tizen / Android) Orientation : \n");
- printf("T -> T : %f %f %f\n", TIZEN_ori[0], TIZEN_ori[1], TIZEN_ori[2]);
- printf("A -> A : %f %f %f\n", Android_ori[0], Android_ori[1], Android_ori[2]);
- printf("T -> A : %f %f %f\n", TA_ori[0], TA_ori[1], TA_ori[2]);
- printf("A -> T : %f %f %f\n", AT_ori[0], AT_ori[1], AT_ori[2]);
-}
-
-int main()
-{
- ev.push_back(SENSOR_ACCELEROMETER);
- sv.push_back("SENSOR_ACCELEROMETER");
-
- ev.push_back(SENSOR_GYROSCOPE);
- sv.push_back("SENSOR_GYROSCOPE");
-
- ev.push_back(SENSOR_MAGNETIC);
- sv.push_back("SENSOR_MAGNETIC");
-
- ev.push_back(SENSOR_ORIENTATION);
- sv.push_back("SENSOR_ORIENTATION");
-
- ev.push_back(SENSOR_ROTATION_VECTOR);
- sv.push_back("SENSOR_ROTATION_VECTOR");
-
- /* 6-axis rotation vector
- ev.push_back(SENSOR_GYROSCOPE_ROTATION_VECTOR);
- sv.push_back("SENSOR_GYROSCOPE_ROTATION_VECTOR");
- */
-
- sensor_h *sh;
- sensor_listener_h lh[10];
-
- int count;
- int num = ev.size();
- bool sup;
-
- for (int i = 0; i < num; ++i) {
- sensor_is_supported(ev[i], &sup);
-
- if (sup) {
- sensor_get_sensor_list(ev[i], &sh, &count);
- sensor_create_listener(sh[0], &lh[i]);
-
- sensor_listener_set_events_cb(lh[i], sensor_callback, NULL);
- sensor_listener_set_option(lh[i], SENSOR_OPTION_ALWAYS_ON);
- sensor_listener_set_interval(lh[i], 100);
- sensor_listener_start(lh[i]);
- }
-
- else {
- printf("%s sensor is not supported in this device.\n", sv[i].c_str());
- }
- }
-
- GMainLoop *loop;
- loop = g_main_loop_new(NULL, FALSE);
- g_main_loop_run(loop);
-
- for (int i = 0; i < num; ++i) {
- sensor_is_supported(ev[i], &sup);
- if (sup)
- sensor_destroy_listener(lh[i]);
- }
-
- return 0;
-}
\ No newline at end of file
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_MAT_H
-#define ANDROID_MAT_H
-
-#include "vec.h"
-#include "traits.h"
-
-// -----------------------------------------------------------------------
-
-namespace android {
-
-template <typename TYPE, size_t C, size_t R>
-class mat;
-
-namespace helpers {
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R>& doAssign(
- mat<TYPE, C, R>& lhs,
- typename TypeTraits<TYPE>::ParameterType rhs) {
- for (size_t i=0 ; i<C ; i++)
- for (size_t j=0 ; j<R ; j++)
- lhs[i][j] = (i==j) ? rhs : 0;
- return lhs;
-}
-
-template <typename TYPE, size_t C, size_t R, size_t D>
-mat<TYPE, C, R> PURE doMul(
- const mat<TYPE, D, R>& lhs,
- const mat<TYPE, C, D>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- TYPE v(0);
- for (size_t k=0 ; k<D ; k++) {
- v += lhs[k][r] * rhs[c][k];
- }
- res[c][r] = v;
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t R, size_t D>
-vec<TYPE, R> PURE doMul(
- const mat<TYPE, D, R>& lhs,
- const vec<TYPE, D>& rhs)
-{
- vec<TYPE, R> res;
- for (size_t r=0 ; r<R ; r++) {
- TYPE v(0);
- for (size_t k=0 ; k<D ; k++) {
- v += lhs[k][r] * rhs[k];
- }
- res[r] = v;
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- const vec<TYPE, R>& lhs,
- const mat<TYPE, C, 1>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = lhs[r] * rhs[c][0];
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- const mat<TYPE, C, R>& rhs,
- typename TypeTraits<TYPE>::ParameterType v)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = rhs[c][r] * v;
- }
- }
- return res;
-}
-
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, C, R> PURE doMul(
- typename TypeTraits<TYPE>::ParameterType v,
- const mat<TYPE, C, R>& rhs)
-{
- mat<TYPE, C, R> res;
- for (size_t c=0 ; c<C ; c++) {
- for (size_t r=0 ; r<R ; r++) {
- res[c][r] = v * rhs[c][r];
- }
- }
- return res;
-}
-
-
-}; // namespace helpers
-
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t C, size_t R>
-class mat : public vec< vec<TYPE, R>, C > {
- typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
- typedef vec< vec<TYPE, R>, C > base;
-public:
- // STL-like interface.
- typedef TYPE value_type;
- typedef TYPE& reference;
- typedef TYPE const& const_reference;
- typedef size_t size_type;
- size_type size() const { return R*C; }
- enum { ROWS = R, COLS = C };
-
-
- // -----------------------------------------------------------------------
- // default constructors
-
- mat() { }
- mat(const mat& rhs) : base(rhs) { }
- mat(const base& rhs) : base(rhs) { } // NOLINT(google-explicit-constructor)
-
- // -----------------------------------------------------------------------
- // conversion constructors
-
- // sets the diagonal to the value, off-diagonal to zero
- mat(pTYPE rhs) { // NOLINT(google-explicit-constructor)
- helpers::doAssign(*this, rhs);
- }
-
- // -----------------------------------------------------------------------
- // Assignment
-
- mat& operator=(const mat& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- mat& operator=(const base& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- mat& operator=(pTYPE rhs) {
- return helpers::doAssign(*this, rhs);
- }
-
- // -----------------------------------------------------------------------
- // non-member function declaration and definition
-
- friend inline mat PURE operator + (const mat& lhs, const mat& rhs) {
- return helpers::doAdd(
- static_cast<const base&>(lhs),
- static_cast<const base&>(rhs));
- }
- friend inline mat PURE operator - (const mat& lhs, const mat& rhs) {
- return helpers::doSub(
- static_cast<const base&>(lhs),
- static_cast<const base&>(rhs));
- }
-
- // matrix*matrix
- template <size_t D>
- friend mat PURE operator * (
- const mat<TYPE, D, R>& lhs,
- const mat<TYPE, C, D>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // matrix*vector
- friend vec<TYPE, R> PURE operator * (
- const mat& lhs, const vec<TYPE, C>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // vector*matrix
- friend mat PURE operator * (
- const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) {
- return helpers::doMul(lhs, rhs);
- }
-
- // matrix*scalar
- friend inline mat PURE operator * (const mat& lhs, pTYPE v) {
- return helpers::doMul(lhs, v);
- }
-
- // scalar*matrix
- friend inline mat PURE operator * (pTYPE v, const mat& rhs) {
- return helpers::doMul(v, rhs);
- }
-
- // -----------------------------------------------------------------------
- // streaming operator to set the columns of the matrix:
- // example:
- // mat33_t m;
- // m << v0 << v1 << v2;
-
- // column_builder<> stores the matrix and knows which column to set
- template<size_t PREV_COLUMN>
- struct column_builder {
- mat& matrix;
- explicit column_builder(mat& matrix) : matrix(matrix) { }
- };
-
- // operator << is not a method of column_builder<> so we can
- // overload it for unauthorized values (partial specialization
- // not allowed in class-scope).
- // we just set the column and return the next column_builder<>
- template<size_t PREV_COLUMN>
- friend column_builder<PREV_COLUMN+1> operator << (
- const column_builder<PREV_COLUMN>& lhs,
- const vec<TYPE, R>& rhs) {
- lhs.matrix[PREV_COLUMN+1] = rhs;
- return column_builder<PREV_COLUMN+1>(lhs.matrix);
- }
-
- // we return void here so we get a compile-time error if the
- // user tries to set too many columns
- friend void operator << (
- const column_builder<C-2>& lhs,
- const vec<TYPE, R>& rhs) {
- lhs.matrix[C-1] = rhs;
- }
-
- // this is where the process starts. we set the first columns and
- // return the next column_builder<>
- column_builder<0> operator << (const vec<TYPE, R>& rhs) {
- (*this)[0] = rhs;
- return column_builder<0>(*this);
- }
-};
-
-// Specialize column matrix so they're exactly equivalent to a vector
-template <typename TYPE, size_t R>
-class mat<TYPE, 1, R> : public vec<TYPE, R> {
- typedef vec<TYPE, R> base;
-public:
- // STL-like interface.
- typedef TYPE value_type;
- typedef TYPE& reference;
- typedef TYPE const& const_reference;
- typedef size_t size_type;
- size_type size() const { return R; }
- enum { ROWS = R, COLS = 1 };
-
- mat() { }
- explicit mat(const base& rhs) : base(rhs) { }
- mat(const mat& rhs) : base(rhs) { }
- explicit mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); }
- mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; }
- mat& operator=(const base& rhs) { base::operator=(rhs); return *this; }
- mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); }
- // we only have one column, so ignore the index
- const base& operator[](size_t) const { return *this; }
- base& operator[](size_t) { return *this; }
- void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; }
-};
-
-// -----------------------------------------------------------------------
-// matrix functions
-
-// transpose. this handles matrices of matrices
-inline int PURE transpose(int v) { return v; }
-inline float PURE transpose(float v) { return v; }
-inline double PURE transpose(double v) { return v; }
-
-// Transpose a matrix
-template <typename TYPE, size_t C, size_t R>
-mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) {
- mat<TYPE, R, C> r;
- for (size_t i=0 ; i<R ; i++)
- for (size_t j=0 ; j<C ; j++)
- r[i][j] = transpose(m[j][i]);
- return r;
-}
-
-// Calculate the trace of a matrix
-template <typename TYPE, size_t C> static TYPE trace(const mat<TYPE, C, C>& m) {
- TYPE t;
- for (size_t i=0 ; i<C ; i++)
- t += m[i][i];
- return t;
-}
-
-// Test positive-semidefiniteness of a matrix
-template <typename TYPE, size_t C>
-static bool isPositiveSemidefinite(const mat<TYPE, C, C>& m, TYPE tolerance) {
- for (size_t i=0 ; i<C ; i++)
- if (m[i][i] < 0)
- return false;
-
- for (size_t i=0 ; i<C ; i++)
- for (size_t j=i+1 ; j<C ; j++)
- if (fabs(m[i][j] - m[j][i]) > tolerance)
- return false;
-
- return true;
-}
-
-// Transpose a vector
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) {
- mat<TYPE, SIZE, 1> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i][0] = transpose(v[i]);
- return r;
-}
-
-// -----------------------------------------------------------------------
-// "dumb" matrix inversion
-template<typename T, size_t N>
-mat<T, N, N> PURE invert(const mat<T, N, N>& src) {
- T t;
- size_t swap;
- mat<T, N, N> tmp(src);
- mat<T, N, N> inverse(1);
-
- for (size_t i=0 ; i<N ; i++) {
- // look for largest element in column
- swap = i;
- for (size_t j=i+1 ; j<N ; j++) {
- if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
- swap = j;
- }
- }
-
- if (swap != i) {
- /* swap rows. */
- for (size_t k=0 ; k<N ; k++) {
- t = tmp[i][k];
- tmp[i][k] = tmp[swap][k];
- tmp[swap][k] = t;
-
- t = inverse[i][k];
- inverse[i][k] = inverse[swap][k];
- inverse[swap][k] = t;
- }
- }
-
- t = 1 / tmp[i][i];
- for (size_t k=0 ; k<N ; k++) {
- tmp[i][k] *= t;
- inverse[i][k] *= t;
- }
- for (size_t j=0 ; j<N ; j++) {
- if (j != i) {
- t = tmp[j][i];
- for (size_t k=0 ; k<N ; k++) {
- tmp[j][k] -= tmp[i][k] * t;
- inverse[j][k] -= inverse[i][k] * t;
- }
- }
- }
- }
- return inverse;
-}
-
-// -----------------------------------------------------------------------
-
-typedef mat<float, 2, 2> mat22_t;
-typedef mat<float, 3, 3> mat33_t;
-typedef mat<float, 4, 4> mat44_t;
-
-// -----------------------------------------------------------------------
-
-}; // namespace android
-
-#endif /* ANDROID_MAT_H */
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_QUAT_H
-#define ANDROID_QUAT_H
-
-#include <math.h>
-
-#include "vec.h"
-#include "mat.h"
-
-// -----------------------------------------------------------------------
-namespace android {
-// -----------------------------------------------------------------------
-
-template <typename TYPE>
-mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
- mat<TYPE, 3, 3> R;
- TYPE q0(q.w);
- TYPE q1(q.x);
- TYPE q2(q.y);
- TYPE q3(q.z);
- TYPE sq_q1 = 2 * q1 * q1;
- TYPE sq_q2 = 2 * q2 * q2;
- TYPE sq_q3 = 2 * q3 * q3;
- TYPE q1_q2 = 2 * q1 * q2;
- TYPE q3_q0 = 2 * q3 * q0;
- TYPE q1_q3 = 2 * q1 * q3;
- TYPE q2_q0 = 2 * q2 * q0;
- TYPE q2_q3 = 2 * q2 * q3;
- TYPE q1_q0 = 2 * q1 * q0;
- R[0][0] = 1 - sq_q2 - sq_q3;
- R[0][1] = q1_q2 - q3_q0;
- R[0][2] = q1_q3 + q2_q0;
- R[1][0] = q1_q2 + q3_q0;
- R[1][1] = 1 - sq_q1 - sq_q3;
- R[1][2] = q2_q3 - q1_q0;
- R[2][0] = q1_q3 - q2_q0;
- R[2][1] = q2_q3 + q1_q0;
- R[2][2] = 1 - sq_q1 - sq_q2;
- return R;
-}
-
-template <typename TYPE>
-vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
- // matrix to quaternion
-
- struct {
- inline TYPE operator()(TYPE v) {
- return v < 0 ? 0 : v;
- }
- } clamp;
-
- vec<TYPE, 4> q;
- const float Hx = R[0].x;
- const float My = R[1].y;
- const float Az = R[2].z;
- q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
- q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
- q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
- q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
- q.x = copysignf(q.x, R[2].y - R[1].z);
- q.y = copysignf(q.y, R[0].z - R[2].x);
- q.z = copysignf(q.z, R[1].x - R[0].y);
- // guaranteed to be unit-quaternion
- return q;
-}
-
-template <typename TYPE>
-vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
- vec<TYPE, 4> r(q);
- if (r.w < 0) {
- r = -r;
- }
- return normalize(r);
-}
-
-// -----------------------------------------------------------------------
-
-typedef vec4_t quat_t;
-
-// -----------------------------------------------------------------------
-}; // namespace android
-
-#endif /* ANDROID_QUAT_H */
+++ /dev/null
-#include <math.h>
-
-#define QUAT (M_PI/4)
-#define HALF (M_PI/2)
-#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
-
-const float RAD2DEG = 57.29577951;
-
-int quat_to_matrix(const float *quat, float *R)
-{
- if (quat == NULL || R == NULL)
- return -EINVAL;
-
- float q0 = quat[3];
- float q1 = quat[0];
- float q2 = quat[1];
- float q3 = quat[2];
-
- float sq_q1 = 2 * q1 * q1;
- float sq_q2 = 2 * q2 * q2;
- float sq_q3 = 2 * q3 * q3;
- float q1_q2 = 2 * q1 * q2;
- float q3_q0 = 2 * q3 * q0;
- float q1_q3 = 2 * q1 * q3;
- float q2_q0 = 2 * q2 * q0;
- float q2_q3 = 2 * q2 * q3;
- float q1_q0 = 2 * q1 * q0;
-
- R[0] = 1 - sq_q2 - sq_q3;
- R[1] = q1_q2 - q3_q0;
- R[2] = q1_q3 + q2_q0;
- R[3] = q1_q2 + q3_q0;
- R[4] = 1 - sq_q1 - sq_q3;
- R[5] = q2_q3 - q1_q0;
- R[6] = q1_q3 - q2_q0;
- R[7] = q2_q3 + q1_q0;
- R[8] = 1 - sq_q1 - sq_q2;
-
- return 0;
-}
-
-int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
-{
- int error;
- float g[3];
- float R[9];
-
- error = quat_to_matrix(quat, R);
-
- if (error < 0)
- return error;
-
- float xyz_z = ARCTAN(R[3], R[0]);
- float yxz_x = asinf(R[7]);
- float yxz_y = ARCTAN(-R[6], R[8]);
- float yxz_z = ARCTAN(-R[1], R[4]);
-
- float a = fabs(yxz_x / HALF);
- a = a * a;
-
- float p = (fabs(yxz_y) / HALF - 1.0);
-
- if (p < 0)
- p = 0;
-
- float v = 1 + (1 - a) / a * p;
-
- if (v > 20)
- v = 20;
-
- if (yxz_x * yxz_y > 0) {
- if (yxz_z > 0 && xyz_z < 0)
- xyz_z += M_PI * 2;
- } else {
- if (yxz_z < 0 && xyz_z > 0)
- xyz_z -= M_PI * 2;
- }
-
- g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
- g[0] *= -1;
-
- float tmp = R[7];
-
- if (tmp > 1.0f)
- tmp = 1.0f;
- else if (tmp < -1.0f)
- tmp = -1.0f;
-
- g[1] = -asinf(tmp);
- if (R[8] < 0)
- g[1] = M_PI - g[1];
-
- if (g[1] > M_PI)
- g[1] -= M_PI * 2;
-
- if ((fabs(R[7]) > QUAT))
- g[2] = (float) atan2f(R[6], R[7]);
- else
- g[2] = (float) atan2f(R[6], R[8]);
-
- if (g[2] > HALF)
- g[2] = M_PI - g[2];
- else if (g[2] < -HALF)
- g[2] = -M_PI - g[2];
-
- g[0] *= RAD2DEG;
- g[1] *= RAD2DEG;
- g[2] *= RAD2DEG;
-
- if (g[0] < 0)
- g[0] += 360;
-
- azimuth = g[0];
- pitch = g[1];
- roll = g[2];
-
- return 0;
-}
\ No newline at end of file
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_TRAITS_H
-#define ANDROID_TRAITS_H
-
-// -----------------------------------------------------------------------
-// Typelists
-
-namespace android {
-
-// end-of-list marker
-class NullType {};
-
-// type-list node
-template <typename T, typename U>
-struct TypeList {
- typedef T Head;
- typedef U Tail;
-};
-
-// helpers to build typelists
-#define TYPELIST_1(T1) TypeList<T1, NullType>
-#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)>
-#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)>
-#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)>
-
-// typelists algorithms
-namespace TL {
-template <typename TList, typename T> struct IndexOf;
-
-template <typename T>
-struct IndexOf<NullType, T> {
- enum { value = -1 };
-};
-
-template <typename T, typename Tail>
-struct IndexOf<TypeList<T, Tail>, T> {
- enum { value = 0 };
-};
-
-template <typename Head, typename Tail, typename T>
-struct IndexOf<TypeList<Head, Tail>, T> {
-private:
- enum { temp = IndexOf<Tail, T>::value };
-public:
- enum { value = temp == -1 ? -1 : 1 + temp };
-};
-
-}; // namespace TL
-
-// type selection based on a boolean
-template <bool flag, typename T, typename U>
-struct Select {
- typedef T Result;
-};
-template <typename T, typename U>
-struct Select<false, T, U> {
- typedef U Result;
-};
-
-// -----------------------------------------------------------------------
-// Type traits
-
-template <typename T>
-class TypeTraits {
- typedef TYPELIST_4(
- unsigned char, unsigned short,
- unsigned int, unsigned long int) UnsignedInts;
-
- typedef TYPELIST_4(
- signed char, signed short,
- signed int, signed long int) SignedInts;
-
- typedef TYPELIST_1(
- bool) OtherInts;
-
- typedef TYPELIST_3(
- float, double, long double) Floats;
-
- template<typename U> struct PointerTraits {
- enum { result = false };
- typedef NullType PointeeType;
- };
- template<typename U> struct PointerTraits<U*> {
- enum { result = true };
- typedef U PointeeType;
- };
-
-public:
- enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 };
- enum { isStdSignedInt = TL::IndexOf<SignedInts, T>::value >= 0 };
- enum { isStdIntegral = TL::IndexOf<OtherInts, T>::value >= 0 || isStdUnsignedInt || isStdSignedInt };
- enum { isStdFloat = TL::IndexOf<Floats, T>::value >= 0 };
- enum { isPointer = PointerTraits<T>::result };
- enum { isStdArith = isStdIntegral || isStdFloat };
-
- // best parameter type for given type
- typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType;
-};
-
-// -----------------------------------------------------------------------
-}; // namespace android
-
-#endif /* ANDROID_TRAITS_H */
+++ /dev/null
-/*
- * Copyright (C) 2011 The Android Open Source Project
- *
- * 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.
- */
-
-// released in android-11.0.0_r9
-
-#ifndef ANDROID_VEC_H
-#define ANDROID_VEC_H
-
-#include <math.h>
-
-#include <stdint.h>
-#include <stddef.h>
-
-#include "traits.h"
-
-// -----------------------------------------------------------------------
-
-#define PURE __attribute__((pure))
-
-namespace android {
-
-// -----------------------------------------------------------------------
-// non-inline helpers
-
-template <typename TYPE, size_t SIZE>
-class vec;
-
-template <typename TYPE, size_t SIZE>
-struct vbase;
-
-namespace helpers {
-
-template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
-template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
-
-template < template<typename T, size_t S> class VEC,
- typename TYPE, size_t SIZE, size_t S>
-vec<TYPE, SIZE>& doAssign(
- vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) {
- const size_t minSize = min(SIZE, S);
- const size_t maxSize = max(SIZE, S);
- for (size_t i=0 ; i<minSize ; i++)
- lhs[i] = rhs[i];
- for (size_t i=minSize ; i<maxSize ; i++)
- lhs[i] = 0;
- return lhs;
-}
-
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE,
- size_t SIZE
->
-VLHS<TYPE, SIZE> PURE doAdd(
- const VLHS<TYPE, SIZE>& lhs,
- const VRHS<TYPE, SIZE>& rhs) {
- VLHS<TYPE, SIZE> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i] = lhs[i] + rhs[i];
- return r;
-}
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE,
- size_t SIZE
->
-VLHS<TYPE, SIZE> PURE doSub(
- const VLHS<TYPE, SIZE>& lhs,
- const VRHS<TYPE, SIZE>& rhs) {
- VLHS<TYPE, SIZE> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i] = lhs[i] - rhs[i];
- return r;
-}
-
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-VEC<TYPE, SIZE> PURE doMulScalar(
- const VEC<TYPE, SIZE>& lhs,
- typename TypeTraits<TYPE>::ParameterType rhs) {
- VEC<TYPE, SIZE> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i] = lhs[i] * rhs;
- return r;
-}
-
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-VEC<TYPE, SIZE> PURE doScalarMul(
- typename TypeTraits<TYPE>::ParameterType lhs,
- const VEC<TYPE, SIZE>& rhs) {
- VEC<TYPE, SIZE> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i] = lhs * rhs[i];
- return r;
-}
-
-}; // namespace helpers
-
-// -----------------------------------------------------------------------
-// Below we define the mathematical operators for vectors.
-// We use template template arguments so we can generically
-// handle the case where the right-hand-size and left-hand-side are
-// different vector types (but with same value_type and size).
-// This is needed for performance when using ".xy{z}" element access
-// on vec<>. Without this, an extra conversion to vec<> would be needed.
-//
-// example:
-// vec4_t a;
-// vec3_t b;
-// vec3_t c = a.xyz + b;
-//
-// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring
-// a conversion of vbase<> to vec<>. The template gunk below avoids this,
-// by allowing the addition on these different vector types directly
-//
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE,
- size_t SIZE
->
-inline VLHS<TYPE, SIZE> PURE operator + (
- const VLHS<TYPE, SIZE>& lhs,
- const VRHS<TYPE, SIZE>& rhs) {
- return helpers::doAdd(lhs, rhs);
-}
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE,
- size_t SIZE
->
-inline VLHS<TYPE, SIZE> PURE operator - (
- const VLHS<TYPE, SIZE>& lhs,
- const VRHS<TYPE, SIZE>& rhs) {
- return helpers::doSub(lhs, rhs);
-}
-
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-inline VEC<TYPE, SIZE> PURE operator * (
- const VEC<TYPE, SIZE>& lhs,
- typename TypeTraits<TYPE>::ParameterType rhs) {
- return helpers::doMulScalar(lhs, rhs);
-}
-
-template <
- template<typename T, size_t S> class VEC,
- typename TYPE,
- size_t SIZE
->
-inline VEC<TYPE, SIZE> PURE operator * (
- typename TypeTraits<TYPE>::ParameterType lhs,
- const VEC<TYPE, SIZE>& rhs) {
- return helpers::doScalarMul(lhs, rhs);
-}
-
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE,
- size_t SIZE
->
-TYPE PURE dot_product(
- const VLHS<TYPE, SIZE>& lhs,
- const VRHS<TYPE, SIZE>& rhs) {
- TYPE r(0);
- for (size_t i=0 ; i<SIZE ; i++)
- r += lhs[i] * rhs[i];
- return r;
-}
-
-template <
- template<typename T, size_t S> class V,
- typename TYPE,
- size_t SIZE
->
-TYPE PURE length(const V<TYPE, SIZE>& v) {
- return sqrt(dot_product(v, v));
-}
-
-template <
- template<typename T, size_t S> class V,
- typename TYPE,
- size_t SIZE
->
-TYPE PURE length_squared(const V<TYPE, SIZE>& v) {
- return dot_product(v, v);
-}
-
-template <
- template<typename T, size_t S> class V,
- typename TYPE,
- size_t SIZE
->
-V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
- return v * (1/length(v));
-}
-
-template <
- template<typename T, size_t S> class VLHS,
- template<typename T, size_t S> class VRHS,
- typename TYPE
->
-VLHS<TYPE, 3> PURE cross_product(
- const VLHS<TYPE, 3>& u,
- const VRHS<TYPE, 3>& v) {
- VLHS<TYPE, 3> r;
- r.x = u.y*v.z - u.z*v.y;
- r.y = u.z*v.x - u.x*v.z;
- r.z = u.x*v.y - u.y*v.x;
- return r;
-}
-
-
-template <typename TYPE, size_t SIZE>
-vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) {
- vec<TYPE, SIZE> r;
- for (size_t i=0 ; i<SIZE ; i++)
- r[i] = -lhs[i];
- return r;
-}
-
-// -----------------------------------------------------------------------
-
-// This our basic vector type, it just implements the data storage
-// and accessors.
-
-template <typename TYPE, size_t SIZE>
-struct vbase {
- TYPE v[SIZE];
- inline const TYPE& operator[](size_t i) const { return v[i]; }
- inline TYPE& operator[](size_t i) { return v[i]; }
-};
-template<> struct vbase<float, 2> {
- union {
- float v[2];
- struct { float x, y; };
- struct { float s, t; };
- };
- inline const float& operator[](size_t i) const { return v[i]; }
- inline float& operator[](size_t i) { return v[i]; }
-};
-template<> struct vbase<float, 3> {
- union {
- float v[3];
- struct { float x, y, z; };
- struct { float s, t, r; };
- vbase<float, 2> xy;
- vbase<float, 2> st;
- };
- inline const float& operator[](size_t i) const { return v[i]; }
- inline float& operator[](size_t i) { return v[i]; }
-};
-template<> struct vbase<float, 4> {
- union {
- float v[4];
- struct { float x, y, z, w; };
- struct { float s, t, r, q; };
- vbase<float, 3> xyz;
- vbase<float, 3> str;
- vbase<float, 2> xy;
- vbase<float, 2> st;
- };
- inline const float& operator[](size_t i) const { return v[i]; }
- inline float& operator[](size_t i) { return v[i]; }
-};
-
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t SIZE>
-class vec : public vbase<TYPE, SIZE>
-{
- typedef typename TypeTraits<TYPE>::ParameterType pTYPE;
- typedef vbase<TYPE, SIZE> base;
-
-public:
- // STL-like interface.
- typedef TYPE value_type;
- typedef TYPE& reference;
- typedef TYPE const& const_reference;
- typedef size_t size_type;
-
- typedef TYPE* iterator;
- typedef TYPE const* const_iterator;
- iterator begin() { return base::v; }
- iterator end() { return base::v + SIZE; }
- const_iterator begin() const { return base::v; }
- const_iterator end() const { return base::v + SIZE; }
- size_type size() const { return SIZE; }
-
- // -----------------------------------------------------------------------
- // default constructors
-
- vec() { }
- vec(const vec& rhs) : base(rhs) { }
- vec(const base& rhs) : base(rhs) { } // NOLINT(google-explicit-constructor)
-
- // -----------------------------------------------------------------------
- // conversion constructors
-
- vec(pTYPE rhs) { // NOLINT(google-explicit-constructor)
- for (size_t i=0 ; i<SIZE ; i++)
- base::operator[](i) = rhs;
- }
-
- template < template<typename T, size_t S> class VEC, size_t S>
- explicit vec(const VEC<TYPE, S>& rhs) {
- helpers::doAssign(*this, rhs);
- }
-
- explicit vec(TYPE const* array) {
- for (size_t i=0 ; i<SIZE ; i++)
- base::operator[](i) = array[i];
- }
-
- // -----------------------------------------------------------------------
- // Assignment
-
- vec& operator = (const vec& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- vec& operator = (const base& rhs) {
- base::operator=(rhs);
- return *this;
- }
-
- vec& operator = (pTYPE rhs) {
- for (size_t i=0 ; i<SIZE ; i++)
- base::operator[](i) = rhs;
- return *this;
- }
-
- template < template<typename T, size_t S> class VEC, size_t S>
- vec& operator = (const VEC<TYPE, S>& rhs) {
- return helpers::doAssign(*this, rhs);
- }
-
- // -----------------------------------------------------------------------
- // operation-assignment
-
- vec& operator += (const vec& rhs);
- vec& operator -= (const vec& rhs);
- vec& operator *= (pTYPE rhs);
-
- // -----------------------------------------------------------------------
- // non-member function declaration and definition
- // NOTE: we declare the non-member function as friend inside the class
- // so that they are known to the compiler when the class is instantiated.
- // This helps the compiler doing template argument deduction when the
- // passed types are not identical. Essentially this helps with
- // type conversion so that you can multiply a vec<float> by an scalar int
- // (for instance).
-
- friend inline vec PURE operator + (const vec& lhs, const vec& rhs) {
- return helpers::doAdd(lhs, rhs);
- }
- friend inline vec PURE operator - (const vec& lhs, const vec& rhs) {
- return helpers::doSub(lhs, rhs);
- }
- friend inline vec PURE operator * (const vec& lhs, pTYPE v) {
- return helpers::doMulScalar(lhs, v);
- }
- friend inline vec PURE operator * (pTYPE v, const vec& rhs) {
- return helpers::doScalarMul(v, rhs);
- }
- friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) {
- return android::dot_product(lhs, rhs);
- }
-};
-
-// -----------------------------------------------------------------------
-
-template <typename TYPE, size_t SIZE>
-vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) {
- vec<TYPE, SIZE>& lhs(*this);
- for (size_t i=0 ; i<SIZE ; i++)
- lhs[i] += rhs[i];
- return lhs;
-}
-
-template <typename TYPE, size_t SIZE>
-vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) {
- vec<TYPE, SIZE>& lhs(*this);
- for (size_t i=0 ; i<SIZE ; i++)
- lhs[i] -= rhs[i];
- return lhs;
-}
-
-template <typename TYPE, size_t SIZE>
-vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) {
- vec<TYPE, SIZE>& lhs(*this);
- for (size_t i=0 ; i<SIZE ; i++)
- lhs[i] *= rhs;
- return lhs;
-}
-
-// -----------------------------------------------------------------------
-
-typedef vec<float, 2> vec2_t;
-typedef vec<float, 3> vec3_t;
-typedef vec<float, 4> vec4_t;
-
-// -----------------------------------------------------------------------
-
-}; // namespace android
-
-#endif /* ANDROID_VEC_H */
--- /dev/null
+#define SENSOR_NUM 28
+
+struct sensor_info {
+ sensor_type_e type;
+ const char* name;
+ int value_num;
+};
+
+static const struct sensor_info sensor_infos[] = {
+ {SENSOR_ALL, "All", 0},
+ {SENSOR_ACCELEROMETER, "Accelerometer", 3},
+ {SENSOR_GRAVITY, "Gravity sensor", 3},
+ {SENSOR_LINEAR_ACCELERATION, "Linear acceleration sensor", 3},
+ {SENSOR_MAGNETIC, "Magnetic sensor", 3},
+ {SENSOR_ROTATION_VECTOR, "Rotation vector sensor", 4},
+ {SENSOR_ORIENTATION, "Orientation sensor", 3},
+ {SENSOR_GYROSCOPE, "Gyroscope", 3},
+ {SENSOR_LIGHT, "Light sensor", 1},
+ {SENSOR_PROXIMITY, "Proximity sensor", 1},
+ {SENSOR_PRESSURE, "Pressure sensor", 1},
+ {SENSOR_ULTRAVIOLET, "Ultraviolet sensor", 1},
+ {SENSOR_TEMPERATURE, "Temperature sensor", 1},
+ {SENSOR_HUMIDITY, "Humidity sensor", 1},
+ {SENSOR_HRM, "Heart-rate monitor", 1},
+ {SENSOR_HRM_LED_GREEN, "Green LED sensor of HRM", 1},
+ {SENSOR_HRM_LED_IR, "Infra-Red LED sensor of HRM", 1},
+ {SENSOR_HRM_LED_RED, "Red LED sensor of HRM", 1},
+ {SENSOR_GYROSCOPE_UNCALIBRATED, "Uncalibrated Gyroscope sensor", 6},
+ {SENSOR_GEOMAGNETIC_UNCALIBRATED, "Uncalibrated Geomagnetic sensor", 6},
+ {SENSOR_GYROSCOPE_ROTATION_VECTOR, "Gyroscope-based rotation vector sensor", 4},
+ {SENSOR_GEOMAGNETIC_ROTATION_VECTOR, "Geomagnetic-based rotation vector sensor", 4},
+ {SENSOR_SIGNIFICANT_MOTION, "Significant motion sensor", 1},
+ {SENSOR_HRM_BATCH, "Heart-rate monitor batch sensor", 1},
+ {SENSOR_HRM_LED_GREEN_BATCH, "Green LED of HRM batch sensor", 1},
+ {SENSOR_HUMAN_PEDOMETER, "Pedometer", 3},
+ {SENSOR_HUMAN_SLEEP_MONITOR, "Sleep monitor", 1},
+ {SENSOR_HUMAN_SLEEP_DETECTOR, "Sleep detector", 1},
+};
--- /dev/null
+/*
+ * sensor-test
+ *
+ * Copyright (c) 2022 Samsung Electronics Co., Ltd.
+ *
+ * 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 <stdio.h>
+#include <glib.h>
+#include <sensor.h>
+
+#include "spec_test.h"
+#include "sensor_info.h"
+
+#define TEST_COUNT_ONE 100
+#define TEST_COUNT_ALL 1000
+#define PASS 90
+
+static GMainLoop *loop;
+static bool turned_on[SENSOR_NUM];
+static int min_range[SENSOR_NUM];
+static int max_range[SENSOR_NUM];
+static int called_num[SENSOR_NUM];
+static int miss[SENSOR_NUM];
+static int callback_count;
+static bool running;
+
+
+static void sensor_one_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
+{
+ sensor_type_e type;
+ sensor_get_type(sensor, &type);
+ int num = 0;
+ bool missed;
+
+ ++callback_count;
+
+ for (int i = 1; i < SENSOR_NUM; ++i){
+ if (sensor_infos[i].type == type){
+ num = i;
+ break;
+ }
+ }
+
+ for (int i = 0; i < events_count; ++i) {
+ missed = false;
+ ++called_num[num];
+
+ int value_count;
+ if (events[i].value_count < sensor_infos[num].value_num)
+ value_count = events[i].value_count;
+ else
+ value_count = sensor_infos[num].value_num;
+
+ for (int j = 0; j < value_count; ++j) {
+ if (events[i].values[j] < min_range[num] || events[i].values[j] > max_range[num])
+ missed = true;
+ }
+
+ if (missed) {
+ ++miss[num];
+ g_print("miss : ");
+ for (int j = 0; j < value_count; ++j)
+ g_print("%lf ", events[i].values[j]);
+ g_print("\n");
+ }
+ }
+
+
+ if (running && callback_count >= TEST_COUNT_ONE) {
+ g_print("done.\n");
+ running = false;
+ g_main_loop_quit(loop);
+ }
+
+ else if (callback_count == TEST_COUNT_ONE / 10 * 2) {
+ g_print("20%%...\n");
+ }
+
+ else if (callback_count == TEST_COUNT_ONE / 10 * 4) {
+ g_print("40%%...\n");
+ }
+
+ else if (callback_count == TEST_COUNT_ONE / 10 * 6) {
+ g_print("60%%...\n");
+ }
+
+ else if (callback_count == TEST_COUNT_ONE / 10 * 8) {
+ g_print("80%%...\n");
+ }
+}
+
+static void sensor_all_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
+{
+ sensor_type_e type;
+ sensor_get_type(sensor, &type);
+ int num = 0;
+ bool missed;
+
+ ++callback_count;
+
+ for (int i = 1; i < SENSOR_NUM; ++i){
+ if (sensor_infos[i].type == type){
+ num = i;
+ break;
+ }
+ }
+
+ for (int i = 0; i < events_count; ++i) {
+ missed = false;
+ ++called_num[num];
+
+ int value_count;
+ if (events[i].value_count < sensor_infos[num].value_num)
+ value_count = events[i].value_count;
+ else
+ value_count = sensor_infos[num].value_num;
+
+ for (int j = 0; j < value_count; ++j) {
+ if (events[i].values[j] < min_range[num] || events[i].values[j] > max_range[num]) {
+ missed = true;
+ }
+ }
+
+ if (missed)
+ ++miss[num];
+ }
+
+ if (running && callback_count >= TEST_COUNT_ALL) {
+ g_print("done.\n");
+ running = false;
+ g_main_loop_quit(loop);
+ }
+
+ else if (callback_count == TEST_COUNT_ALL / 10 * 2) {
+ g_print("20%%...");
+ }
+
+ else if (callback_count == TEST_COUNT_ALL / 10 * 4) {
+ g_print("40%%...");
+ }
+
+ else if (callback_count == TEST_COUNT_ALL / 10 * 6) {
+ g_print("60%%...");
+ }
+
+ else if (callback_count == TEST_COUNT_ALL / 10 * 8) {
+ g_print("80%%...");
+ }
+}
+
+static int create_sensor_listener(int num, sensor_h *sensor, sensor_listener_h *listener, bool all)
+{
+ bool supported;
+ int ret = 0;
+ float min, max;
+ const char *name = sensor_infos[num].name;
+ sensor_type_e type = sensor_infos[num].type;
+
+ ret = sensor_is_supported(type, &supported);
+ if (ret < 0) {
+ printf("fail \"sensor_is_supported\"\n");
+ return ret;
+ }
+
+ else if (!supported) {
+ return -1;
+ }
+
+ ret = sensor_get_default_sensor(type, sensor);
+ if (ret < 0) {
+ printf("fail \"sensor_get_default_sensor\"\n");
+ return ret;
+ }
+
+ ret = sensor_create_listener(*sensor, listener);
+ if (ret < 0) {
+ printf("fail \"sensor_create_listener\"\n");
+ return ret;
+ }
+
+ if (all)
+ ret = sensor_listener_set_events_cb(*listener, sensor_all_callback, NULL);
+ else
+ ret = sensor_listener_set_events_cb(*listener, sensor_one_callback, NULL);
+ if (ret < 0) {
+ printf("fail \"sensor_listener_set_events_cb\"\n");
+ return ret;
+ }
+
+ ret = sensor_listener_set_option(*listener, SENSOR_OPTION_ALWAYS_ON);
+ if (ret < 0) {
+ printf("fail \"sensor_listener_set_option\"\n");
+ return ret;
+ }
+
+ ret = sensor_listener_set_interval(*listener, 10);
+ if (ret < 0) {
+ printf("fail \"sensor_listener_set_interval\"\n");
+ return ret;
+ }
+
+ ret = sensor_get_min_range(*sensor, &min);
+ if (ret < 0) {
+ printf("fail \"sensor_get_min_range\"\n");
+ return ret;
+ }
+
+ ret = sensor_get_max_range(*sensor, &max);
+ if (ret < 0) {
+ printf("fail \"sensor_get_max_range\"\n");
+ return ret;
+ }
+
+ min_range[num] = min;
+ max_range[num] = max;
+
+ printf("%s : %f ~ %f\n", name, min, max);
+
+ ret = sensor_listener_start(*listener);
+ if (ret < 0) {
+ printf("%s error in listnener_start : %d\n", name, ret);
+ return ret;
+ }
+
+ return 0;
+}
+
+static void spec_test_one(int type)
+{
+ sensor_h sensor;
+ sensor_listener_h listener;
+ int ret = create_sensor_listener(type, &sensor, &listener, false);
+ if (ret < 0)
+ return;
+
+ running = true;
+ loop = g_main_loop_new(NULL, FALSE);
+ g_main_loop_run(loop);
+
+ printf("=====================================\n");
+ printf("test result\n");
+ printf("=====================================\n");
+
+ double rate = (double) (called_num[type] - miss[type]) / called_num[type] * 100;
+ printf("%s : %d / %d \n", sensor_infos[type].name, called_num[type] - miss[type], called_num[type]);
+
+ if (rate >= PASS)
+ printf("PASS %.2lf%%\n", rate);
+ else
+ printf("FAIL %.2lf%%\n", rate);
+}
+
+static void spec_test_all(void)
+{
+ sensor_h sensor[SENSOR_NUM];
+ sensor_listener_h listener[SENSOR_NUM];
+ int usable = 0;
+
+ for (int i = 1; i < SENSOR_NUM; ++i) {
+ called_num[i] = 0;
+ miss[i] = 0;
+ if (create_sensor_listener(i, &sensor[i], &listener[i], true)) {
+ turned_on[i] = false;
+ }
+ else {
+ turned_on[i] = true;
+ ++usable;
+ }
+ }
+
+ if (usable == 0) {
+ printf("There is no supported sensor.\n");
+ return;
+ }
+
+ printf("testing... ");
+
+ running = true;
+ loop = g_main_loop_new(NULL, FALSE);
+ g_main_loop_run(loop);
+
+ printf("=====================================\n");
+ printf("test result\n");
+ printf("=====================================\n");
+
+ int total_miss = 0;
+ double rate = 0;
+ for (int i = 1; i < SENSOR_NUM; ++i) {
+ if (turned_on[i]) {
+ if (called_num[i]) {
+ rate = (double) (called_num[i] - miss[i]) / called_num[i] * 100;
+ printf("%s : %d / %d \n", sensor_infos[i].name, called_num[i] - miss[i], called_num[i]);
+
+ if (rate >= PASS)
+ printf("PASS %.2lf%%\n", rate);
+ else
+ printf("FAIL %.2lf%%\n", rate);
+ }
+ else {
+ printf("%s value change was not occurred.\n", sensor_infos[i].name);
+ }
+ }
+ total_miss += miss[i];
+ }
+
+ printf("total : %d / %d \n", callback_count - total_miss, callback_count);
+ rate = (double) (callback_count - total_miss) / callback_count * 100;
+ if (rate >= PASS)
+ printf("PASS %.2lf%%\n", rate);
+ else
+ printf("FAIL %.2lf%%\n", rate);
+
+ printf("=====================================\n");
+ printf("Not supported sensors list\n");
+ printf("=====================================\n");
+
+ for (int i = 1; i < SENSOR_NUM; ++i) {
+ if (turned_on[i] == false)
+ printf("%s\n", sensor_infos[i].name);
+ }
+}
+
+void spec_test(int type)
+{
+ printf("=====================================\n");
+ printf("Sensor spec test (%s)\n", sensor_infos[type].name);
+ printf("=====================================\n");
+
+ if (type == 0)
+ spec_test_all();
+ else
+ spec_test_one(type);
+}
--- /dev/null
+/*
+ * sensor-test
+ *
+ * Copyright (c) 2022 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+void spec_test(int type);
--- /dev/null
+/*
+ * sensor-test
+ *
+ * Copyright (c) 2022 Samsung Electronics Co., Ltd.
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <sensor.h>
+
+#include "stress_test.h"
+#include "sensor_info.h"
+
+void sensor_events_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
+{
+}
+
+void sensor_listener_set_accuracy_callback(sensor_h sensor, unsigned long long timestamp, sensor_data_accuracy_e accuracy, void *data)
+{
+}
+
+void stress_test(int type)
+{
+ printf("=====================================\n");
+ printf("Sensor stress test (%s)\n", sensor_infos[type].name);
+ printf("=====================================\n");
+ printf("Testing... Check sensord memory. (e.g. memps -s <pid>)\n");
+ printf("If you need, additional execution of this stress test binary in another shell can add more stress to the sensord.\n.");
+
+ sensor_h sh;
+ sensor_listener_h lh;
+ sensor_event_s* events;
+ int ret, count;
+ while (1) {
+ ret = sensor_get_default_sensor(sensor_infos[type].type, &sh);
+ if (ret < 0) {
+ printf("fail \"get_default_sensor\": %d \n", ret);
+ break;
+ }
+
+ ret = sensor_create_listener(sh, &lh);
+ if (ret < 0) {
+ printf("fail \"create_listener\": %d\n", ret);
+ break;
+ }
+
+ ret = sensor_listener_start(lh);
+ if (ret < 0)
+ printf("fail \"listener_start\": %d\n", ret);
+
+ ret = sensor_listener_set_events_cb(lh, sensor_events_callback, NULL);
+ if (ret < 0)
+ printf("fail \"listener_set_events_cb\": %d\n", ret);
+
+ ret = sensor_listener_unset_event_cb(lh);
+ if (ret < 0)
+ printf("fail \"listener_unset_event_cb\": %d\n", ret);
+
+ ret = sensor_listener_set_accuracy_cb(lh, sensor_listener_set_accuracy_callback, NULL);
+ if (ret < 0)
+ printf("fail \"listener_set_accuracy_cb\": %d\n", ret);
+
+ ret = sensor_listener_unset_accuracy_cb(lh);
+ if (ret < 0)
+ printf("fail \"listener_unset_accuracy_cb\": %d\n", ret);
+
+ ret = sensor_listener_read_data_list(lh, &events, &count);
+ if (ret == SENSOR_ERROR_OPERATION_FAILED)
+ printf("fail \"listener_read_data\". There is no data to read.\n");
+ else if (ret < 0)
+ printf("fail \"listener_read_data\": %d\n", ret);
+ else
+ free(events);
+
+ ret = sensor_listener_set_interval(lh, 10);
+ if (ret < 0)
+ printf("fail \"listener_set_interval\": %d\n", ret);
+
+ ret = sensor_listener_set_max_batch_latency(lh, 10);
+ if (ret < 0 && ret != SENSOR_ERROR_NOT_SUPPORTED)
+ printf("fail \"listener_set_max_batch_latency\": %d\n", ret);
+
+ ret = sensor_listener_set_attribute_int(lh, SENSOR_ATTRIBUTE_AXIS_ORIENTATION, 1);
+ if (ret < 0)
+ printf("fail \"listener_set_attribute_int\": %d\n", ret);
+
+ ret = sensor_listener_set_option(lh, SENSOR_OPTION_ALWAYS_ON);
+ if (ret < 0)
+ printf("fail \"listener_set_option\": %d\n", ret);
+
+ ret = sensor_listener_stop(lh);
+ if (ret < 0)
+ printf("fail \"listener_stop\": %d\n", ret);
+
+ ret = sensor_destroy_listener(lh);
+ if (ret < 0)
+ printf("fail \"destroy_listener\": %d\n", ret);
+ }
+
+ printf("Test was terminated with error.\n");
+}
\ No newline at end of file
--- /dev/null
+/*
+ * sensor-test
+ *
+ * Copyright (c) 2022 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+void stress_test(int type);
+++ /dev/null
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-PROJECT(sensor-test C)
-
-SET(PKG_MODULES
- glib-2.0)
-
-INCLUDE(FindPkgConfig)
-PKG_CHECK_MODULES(PKGS REQUIRED ${PKG_MODULES})
-
-INCLUDE_DIRECTORIES(${PKGS_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
-
-FOREACH(flag ${REQUIRED_PKGS_CFLAGS})
- SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}")
-ENDFOREACH(flag)
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS} -fPIE")
-SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
-
-# Installing files
-FILE(GLOB_RECURSE SRCS main.c spec_test.c stress_test.c)
-ADD_EXECUTABLE(${PROJECT_NAME} ${SRCS})
-SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE C)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PKGS_LDFLAGS} capi-system-sensor)
-INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR})
+++ /dev/null
-/*
- * sensor-test
- *
- * Copyright (c) 2022 Samsung Electronics Co., Ltd.
- *
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <getopt.h>
-
-#include <sensor.h>
-
-#include "spec_test.h"
-#include "stress_test.h"
-#include "sensor_info.h"
-
-static void usage(void)
-{
- static bool printed = false;
-
- if (!printed) {
- printf("Usage : sensor-test [OPTION]\n");
- printf("Example : sensor-test -t stress -s 1\n");
- printf("[-h|--help] : Show this help\n");
- printf("[-t|--test_type] : spec, stress\n");
- printf("\tspec : verifying measured value of range\n");
- printf("\tstress : giving stress to sensord\n");
- printf("[-s|--sensor_type] : (default is all in spec, accelerometer in stress)\n");
- printf("\t0 : %s (It is valid in only spec test, not stress test)\n", sensor_infos[0].name);
- for (int i = 1; i < SENSOR_NUM; ++i) {
- printf("\t%d : %s\n", i, sensor_infos[i].name);
- }
- printed = true;
- }
-}
-
-int main(int argc, char *argv[])
-{
- if (argc < 2) {
- usage();
- return 0;
- }
-
- static const struct option long_options[] = {
- {"test_type", required_argument, NULL, 't'},
- {"sensor_type", required_argument, NULL, 's'},
- {"help", no_argument, NULL, 'h'},
- {0, 0, 0, 0},
- };
-
- int mode = 0;
- int sensor_type = 0;
-
- for (;;) {
- int opt = getopt_long(argc, argv, "t:s:h", long_options, NULL);
- if (opt == -1)
- break;
-
- switch (opt) {
- case 't':
- if (!strcmp(optarg, "spec"))
- mode = 1;
- else if (!strcmp(optarg, "stress"))
- mode = 2;
- break;
- case 's':
- sensor_type = atoi(optarg);
- if (sensor_type < 0 || sensor_type >= SENSOR_NUM) {
- printf("Wrong sensor number. Sensor number range is 0 ~ %d.\n", SENSOR_NUM - 1);
- return 0;
- }
- break;
- case 'h':
- usage();
- break;
- default:
- break;
- }
- }
-
- if (sensor_type) {
- bool supported;
- int ret;
- ret = sensor_is_supported(sensor_infos[sensor_type].type, &supported);
- if (ret < 0 || !supported) {
- printf("%s is not supported in this device.\n", sensor_infos[sensor_type].name);
- return 0;
- }
- }
-
- else if (mode == 2) {
- sensor_type = 1;
- }
-
- if (mode == 1)
- spec_test(sensor_type);
- else if (mode == 2)
- stress_test(sensor_type);
- else {
- printf("Enter the test type (spec or stress).\n");
- usage();
- }
-
- return 0;
-}
+++ /dev/null
-#define SENSOR_NUM 28
-
-struct sensor_info {
- sensor_type_e type;
- const char* name;
- int value_num;
-};
-
-static const struct sensor_info sensor_infos[] = {
- {SENSOR_ALL, "All", 0},
- {SENSOR_ACCELEROMETER, "Accelerometer", 3},
- {SENSOR_GRAVITY, "Gravity sensor", 3},
- {SENSOR_LINEAR_ACCELERATION, "Linear acceleration sensor", 3},
- {SENSOR_MAGNETIC, "Magnetic sensor", 3},
- {SENSOR_ROTATION_VECTOR, "Rotation vector sensor", 4},
- {SENSOR_ORIENTATION, "Orientation sensor", 3},
- {SENSOR_GYROSCOPE, "Gyroscope", 3},
- {SENSOR_LIGHT, "Light sensor", 1},
- {SENSOR_PROXIMITY, "Proximity sensor", 1},
- {SENSOR_PRESSURE, "Pressure sensor", 1},
- {SENSOR_ULTRAVIOLET, "Ultraviolet sensor", 1},
- {SENSOR_TEMPERATURE, "Temperature sensor", 1},
- {SENSOR_HUMIDITY, "Humidity sensor", 1},
- {SENSOR_HRM, "Heart-rate monitor", 1},
- {SENSOR_HRM_LED_GREEN, "Green LED sensor of HRM", 1},
- {SENSOR_HRM_LED_IR, "Infra-Red LED sensor of HRM", 1},
- {SENSOR_HRM_LED_RED, "Red LED sensor of HRM", 1},
- {SENSOR_GYROSCOPE_UNCALIBRATED, "Uncalibrated Gyroscope sensor", 6},
- {SENSOR_GEOMAGNETIC_UNCALIBRATED, "Uncalibrated Geomagnetic sensor", 6},
- {SENSOR_GYROSCOPE_ROTATION_VECTOR, "Gyroscope-based rotation vector sensor", 4},
- {SENSOR_GEOMAGNETIC_ROTATION_VECTOR, "Geomagnetic-based rotation vector sensor", 4},
- {SENSOR_SIGNIFICANT_MOTION, "Significant motion sensor", 1},
- {SENSOR_HRM_BATCH, "Heart-rate monitor batch sensor", 1},
- {SENSOR_HRM_LED_GREEN_BATCH, "Green LED of HRM batch sensor", 1},
- {SENSOR_HUMAN_PEDOMETER, "Pedometer", 3},
- {SENSOR_HUMAN_SLEEP_MONITOR, "Sleep monitor", 1},
- {SENSOR_HUMAN_SLEEP_DETECTOR, "Sleep detector", 1},
-};
+++ /dev/null
-/*
- * sensor-test
- *
- * Copyright (c) 2022 Samsung Electronics Co., Ltd.
- *
- * 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 <stdio.h>
-#include <glib.h>
-#include <sensor.h>
-
-#include "spec_test.h"
-#include "sensor_info.h"
-
-#define TEST_COUNT_ONE 100
-#define TEST_COUNT_ALL 1000
-#define PASS 90
-
-static GMainLoop *loop;
-static bool turned_on[SENSOR_NUM];
-static int min_range[SENSOR_NUM];
-static int max_range[SENSOR_NUM];
-static int called_num[SENSOR_NUM];
-static int miss[SENSOR_NUM];
-static int callback_count;
-static bool running;
-
-
-static void sensor_one_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
-{
- sensor_type_e type;
- sensor_get_type(sensor, &type);
- int num = 0;
- bool missed;
-
- ++callback_count;
-
- for (int i = 1; i < SENSOR_NUM; ++i){
- if (sensor_infos[i].type == type){
- num = i;
- break;
- }
- }
-
- for (int i = 0; i < events_count; ++i) {
- missed = false;
- ++called_num[num];
-
- int value_count;
- if (events[i].value_count < sensor_infos[num].value_num)
- value_count = events[i].value_count;
- else
- value_count = sensor_infos[num].value_num;
-
- for (int j = 0; j < value_count; ++j) {
- if (events[i].values[j] < min_range[num] || events[i].values[j] > max_range[num])
- missed = true;
- }
-
- if (missed) {
- ++miss[num];
- g_print("miss : ");
- for (int j = 0; j < value_count; ++j)
- g_print("%lf ", events[i].values[j]);
- g_print("\n");
- }
- }
-
-
- if (running && callback_count >= TEST_COUNT_ONE) {
- g_print("done.\n");
- running = false;
- g_main_loop_quit(loop);
- }
-
- else if (callback_count == TEST_COUNT_ONE / 10 * 2) {
- g_print("20%%...\n");
- }
-
- else if (callback_count == TEST_COUNT_ONE / 10 * 4) {
- g_print("40%%...\n");
- }
-
- else if (callback_count == TEST_COUNT_ONE / 10 * 6) {
- g_print("60%%...\n");
- }
-
- else if (callback_count == TEST_COUNT_ONE / 10 * 8) {
- g_print("80%%...\n");
- }
-}
-
-static void sensor_all_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
-{
- sensor_type_e type;
- sensor_get_type(sensor, &type);
- int num = 0;
- bool missed;
-
- ++callback_count;
-
- for (int i = 1; i < SENSOR_NUM; ++i){
- if (sensor_infos[i].type == type){
- num = i;
- break;
- }
- }
-
- for (int i = 0; i < events_count; ++i) {
- missed = false;
- ++called_num[num];
-
- int value_count;
- if (events[i].value_count < sensor_infos[num].value_num)
- value_count = events[i].value_count;
- else
- value_count = sensor_infos[num].value_num;
-
- for (int j = 0; j < value_count; ++j) {
- if (events[i].values[j] < min_range[num] || events[i].values[j] > max_range[num]) {
- missed = true;
- }
- }
-
- if (missed)
- ++miss[num];
- }
-
- if (running && callback_count >= TEST_COUNT_ALL) {
- g_print("done.\n");
- running = false;
- g_main_loop_quit(loop);
- }
-
- else if (callback_count == TEST_COUNT_ALL / 10 * 2) {
- g_print("20%%...");
- }
-
- else if (callback_count == TEST_COUNT_ALL / 10 * 4) {
- g_print("40%%...");
- }
-
- else if (callback_count == TEST_COUNT_ALL / 10 * 6) {
- g_print("60%%...");
- }
-
- else if (callback_count == TEST_COUNT_ALL / 10 * 8) {
- g_print("80%%...");
- }
-}
-
-static int create_sensor_listener(int num, sensor_h *sensor, sensor_listener_h *listener, bool all)
-{
- bool supported;
- int ret = 0;
- float min, max;
- const char *name = sensor_infos[num].name;
- sensor_type_e type = sensor_infos[num].type;
-
- ret = sensor_is_supported(type, &supported);
- if (ret < 0) {
- printf("fail \"sensor_is_supported\"\n");
- return ret;
- }
-
- else if (!supported) {
- return -1;
- }
-
- ret = sensor_get_default_sensor(type, sensor);
- if (ret < 0) {
- printf("fail \"sensor_get_default_sensor\"\n");
- return ret;
- }
-
- ret = sensor_create_listener(*sensor, listener);
- if (ret < 0) {
- printf("fail \"sensor_create_listener\"\n");
- return ret;
- }
-
- if (all)
- ret = sensor_listener_set_events_cb(*listener, sensor_all_callback, NULL);
- else
- ret = sensor_listener_set_events_cb(*listener, sensor_one_callback, NULL);
- if (ret < 0) {
- printf("fail \"sensor_listener_set_events_cb\"\n");
- return ret;
- }
-
- ret = sensor_listener_set_option(*listener, SENSOR_OPTION_ALWAYS_ON);
- if (ret < 0) {
- printf("fail \"sensor_listener_set_option\"\n");
- return ret;
- }
-
- ret = sensor_listener_set_interval(*listener, 10);
- if (ret < 0) {
- printf("fail \"sensor_listener_set_interval\"\n");
- return ret;
- }
-
- ret = sensor_get_min_range(*sensor, &min);
- if (ret < 0) {
- printf("fail \"sensor_get_min_range\"\n");
- return ret;
- }
-
- ret = sensor_get_max_range(*sensor, &max);
- if (ret < 0) {
- printf("fail \"sensor_get_max_range\"\n");
- return ret;
- }
-
- min_range[num] = min;
- max_range[num] = max;
-
- printf("%s : %f ~ %f\n", name, min, max);
-
- ret = sensor_listener_start(*listener);
- if (ret < 0) {
- printf("%s error in listnener_start : %d\n", name, ret);
- return ret;
- }
-
- return 0;
-}
-
-static void spec_test_one(int type)
-{
- sensor_h sensor;
- sensor_listener_h listener;
- int ret = create_sensor_listener(type, &sensor, &listener, false);
- if (ret < 0)
- return;
-
- running = true;
- loop = g_main_loop_new(NULL, FALSE);
- g_main_loop_run(loop);
-
- printf("=====================================\n");
- printf("test result\n");
- printf("=====================================\n");
-
- double rate = (double) (called_num[type] - miss[type]) / called_num[type] * 100;
- printf("%s : %d / %d \n", sensor_infos[type].name, called_num[type] - miss[type], called_num[type]);
-
- if (rate >= PASS)
- printf("PASS %.2lf%%\n", rate);
- else
- printf("FAIL %.2lf%%\n", rate);
-}
-
-static void spec_test_all(void)
-{
- sensor_h sensor[SENSOR_NUM];
- sensor_listener_h listener[SENSOR_NUM];
- int usable = 0;
-
- for (int i = 1; i < SENSOR_NUM; ++i) {
- called_num[i] = 0;
- miss[i] = 0;
- if (create_sensor_listener(i, &sensor[i], &listener[i], true)) {
- turned_on[i] = false;
- }
- else {
- turned_on[i] = true;
- ++usable;
- }
- }
-
- if (usable == 0) {
- printf("There is no supported sensor.\n");
- return;
- }
-
- printf("testing... ");
-
- running = true;
- loop = g_main_loop_new(NULL, FALSE);
- g_main_loop_run(loop);
-
- printf("=====================================\n");
- printf("test result\n");
- printf("=====================================\n");
-
- int total_miss = 0;
- double rate = 0;
- for (int i = 1; i < SENSOR_NUM; ++i) {
- if (turned_on[i]) {
- if (called_num[i]) {
- rate = (double) (called_num[i] - miss[i]) / called_num[i] * 100;
- printf("%s : %d / %d \n", sensor_infos[i].name, called_num[i] - miss[i], called_num[i]);
-
- if (rate >= PASS)
- printf("PASS %.2lf%%\n", rate);
- else
- printf("FAIL %.2lf%%\n", rate);
- }
- else {
- printf("%s value change was not occurred.\n", sensor_infos[i].name);
- }
- }
- total_miss += miss[i];
- }
-
- printf("total : %d / %d \n", callback_count - total_miss, callback_count);
- rate = (double) (callback_count - total_miss) / callback_count * 100;
- if (rate >= PASS)
- printf("PASS %.2lf%%\n", rate);
- else
- printf("FAIL %.2lf%%\n", rate);
-
- printf("=====================================\n");
- printf("Not supported sensors list\n");
- printf("=====================================\n");
-
- for (int i = 1; i < SENSOR_NUM; ++i) {
- if (turned_on[i] == false)
- printf("%s\n", sensor_infos[i].name);
- }
-}
-
-void spec_test(int type)
-{
- printf("=====================================\n");
- printf("Sensor spec test (%s)\n", sensor_infos[type].name);
- printf("=====================================\n");
-
- if (type == 0)
- spec_test_all();
- else
- spec_test_one(type);
-}
+++ /dev/null
-/*
- * sensor-test
- *
- * Copyright (c) 2022 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-void spec_test(int type);
+++ /dev/null
-/*
- * sensor-test
- *
- * Copyright (c) 2022 Samsung Electronics Co., Ltd.
- *
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <sensor.h>
-
-#include "stress_test.h"
-#include "sensor_info.h"
-
-void sensor_events_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data)
-{
-}
-
-void sensor_listener_set_accuracy_callback(sensor_h sensor, unsigned long long timestamp, sensor_data_accuracy_e accuracy, void *data)
-{
-}
-
-void stress_test(int type)
-{
- printf("=====================================\n");
- printf("Sensor stress test (%s)\n", sensor_infos[type].name);
- printf("=====================================\n");
- printf("Testing... Check sensord memory. (e.g. memps -s <pid>)\n");
- printf("If you need, additional execution of this stress test binary in another shell can add more stress to the sensord.\n.");
-
- sensor_h sh;
- sensor_listener_h lh;
- sensor_event_s* events;
- int ret, count;
- while (1) {
- ret = sensor_get_default_sensor(sensor_infos[type].type, &sh);
- if (ret < 0) {
- printf("fail \"get_default_sensor\": %d \n", ret);
- break;
- }
-
- ret = sensor_create_listener(sh, &lh);
- if (ret < 0) {
- printf("fail \"create_listener\": %d\n", ret);
- break;
- }
-
- ret = sensor_listener_start(lh);
- if (ret < 0)
- printf("fail \"listener_start\": %d\n", ret);
-
- ret = sensor_listener_set_events_cb(lh, sensor_events_callback, NULL);
- if (ret < 0)
- printf("fail \"listener_set_events_cb\": %d\n", ret);
-
- ret = sensor_listener_unset_event_cb(lh);
- if (ret < 0)
- printf("fail \"listener_unset_event_cb\": %d\n", ret);
-
- ret = sensor_listener_set_accuracy_cb(lh, sensor_listener_set_accuracy_callback, NULL);
- if (ret < 0)
- printf("fail \"listener_set_accuracy_cb\": %d\n", ret);
-
- ret = sensor_listener_unset_accuracy_cb(lh);
- if (ret < 0)
- printf("fail \"listener_unset_accuracy_cb\": %d\n", ret);
-
- ret = sensor_listener_read_data_list(lh, &events, &count);
- if (ret == SENSOR_ERROR_OPERATION_FAILED)
- printf("fail \"listener_read_data\". There is no data to read.\n");
- else if (ret < 0)
- printf("fail \"listener_read_data\": %d\n", ret);
- else
- free(events);
-
- ret = sensor_listener_set_interval(lh, 10);
- if (ret < 0)
- printf("fail \"listener_set_interval\": %d\n", ret);
-
- ret = sensor_listener_set_max_batch_latency(lh, 10);
- if (ret < 0 && ret != SENSOR_ERROR_NOT_SUPPORTED)
- printf("fail \"listener_set_max_batch_latency\": %d\n", ret);
-
- ret = sensor_listener_set_attribute_int(lh, SENSOR_ATTRIBUTE_AXIS_ORIENTATION, 1);
- if (ret < 0)
- printf("fail \"listener_set_attribute_int\": %d\n", ret);
-
- ret = sensor_listener_set_option(lh, SENSOR_OPTION_ALWAYS_ON);
- if (ret < 0)
- printf("fail \"listener_set_option\": %d\n", ret);
-
- ret = sensor_listener_stop(lh);
- if (ret < 0)
- printf("fail \"listener_stop\": %d\n", ret);
-
- ret = sensor_destroy_listener(lh);
- if (ret < 0)
- printf("fail \"destroy_listener\": %d\n", ret);
- }
-
- printf("Test was terminated with error.\n");
-}
\ No newline at end of file
+++ /dev/null
-/*
- * sensor-test
- *
- * Copyright (c) 2022 Samsung Electronics Co., Ltd.
- *
- * 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.
- *
- */
-
-void stress_test(int type);
projects / platform / core / api / sensor.git / commitdiff