From: taemin.yeom Date: Mon, 23 Aug 2021 08:52:31 +0000 (+0900) Subject: Add a test verifying orientation X-Git-Tag: submit/tizen/20210831.021755^0 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=f57473332de5d7de9919c0d092d560602af52347;p=platform%2Fcore%2Fapi%2Fsensor.git Add a test verifying orientation Change-Id: I53c761da83f6aa7911632ccba2db4a4c7b10f769 Signed-off-by: taemin.yeom --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 6cd4d80..e9338fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,4 +57,5 @@ INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.pc DESTINATION ${CMAKE INSTALL(FILES include/sensor.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor) INSTALL(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) -ADD_SUBDIRECTORY(test) \ No newline at end of file +ADD_SUBDIRECTORY(test/stress_test) +ADD_SUBDIRECTORY(test/orientation_test) \ No newline at end of file diff --git a/packaging/capi-system-sensor.spec b/packaging/capi-system-sensor.spec index 48e50ed..0b49033 100644 --- a/packaging/capi-system-sensor.spec +++ b/packaging/capi-system-sensor.spec @@ -103,7 +103,9 @@ install -m 0644 gcov-obj/* %{buildroot}%{_datadir}/gcov/obj %files test %manifest packaging/capi-system-sensor.manifest %defattr(-,root,root,-) -%{_bindir}/sensor-stresstests +%{_bindir}/sensor-stress_test +%{_bindir}/sensor-orientation_test + %if 0%{?gcov:1} %files gcov diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt deleted file mode 100644 index 8cf32be..0000000 --- a/test/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.6) -PROJECT(sensor-stresstests 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 ${PKGS_CFLAGS}) - SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${flag}") -ENDFOREACH(flag) - -# Installing files -FILE(GLOB_RECURSE SRCS main.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}) diff --git a/test/main.c b/test/main.c deleted file mode 100644 index 97683ab..0000000 --- a/test/main.c +++ /dev/null @@ -1,253 +0,0 @@ -#define SENSOR_NUM 27 -#define TEST_COUNT 1000 -#define PASS 90 - -#include -#include - -static const sensor_type_e sensor_type[] = -{ - SENSOR_ACCELEROMETER, - SENSOR_GRAVITY, - SENSOR_LINEAR_ACCELERATION, - SENSOR_MAGNETIC, - SENSOR_ROTATION_VECTOR, - SENSOR_ORIENTATION, - SENSOR_GYROSCOPE, - SENSOR_LIGHT, - SENSOR_PROXIMITY, - SENSOR_PRESSURE, - SENSOR_ULTRAVIOLET, - SENSOR_TEMPERATURE, - SENSOR_HUMIDITY, - SENSOR_HRM, - SENSOR_HRM_LED_GREEN, - SENSOR_HRM_LED_IR, - SENSOR_HRM_LED_RED, - SENSOR_GYROSCOPE_UNCALIBRATED, - SENSOR_GEOMAGNETIC_UNCALIBRATED, - SENSOR_GYROSCOPE_ROTATION_VECTOR, - SENSOR_GEOMAGNETIC_ROTATION_VECTOR, - SENSOR_SIGNIFICANT_MOTION, - SENSOR_HRM_BATCH, - SENSOR_HRM_LED_GREEN_BATCH, - SENSOR_HUMAN_PEDOMETER, - SENSOR_HUMAN_SLEEP_MONITOR, - SENSOR_HUMAN_SLEEP_DETECTOR -}; - -static const char *sensor_name[] = { - "Accelerometer", - "Gravity sensor", - "Linear acceleration sensor", - "Magnetic sensor", - "Rotation vector sensor", - "Orientation sensor", - "Gyroscope", - "Light sensor", - "Proximity sensor", - "Pressure sensor", - "Ultraviolet sensor", - "Temperature sensor", - "Humidity sensor", - "Heart-rate monitor", - "Green LED sensor of HRM", - "Infra-Red LED sensor of HRM", - "Red LED sensor of HRM", - "Uncalibrated Gyroscope sensor", - "Uncalibrated Geomagnetic sensor", - "Gyroscope-based rotation vector sensor", - "Geomagnetic-based rotation vector sensor", - "Significant motion sensor", - "Heart-rate monitor batch sensor", - "Green LED of HRM batch sensor", - "Pedometer", - "Sleep monitor", - "Sleep detector" -}; - -// the number of value to be verified of the sensor -static const int value_num[] = {3, 3, 3, 3, 4, 3, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 6, 6, 4, 4, 1, 1, 1, 3, 1}; - -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_callback(sensor_h sensor, sensor_event_s events[], int events_count, void *user_data) -{ - const char *name; - sensor_type_e type; - sensor_get_type(sensor, &type); - int num = 0; - bool missed; - - ++callback_count; - - for (int i = 0; i < SENSOR_NUM; ++i){ - if (sensor_type[i] == type){ - name = sensor_name[i]; - num = i; - break; - } - } - - for (int i = 0; i < events_count; ++i) { - missed = false; - ++called_num[num]; - - int value_count; - if (events[i].value_count < value_num[num]) - value_count = events[i].value_count; - else - value_count = value_num[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) { - g_print("done.\n"); - running = false; - g_main_loop_quit(loop); - } - - else if (callback_count == TEST_COUNT / 10 * 2) { - g_print("20%%... "); - } - - else if (callback_count == TEST_COUNT / 10 * 4) { - g_print("40%%... "); - } - - else if (callback_count == TEST_COUNT / 10 * 6) { - g_print("60%%... "); - } - - else if (callback_count == TEST_COUNT / 10 * 8) { - g_print("80%%... "); - } -} - -static int create_sensor_listener(int num, sensor_h *sensor, sensor_listener_h *listener) -{ - bool supported; - int ret; - float min, max; - const char *name = sensor_name[num]; - sensor_type_e type = sensor_type[num]; - - sensor_is_supported(type, &supported); - if(!supported) { - return -1; - } - - sensor_get_default_sensor(type, sensor); - - sensor_create_listener(*sensor, listener); - sensor_listener_set_events_cb(*listener, sensor_callback, NULL); - sensor_listener_set_option(*listener, SENSOR_OPTION_ALWAYS_ON); - sensor_listener_set_interval(*listener, 100); - - sensor_get_min_range(*sensor, &min); - sensor_get_max_range(*sensor, &max); - min_range[num] = min; - max_range[num] = max; - - g_print("%s : %f ~ %f\n", name, min, max); - - ret = sensor_listener_start(*listener); - if (ret < 0) { - g_print("%s error in listnener_start : %d\n", name, ret); - return ret; - } - - return 0; -} - -int main() -{ - g_print("====================\n"); - g_print("sensor stress test\n"); - g_print("====================\n"); - - sensor_h sensor[29]; - sensor_listener_h listener[29]; - int usable = 0; - - for (int i = 0; i < SENSOR_NUM; ++i) { - called_num[i] = 0; - miss[i] = 0; - if (create_sensor_listener(i, &sensor[i], &listener[i])) { - turned_on[i] = false; - } - else { - turned_on[i] = true; - ++usable; - } - } - - if (usable == 0) { - g_print("There is no supported sensor.\n"); - return -1; - } - - g_print("testing... "); - - running = true; - loop = g_main_loop_new(NULL, FALSE); - g_main_loop_run(loop); - - g_print("====================\n"); - g_print("test result\n"); - g_print("====================\n"); - - int total_miss = 0; - double rate = 0; - for (int i = 0; i < SENSOR_NUM; ++i) { - if (turned_on[i]) { - if (called_num[i]) { - rate = (double) (called_num[i] - miss[i]) / called_num[i] * 100; - g_print("%s : %d / %d \n", sensor_name[i], called_num[i] - miss[i], called_num[i]); - - if (rate >= PASS) - g_print("PASS %.2lf%%\n", rate); - else - g_print("FAIL %.2lf%%\n", rate); - } - else { - g_print("%s value change was not occurred.\n", sensor_name[i]); - } - } - total_miss += miss[i]; - } - - g_print("total : %d / %d \n", callback_count - total_miss, callback_count); - rate = (double) (callback_count - total_miss) / callback_count * 100; - if (rate >= PASS) - g_print("PASS %.2lf%%\n", rate); - else - g_print("FAIL %.2lf%%\n", rate); - - g_print("====================\n"); - g_print("Not supported sensors list\n"); - g_print("====================\n"); - - for (int i = 0; i < SENSOR_NUM; ++i) { - if (turned_on[i] == false) - g_print("%s\n", sensor_name[i]); - } - - return 0; -} - diff --git a/test/orientation_test/CMakeLists.txt b/test/orientation_test/CMakeLists.txt new file mode 100644 index 0000000..0ed1401 --- /dev/null +++ b/test/orientation_test/CMakeLists.txt @@ -0,0 +1,36 @@ +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) + +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}) + +#CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +#PROJECT(sensor-stresstests C) +# +#SET(PKG_MODULES +# glib-2.0) +# +#INCLUDE(FindPkgConfig) +#PKG_CHECK_MODULES(PKGS REQUIRED ${PKG_MODULES}) +# +# +#FOREACH(flag ${PKGS_CFLAGS}) +# SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${flag}") +#ENDFOREACH(flag) +# +## Installing files +#FILE(GLOB_RECURSE SRCS main.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}) diff --git a/test/orientation_test/errors.h b/test/orientation_test/errors.h new file mode 100644 index 0000000..95e8092 --- /dev/null +++ b/test/orientation_test/errors.h @@ -0,0 +1,81 @@ +/* + * 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 +#include +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 diff --git a/test/orientation_test/fusion.cpp b/test/orientation_test/fusion.cpp new file mode 100644 index 0000000..4c6b7a0 --- /dev/null +++ b/test/orientation_test/fusion.cpp @@ -0,0 +1,563 @@ +/* + * 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 +#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 +static mat scaleCovariance( + const mat& A, + const mat& P) { + // A*P*transpose(A); + mat APAt; + for (size_t r=0 ; r +static mat crossMatrix(const vec& p, OTHER_TYPE diag) { + mat 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 +class Covariance { + mat mSumXX; + vec mSumX; + size_t mN; +public: + Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } + void update(const vec& x) { + mSumXX += x*transpose(x); + mSumX += x; + mN++; + } + mat 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 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 + diff --git a/test/orientation_test/fusion.h b/test/orientation_test/fusion.h new file mode 100644 index 0000000..00bd481 --- /dev/null +++ b/test/orientation_test/fusion.h @@ -0,0 +1,104 @@ +/* + * 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 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 P; + + /* + * the process noise covariance matrix + */ + mat 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 Phi; + vec3_t Ba, Bm; + uint32_t mInitState; + float mGyroRate; + vec 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 diff --git a/test/orientation_test/main.cpp b/test/orientation_test/main.cpp new file mode 100644 index 0000000..2c218f5 --- /dev/null +++ b/test/orientation_test/main.cpp @@ -0,0 +1,200 @@ +#include +#include +#include +#include + +#include +#include + +#include "tizen_orientation.h" +#include "fusion.h" + +const float TIMEUNIT = 1000000.0f; + +std::vector ev; +std::vector 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 diff --git a/test/orientation_test/mat.h b/test/orientation_test/mat.h new file mode 100644 index 0000000..24647ad --- /dev/null +++ b/test/orientation_test/mat.h @@ -0,0 +1,395 @@ +/* + * 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 +class mat; + +namespace helpers { + +template +mat& doAssign( + mat& lhs, + typename TypeTraits::ParameterType rhs) { + for (size_t i=0 ; i +mat PURE doMul( + const mat& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +vec PURE doMul( + const mat& lhs, + const vec& rhs) +{ + vec res; + for (size_t r=0 ; r +mat PURE doMul( + const vec& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + const mat& rhs, + typename TypeTraits::ParameterType v) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + typename TypeTraits::ParameterType v, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +class mat : public vec< vec, C > { + typedef typename TypeTraits::ParameterType pTYPE; + typedef vec< vec, 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(lhs), + static_cast(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast(lhs), + static_cast(rhs)); + } + + // matrix*matrix + template + friend mat PURE operator * ( + const mat& lhs, + const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec PURE operator * ( + const mat& lhs, const vec& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec& lhs, const mat& 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 + 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 + friend column_builder operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder(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& lhs, + const vec& 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& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template +class mat : public vec { + typedef vec 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& 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 +mat PURE transpose(const mat& m) { + mat r; + for (size_t i=0 ; i static TYPE trace(const mat& m) { + TYPE t; + for (size_t i=0 ; i +static bool isPositiveSemidefinite(const mat& m, TYPE tolerance) { + for (size_t i=0 ; i tolerance) + return false; + + return true; +} + +// Transpose a vector +template < + template class VEC, + typename TYPE, + size_t SIZE +> +mat PURE transpose(const VEC& v) { + mat r; + for (size_t i=0 ; i +mat PURE invert(const mat& src) { + T t; + size_t swap; + mat tmp(src); + mat inverse(1); + + for (size_t i=0 ; i fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k mat22_t; +typedef mat mat33_t; +typedef mat mat44_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_MAT_H */ diff --git a/test/orientation_test/quat.h b/test/orientation_test/quat.h new file mode 100644 index 0000000..0b3b1bb --- /dev/null +++ b/test/orientation_test/quat.h @@ -0,0 +1,100 @@ +/* + * 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 + +#include "vec.h" +#include "mat.h" + +// ----------------------------------------------------------------------- +namespace android { +// ----------------------------------------------------------------------- + +template +mat quatToMatrix(const vec& q) { + mat 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 +vec matrixToQuat(const mat& R) { + // matrix to quaternion + + struct { + inline TYPE operator()(TYPE v) { + return v < 0 ? 0 : v; + } + } clamp; + + vec 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 +vec normalize_quat(const vec& q) { + vec r(q); + if (r.w < 0) { + r = -r; + } + return normalize(r); +} + +// ----------------------------------------------------------------------- + +typedef vec4_t quat_t; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_QUAT_H */ diff --git a/test/orientation_test/tizen_orientation.h b/test/orientation_test/tizen_orientation.h new file mode 100644 index 0000000..40ae45f --- /dev/null +++ b/test/orientation_test/tizen_orientation.h @@ -0,0 +1,118 @@ +#include + +#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 diff --git a/test/orientation_test/traits.h b/test/orientation_test/traits.h new file mode 100644 index 0000000..a371b74 --- /dev/null +++ b/test/orientation_test/traits.h @@ -0,0 +1,120 @@ +/* + * 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 +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList +#define TYPELIST_2(T1, T2) TypeList +#define TYPELIST_3(T1, T2, T3) TypeList +#define TYPELIST_4(T1, T2, T3, T4) TypeList + +// typelists algorithms +namespace TL { +template struct IndexOf; + +template +struct IndexOf { + enum { value = -1 }; +}; + +template +struct IndexOf, T> { + enum { value = 0 }; +}; + +template +struct IndexOf, T> { +private: + enum { temp = IndexOf::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template +struct Select { + typedef T Result; +}; +template +struct Select { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template +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 struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template struct PointerTraits { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf::value >= 0 }; + enum { isStdIntegral = TL::IndexOf::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf::value >= 0 }; + enum { isPointer = PointerTraits::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select::Result ParameterType; +}; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_TRAITS_H */ diff --git a/test/orientation_test/vec.h b/test/orientation_test/vec.h new file mode 100644 index 0000000..ec689c9 --- /dev/null +++ b/test/orientation_test/vec.h @@ -0,0 +1,440 @@ +/* + * 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 + +#include +#include + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template +class vec; + +template +struct vbase; + +namespace helpers { + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +template < template class VEC, + typename TYPE, size_t SIZE, size_t S> +vec& doAssign( + vec& lhs, const VEC& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doAdd( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doSub( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doMulScalar( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + VEC r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doScalarMul( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + VEC r; + for (size_t i=0 ; i. 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 class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator + ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator - ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS& lhs, + const VRHS& rhs) { + TYPE r(0); + for (size_t i=0 ; i class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V& v) { + return sqrt(dot_product(v, v)); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length_squared(const V& v) { + return dot_product(v, v); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +V PURE normalize(const V& v) { + return v * (1/length(v)); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE +> +VLHS PURE cross_product( + const VLHS& u, + const VRHS& v) { + VLHS 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 +vec PURE operator - (const vec& lhs) { + vec r; + for (size_t i=0 ; i +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 { + 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 { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase xyz; + vbase str; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template +class vec : public vbase +{ + typedef typename TypeTraits::ParameterType pTYPE; + typedef vbase 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 class VEC, size_t S> + explicit vec(const VEC& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i class VEC, size_t S> + vec& operator = (const VEC& 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 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 +vec& vec::operator += (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator -= (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator *= (vec::pTYPE rhs) { + vec& lhs(*this); + for (size_t i=0 ; i vec2_t; +typedef vec vec3_t; +typedef vec vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */ diff --git a/test/stress_test/CMakeLists.txt b/test/stress_test/CMakeLists.txt new file mode 100644 index 0000000..ba82a30 --- /dev/null +++ b/test/stress_test/CMakeLists.txt @@ -0,0 +1,22 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +PROJECT(sensor-stress_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 ${PKGS_CFLAGS}) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${flag}") +ENDFOREACH(flag) + +# Installing files +FILE(GLOB_RECURSE SRCS main.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}) diff --git a/test/stress_test/main.c b/test/stress_test/main.c new file mode 100644 index 0000000..355135e --- /dev/null +++ b/test/stress_test/main.c @@ -0,0 +1,251 @@ +#define SENSOR_NUM 27 +#define TEST_COUNT 1000 +#define PASS 90 + +#include +#include + +static const sensor_type_e sensor_type[] = +{ + SENSOR_ACCELEROMETER, + SENSOR_GRAVITY, + SENSOR_LINEAR_ACCELERATION, + SENSOR_MAGNETIC, + SENSOR_ROTATION_VECTOR, + SENSOR_ORIENTATION, + SENSOR_GYROSCOPE, + SENSOR_LIGHT, + SENSOR_PROXIMITY, + SENSOR_PRESSURE, + SENSOR_ULTRAVIOLET, + SENSOR_TEMPERATURE, + SENSOR_HUMIDITY, + SENSOR_HRM, + SENSOR_HRM_LED_GREEN, + SENSOR_HRM_LED_IR, + SENSOR_HRM_LED_RED, + SENSOR_GYROSCOPE_UNCALIBRATED, + SENSOR_GEOMAGNETIC_UNCALIBRATED, + SENSOR_GYROSCOPE_ROTATION_VECTOR, + SENSOR_GEOMAGNETIC_ROTATION_VECTOR, + SENSOR_SIGNIFICANT_MOTION, + SENSOR_HRM_BATCH, + SENSOR_HRM_LED_GREEN_BATCH, + SENSOR_HUMAN_PEDOMETER, + SENSOR_HUMAN_SLEEP_MONITOR, + SENSOR_HUMAN_SLEEP_DETECTOR +}; + +static const char *sensor_name[] = { + "Accelerometer", + "Gravity sensor", + "Linear acceleration sensor", + "Magnetic sensor", + "Rotation vector sensor", + "Orientation sensor", + "Gyroscope", + "Light sensor", + "Proximity sensor", + "Pressure sensor", + "Ultraviolet sensor", + "Temperature sensor", + "Humidity sensor", + "Heart-rate monitor", + "Green LED sensor of HRM", + "Infra-Red LED sensor of HRM", + "Red LED sensor of HRM", + "Uncalibrated Gyroscope sensor", + "Uncalibrated Geomagnetic sensor", + "Gyroscope-based rotation vector sensor", + "Geomagnetic-based rotation vector sensor", + "Significant motion sensor", + "Heart-rate monitor batch sensor", + "Green LED of HRM batch sensor", + "Pedometer", + "Sleep monitor", + "Sleep detector" +}; + +// the number of value to be verified of the sensor +static const int value_num[] = {3, 3, 3, 3, 4, 3, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 6, 6, 4, 4, 1, 1, 1, 3, 1}; + +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_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 = 0; i < SENSOR_NUM; ++i){ + if (sensor_type[i] == 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 < value_num[num]) + value_count = events[i].value_count; + else + value_count = value_num[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) { + g_print("done.\n"); + running = false; + g_main_loop_quit(loop); + } + + else if (callback_count == TEST_COUNT / 10 * 2) { + g_print("20%%... "); + } + + else if (callback_count == TEST_COUNT / 10 * 4) { + g_print("40%%... "); + } + + else if (callback_count == TEST_COUNT / 10 * 6) { + g_print("60%%... "); + } + + else if (callback_count == TEST_COUNT / 10 * 8) { + g_print("80%%... "); + } +} + +static int create_sensor_listener(int num, sensor_h *sensor, sensor_listener_h *listener) +{ + bool supported; + int ret; + float min, max; + const char *name = sensor_name[num]; + sensor_type_e type = sensor_type[num]; + + sensor_is_supported(type, &supported); + if(!supported) { + return -1; + } + + sensor_get_default_sensor(type, sensor); + + sensor_create_listener(*sensor, listener); + sensor_listener_set_events_cb(*listener, sensor_callback, NULL); + sensor_listener_set_option(*listener, SENSOR_OPTION_ALWAYS_ON); + sensor_listener_set_interval(*listener, 100); + + sensor_get_min_range(*sensor, &min); + sensor_get_max_range(*sensor, &max); + min_range[num] = min; + max_range[num] = max; + + g_print("%s : %f ~ %f\n", name, min, max); + + ret = sensor_listener_start(*listener); + if (ret < 0) { + g_print("%s error in listnener_start : %d\n", name, ret); + return ret; + } + + return 0; +} + +int main() +{ + g_print("====================\n"); + g_print("sensor stress test\n"); + g_print("====================\n"); + + sensor_h sensor[29]; + sensor_listener_h listener[29]; + int usable = 0; + + for (int i = 0; i < SENSOR_NUM; ++i) { + called_num[i] = 0; + miss[i] = 0; + if (create_sensor_listener(i, &sensor[i], &listener[i])) { + turned_on[i] = false; + } + else { + turned_on[i] = true; + ++usable; + } + } + + if (usable == 0) { + g_print("There is no supported sensor.\n"); + return -1; + } + + g_print("testing... "); + + running = true; + loop = g_main_loop_new(NULL, FALSE); + g_main_loop_run(loop); + + g_print("====================\n"); + g_print("test result\n"); + g_print("====================\n"); + + int total_miss = 0; + double rate = 0; + for (int i = 0; i < SENSOR_NUM; ++i) { + if (turned_on[i]) { + if (called_num[i]) { + rate = (double) (called_num[i] - miss[i]) / called_num[i] * 100; + g_print("%s : %d / %d \n", sensor_name[i], called_num[i] - miss[i], called_num[i]); + + if (rate >= PASS) + g_print("PASS %.2lf%%\n", rate); + else + g_print("FAIL %.2lf%%\n", rate); + } + else { + g_print("%s value change was not occurred.\n", sensor_name[i]); + } + } + total_miss += miss[i]; + } + + g_print("total : %d / %d \n", callback_count - total_miss, callback_count); + rate = (double) (callback_count - total_miss) / callback_count * 100; + if (rate >= PASS) + g_print("PASS %.2lf%%\n", rate); + else + g_print("FAIL %.2lf%%\n", rate); + + g_print("====================\n"); + g_print("Not supported sensors list\n"); + g_print("====================\n"); + + for (int i = 0; i < SENSOR_NUM; ++i) { + if (turned_on[i] == false) + g_print("%s\n", sensor_name[i]); + } + + return 0; +} +