| /* |
| $License: |
| Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. |
| $ |
| */ |
| |
| /****************************************************************************** |
| * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $ |
| *****************************************************************************/ |
| |
| #define _USE_MATH_DEFINES |
| #include <stdio.h> |
| #include <stddef.h> |
| #include <math.h> |
| #include <string.h> |
| |
| #include "ml.h" |
| #include "mlmath.h" |
| #include "gesture.h" |
| #include "orientation.h" |
| #include "gestureMenu.h" |
| #include "fifo.h" |
| |
| #undef MPL_LOG_TAG |
| #define MPL_LOG_TAG "gest" |
| #include "log.h" |
| #include "mldl_cfg.h" |
| |
| static unsigned long sensors[] = { |
| INV_NINE_AXIS, |
| INV_THREE_AXIS_GYRO, |
| INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL, |
| INV_THREE_AXIS_ACCEL, |
| INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS, |
| INV_THREE_AXIS_COMPASS, |
| INV_SIX_AXIS_GYRO_ACCEL, |
| INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS, |
| INV_SIX_AXIS_ACCEL_COMPASS, |
| }; |
| |
| static char *sensors_string[] = { |
| "INV_NINE_AXIS", |
| "INV_THREE_AXIS_GYRO", |
| "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL", |
| "INV_THREE_AXIS_ACCEL", |
| "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS", |
| "INV_THREE_AXIS_COMPASS", |
| "INV_SIX_AXIS_GYRO_ACCEL", |
| "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS", |
| "INV_SIX_AXIS_ACCEL_COMPASS", |
| }; |
| |
| /** |
| * Prints the menu with the current thresholds |
| * |
| * @param params The parameters to print |
| */ |
| void PrintGestureMenu(tGestureMenuParams const * const params) |
| { |
| MPL_LOGI("Press h at any time to re-display this menu\n"); |
| MPL_LOGI("TAP PARAMETERS:\n"); |
| MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n"); |
| MPL_LOGI(" j : Increase X threshold : %5d\n", |
| params->xTapThreshold); |
| MPL_LOGI(" J (Shift-j): Decrease X threshold\n"); |
| MPL_LOGI(" k : Increase Y threshold : %5d\n", |
| params->yTapThreshold); |
| MPL_LOGI(" K (Shift-k): Decrease Y threshold\n"); |
| MPL_LOGI(" i : Increase Z threshold : %5d\n", |
| params->zTapThreshold); |
| MPL_LOGI(" I (Shift-i): Decrease Z threshold\n"); |
| MPL_LOGI(" l : Increase tap time : %5d\n", |
| params->tapTime); |
| MPL_LOGI(" L (Shift-l): Decrease tap time\n"); |
| MPL_LOGI(" o : Increase next tap time : %5d\n", |
| params->nextTapTime); |
| MPL_LOGI(" O (Shift-o): Increase next tap time\n"); |
| MPL_LOGI(" u : Increase max Taps : %5d\n", |
| params->maxTaps); |
| MPL_LOGI(" U (Shift-u): Increase max Taps\n"); |
| |
| MPL_LOGI("SHAKE PARAMETERS:\n"); |
| MPL_LOGI(" x : Increase X threshold : %5d\n", |
| params->xShakeThresh); |
| MPL_LOGI(" X (Shift-x): Decrease X threshold\n"); |
| MPL_LOGI(" y : Increase Y threshold : %5d\n", |
| params->yShakeThresh); |
| MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n"); |
| MPL_LOGI(" z : Increase Z threshold : %5d\n", |
| params->zShakeThresh); |
| MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n"); |
| MPL_LOGI(" s : Toggle Shake Function : %5d\n", |
| params->shakeFunction); |
| MPL_LOGI(" t : Increase Shake Time : %5d\n", |
| params->shakeTime); |
| MPL_LOGI(" T (Shift-T): Decrease Shake Time\n"); |
| MPL_LOGI(" n : Increase Next Shake Time : %5d\n", |
| params->nextShakeTime); |
| MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n"); |
| MPL_LOGI(" m : Increase max Shakes : %5d\n", |
| params->maxShakes); |
| MPL_LOGI(" M (Shift-m): Decrease max Shakes\n"); |
| MPL_LOGI("SNAP PARAMETERS:\n"); |
| MPL_LOGI(" p : Increase Pitch threshold : %5d\n", |
| params->xSnapThresh); |
| MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n"); |
| MPL_LOGI(" r : Increase Roll threshold : %5d\n", |
| params->ySnapThresh); |
| MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n"); |
| MPL_LOGI(" a : Increase yAw threshold : %5d\n", |
| params->zSnapThresh); |
| MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n"); |
| MPL_LOGI("YAW ROTATION PARAMETERS:\n"); |
| MPL_LOGI(" e : Increase yaW Rotate time : %5d\n", |
| params->yawRotateTime); |
| MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n"); |
| MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n", |
| params->yawRotateThreshold); |
| MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n"); |
| MPL_LOGI("ORIENTATION PARAMETER:\n"); |
| MPL_LOGI(" d : Increase orientation angle threshold : %5f\n", |
| params->orientationThreshold); |
| MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n"); |
| MPL_LOGI("FIFO RATE:\n"); |
| MPL_LOGI(" f : Increase fifo divider : %5d\n", |
| inv_get_fifo_rate()); |
| MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); |
| MPL_LOGI("REQUESTED SENSORS:\n"); |
| MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n", |
| sensors_string[params->sensorsIndex]); |
| MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); |
| |
| /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */ |
| /* S is available */ |
| |
| MPL_LOGI("\n\n"); |
| } |
| |
| /** |
| * Handles a keyboard input and updates an appropriate threshold, prints then |
| * menu or returns false if the character is not processed. |
| * |
| * @param params The parameters to modify if the thresholds are updated |
| * @param ch The input character |
| * |
| * @return true if the character was processed, false otherwise |
| */ |
| inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch) |
| { |
| int result = INV_SUCCESS; |
| /* Dynamic keyboard processing */ |
| |
| switch (ch) { |
| case 'j': |
| params->xTapThreshold += 20; |
| // Intentionally fall through |
| case 'J': { |
| params->xTapThreshold -= 10; |
| if (params->xTapThreshold < 0) params->xTapThreshold = 0; |
| result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetTapThresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n", |
| params->xTapThreshold); |
| } break; |
| case 'k': |
| params->yTapThreshold += 20; |
| // Intentionally fall through |
| case 'K': { |
| params->yTapThreshold -= 10; |
| if (params->yTapThreshold < 0) params->yTapThreshold = 0; |
| result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetTapThresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n", |
| params->yTapThreshold); |
| } break; |
| case 'i': |
| params->zTapThreshold += 20; |
| // Intentionally fall through |
| case 'I': { |
| params->zTapThreshold -= 10; |
| if (params->zTapThreshold < 0) params->zTapThreshold = 0; |
| result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetTapThresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n", |
| params->zTapThreshold); |
| } break; |
| |
| case 'l': |
| params->tapTime += 20; |
| // Intentionally fall through |
| case 'L': { |
| params->tapTime -= 10; |
| if (params->tapTime < 0) params->tapTime = 0; |
| result = inv_set_next_tap_time(params->tapTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime); |
| } break; |
| case 'o': |
| params->nextTapTime += 20; |
| // Intentionally fall through |
| case 'O': { |
| params->nextTapTime -= 10; |
| if (params->nextTapTime < 0) params->nextTapTime = 0; |
| result = MLSetNextTapTime(params->nextTapTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetNextTapTime returned :%d\n", result); |
| } |
| MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime); |
| } break; |
| case 'u': |
| params->maxTaps += 2; |
| // Intentionally fall through |
| case 'U': { |
| params->maxTaps -= 1; |
| if (params->maxTaps < 0) params->maxTaps = 0; |
| result = inv_set_max_taps(params->maxTaps); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_max_taps returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps); |
| } break; |
| case 's': { |
| int shakeParam; |
| params->shakeFunction = (params->shakeFunction + 1) % 2; |
| switch (params->shakeFunction) |
| { |
| case 0: |
| shakeParam = INV_NO_RETRACTION; |
| MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n"); |
| break; |
| case 1: |
| shakeParam = INV_RETRACTION; |
| MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n"); |
| break; |
| }; |
| result = inv_set_shake_func(shakeParam); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_func returned :%d\n", result); |
| } |
| } break; |
| case 'x': |
| params->xShakeThresh += 200; |
| // Intentionally fall through |
| case 'X': { |
| params->xShakeThresh -= 100; |
| result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh); |
| } break; |
| case 'y': |
| params->yShakeThresh += 200; |
| // Intentionally fall through |
| case 'Y': { |
| params->yShakeThresh -= 100; |
| result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh); |
| } break; |
| case 'z': |
| params->zShakeThresh += 200; |
| // Intentionally fall through |
| case 'Z':{ |
| params->zShakeThresh -= 100; |
| result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh); |
| } break; |
| case 'r': |
| params->ySnapThresh += 20; |
| // Intentionally fall through |
| case 'R': { |
| params->ySnapThresh -= 10; |
| result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh); |
| } break; |
| case 'p': |
| params->xSnapThresh += 20; |
| // Intentionally fall through |
| case 'P': { |
| params->xSnapThresh -= 10; |
| result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n", |
| params->xSnapThresh); |
| } break; |
| case 'a': |
| params->zSnapThresh += 20; |
| case 'A': { |
| params->zSnapThresh -= 10; |
| result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh); |
| } break; |
| |
| case 't': |
| params->shakeTime += 20; |
| case 'T':{ |
| params->shakeTime -= 10; |
| result = inv_set_shake_time(params->shakeTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_time returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime); |
| } break; |
| case 'n': |
| params->nextShakeTime += 20; |
| case 'N':{ |
| params->nextShakeTime -= 10; |
| result = inv_set_next_shake_time(params->nextShakeTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime); |
| } break; |
| case 'm': |
| params->maxShakes += 2; |
| case 'M':{ |
| params->maxShakes -= 1; |
| result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_max_shakes returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes); |
| } break; |
| case 'e': |
| params->yawRotateTime += 20; |
| case 'E':{ |
| params->yawRotateTime -= 10; |
| result = inv_set_yaw_rotate_time(params->yawRotateTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime); |
| } break; |
| case 'w': |
| params->yawRotateThreshold += 2; |
| case 'W':{ |
| params->yawRotateThreshold -= 1; |
| result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold); |
| } break; |
| case 'c': |
| params->shakeRejectValue += 0.20f; |
| case 'C':{ |
| params->shakeRejectValue -= 0.10f; |
| result = inv_set_tap_shake_reject(params->shakeRejectValue); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue); |
| } break; |
| case 'd': |
| params->orientationThreshold += 10; |
| case 'D':{ |
| params->orientationThreshold -= 5; |
| if (params->orientationThreshold > 90) { |
| params->orientationThreshold = 90; |
| } |
| |
| if (params->orientationThreshold < 0 ) { |
| params->orientationThreshold = 0; |
| } |
| |
| result = inv_set_orientation_thresh(params->orientationThreshold, |
| 5, 80, |
| INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result); |
| } |
| MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d," |
| " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n", |
| params->orientationThreshold, 5, 80); |
| } break; |
| case 'f': |
| result = inv_set_fifo_rate(inv_get_fifo_rate() + 1); |
| MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); |
| break; |
| case 'F': |
| { |
| unsigned short newRate = inv_get_fifo_rate(); |
| if (newRate > 0) |
| newRate--; |
| result = inv_set_fifo_rate(newRate); |
| MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); |
| break; |
| } |
| case 'S': |
| params->sensorsIndex++; |
| if (params->sensorsIndex >= ARRAY_SIZE(sensors)) { |
| params->sensorsIndex = 0; |
| } |
| result = inv_set_mpu_sensors( |
| sensors[params->sensorsIndex] & params->available_sensors); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result, |
| sensors_string[params->sensorsIndex]); |
| break; |
| case 'h': |
| case 'H': { |
| PrintGestureMenu(params); |
| } break; |
| default: { |
| result = INV_ERROR; |
| } break; |
| }; |
| return result; |
| } |
| |
| /** |
| * Initializes the tGestureMenuParams to a set of defaults. |
| * |
| * @param params The parameters to initialize. |
| */ |
| void GestureMenuSetDefaults(tGestureMenuParams * const params) |
| { |
| params->xTapThreshold = 100; |
| params->yTapThreshold = 100; |
| params->zTapThreshold = 100; |
| params->tapTime = 100; |
| params->nextTapTime = 600; |
| params->maxTaps = 2; |
| params->shakeRejectValue = 0.8f; |
| params->xShakeThresh = 750; |
| params->yShakeThresh = 750; |
| params->zShakeThresh = 750; |
| params->xSnapThresh = 160; |
| params->ySnapThresh = 320; |
| params->zSnapThresh = 160; |
| params->shakeTime = 100; |
| params->nextShakeTime = 1000; |
| params->shakeFunction = 0; |
| params->maxShakes = 3; |
| params->yawRotateTime = 80; |
| params->yawRotateThreshold = 70; |
| params->orientationThreshold = 60; |
| params->sensorsIndex = 0; |
| params->available_sensors = INV_NINE_AXIS; |
| } |
| |
| void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, |
| unsigned long available_sensors) |
| { |
| params->available_sensors = available_sensors; |
| } |
| /** |
| * Call the appropriate MPL set threshold functions and checkes the error codes |
| * |
| * @param params The parametrs to use in setting the thresholds |
| * |
| * @return INV_SUCCESS or the first error code encountered. |
| */ |
| inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params) |
| { |
| inv_error_t result = INV_SUCCESS; |
| |
| result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_next_tap_time(params->tapTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); |
| return result; |
| } |
| result = MLSetNextTapTime(params->nextTapTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetNextTapTime returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_max_taps(params->maxTaps); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_max_taps returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_tap_shake_reject(params->shakeRejectValue); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); |
| return result; |
| } |
| |
| //Set up shake gesture |
| result = inv_set_shake_func(params->shakeFunction); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_func returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_shake_time(params->shakeTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_shake_time returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_next_shake_time(params->nextShakeTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_max_shakes returned :%d\n", result); |
| return result; |
| } |
| |
| // Yaw rotate settings |
| result = inv_set_yaw_rotate_time(params->yawRotateTime); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); |
| return result; |
| } |
| result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); |
| return result; |
| } |
| |
| // Orientation settings |
| result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80, |
| INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result); |
| return result; |
| } |
| |
| // Requested Sensors |
| result = inv_set_mpu_sensors( |
| sensors[params->sensorsIndex] & params->available_sensors); |
| if (INV_SUCCESS != result) { |
| MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result, |
| sensors[params->sensorsIndex] & params->available_sensors); |
| return result; |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| void PrintGesture(tGesture* gesture) |
| { |
| float speed; |
| char type[1024]; |
| switch (gesture->type) |
| { |
| case INV_TAP: |
| { |
| if (gesture->meta < 0) { |
| snprintf(type,sizeof(type),"-"); |
| } else { |
| snprintf(type,sizeof(type),"+"); |
| } |
| |
| switch (ABS(gesture->meta)) |
| { |
| case 1: |
| strcat(type,"X"); |
| break; |
| case 2: |
| strcat(type,"Y"); |
| break; |
| case 3: |
| strcat(type,"Z"); |
| break; |
| default: |
| strcat(type,"ERROR"); |
| break; |
| }; |
| MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n", |
| type, |
| gesture->num, |
| gesture->strength, |
| gesture->speed, |
| gesture->reserved, |
| (180 / M_PI) * atan2( |
| (float)gesture->strength, (float)gesture->speed), |
| (180 / M_PI) * atan2( |
| (float)gesture->speed, (float)gesture->reserved), |
| (180 / M_PI) * atan2( |
| (float)gesture->strength, (float)gesture->reserved) |
| ); |
| } |
| break; |
| case INV_ROLL_SHAKE: |
| case INV_PITCH_SHAKE: |
| case INV_YAW_SHAKE: |
| { |
| if (gesture->strength){ |
| snprintf(type, sizeof(type), "Snap : "); |
| } else { |
| snprintf(type, sizeof(type), "Shake: "); |
| } |
| |
| if (gesture->meta==0) { |
| strcat(type, "+"); |
| } else { |
| strcat(type, "-"); |
| } |
| |
| if (gesture->type == INV_ROLL_SHAKE) { |
| strcat(type, "Roll "); |
| } else if (gesture->type == INV_PITCH_SHAKE) { |
| strcat(type, "Pitch "); |
| } else if (gesture->type == INV_YAW_SHAKE) { |
| strcat(type, "Yaw "); |
| } |
| |
| speed = (float)gesture->speed + |
| (float)(gesture->reserved / (float)(1 << 16)); |
| MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed); |
| } |
| break; |
| case INV_YAW_IMAGE_ROTATE: |
| { |
| if (gesture->meta == 0) { |
| snprintf(type, sizeof(type), "Positive "); |
| } else { |
| snprintf(type, sizeof(type), "Negative "); |
| } |
| MPL_LOGI("%s Yaw Image Rotation\n", type); |
| } |
| break; |
| default: |
| MPL_LOGE("Unknown Gesture received\n"); |
| break; |
| } |
| } |
| |
| /** |
| * Prints the new or current orientation using MPL_LOGI and remembers the last |
| * orientation to print orientation flips. |
| * |
| * @param orientation the new or current orientation. 0 to reset. |
| */ |
| void PrintOrientation(unsigned short orientation) |
| { |
| // Determine if it was a flip |
| static int sLastOrientation = 0; |
| int flip = orientation | sLastOrientation; |
| |
| if ((INV_X_UP | INV_X_DOWN) == flip) { |
| MPL_LOGI("Flip about the X Axis: \n"); |
| } else if ((INV_Y_UP | INV_Y_DOWN) == flip) { |
| MPL_LOGI("Flip about the Y axis: \n"); |
| } else if ((INV_Z_UP | INV_Z_DOWN) == flip) { |
| MPL_LOGI("Flip about the Z axis: \n"); |
| } |
| sLastOrientation = orientation; |
| |
| switch (orientation) { |
| case INV_X_UP: |
| MPL_LOGI("X Axis is up\n"); |
| break; |
| case INV_X_DOWN: |
| MPL_LOGI("X Axis is down\n"); |
| break; |
| case INV_Y_UP: |
| MPL_LOGI("Y Axis is up\n"); |
| break; |
| case INV_Y_DOWN: |
| MPL_LOGI("Y Axis is down\n"); |
| break; |
| case INV_Z_UP: |
| MPL_LOGI("Z Axis is up\n"); |
| break; |
| case INV_Z_DOWN: |
| MPL_LOGI("Z Axis is down\n"); |
| break; |
| case 0: |
| break; /* Not an error. Resets sLastOrientation */ |
| default: |
| MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation); |
| break; |
| } |
| } |
| |
| |
