AST_Day_Contest / Mbed 2 deprecated 53L0A1_HandGestureRecognition

prova

Dependencies:   X_NUCLEO_53L0A1 mbed

Fork of 53L0A1_HandGestureRecognition by ST

Middlewares/ST/Gestures/GesturesDetect/Src/tof_gestures_SWIPE_1.c@7:d79cbeda2982, 2017-12-15 (annotated)

Committer:
mapellil
Date:
Fri Dec 15 14:16:16 2017 +0000
Revision:
7:d79cbeda2982
Added hand gesture (L-Rswipe) with L+R sensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mapellil 7:d79cbeda2982 1 /*******************************************************************************
mapellil 7:d79cbeda2982 2 Copyright © 2015, STMicroelectronics International N.V.
mapellil 7:d79cbeda2982 3 All rights reserved.
mapellil 7:d79cbeda2982 4
mapellil 7:d79cbeda2982 5 Use and Redistribution are permitted only in accordance with licensing terms
mapellil 7:d79cbeda2982 6 available at www.st.com under software reference X-CUBE-6180XA1, and provided
mapellil 7:d79cbeda2982 7 that the following conditions are met:
mapellil 7:d79cbeda2982 8 * Redistributions of source code must retain the above copyright
mapellil 7:d79cbeda2982 9 notice, this list of conditions and the following disclaimer.
mapellil 7:d79cbeda2982 10 * Redistributions in binary form must reproduce the above copyright
mapellil 7:d79cbeda2982 11 notice, this list of conditions and the following disclaimer in the
mapellil 7:d79cbeda2982 12 documentation and/or other materials provided with the distribution.
mapellil 7:d79cbeda2982 13 * Neither the name of STMicroelectronics nor the
mapellil 7:d79cbeda2982 14 names of its contributors may be used to endorse or promote products
mapellil 7:d79cbeda2982 15 derived from this software without specific prior written permission.
mapellil 7:d79cbeda2982 16
mapellil 7:d79cbeda2982 17 THIS SOFTWARE IS PROTECTED BY STMICROELECTRONICS PATENTS AND COPYRIGHTS.
mapellil 7:d79cbeda2982 18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
mapellil 7:d79cbeda2982 19 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
mapellil 7:d79cbeda2982 20 WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
mapellil 7:d79cbeda2982 21 NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
mapellil 7:d79cbeda2982 22 IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
mapellil 7:d79cbeda2982 23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
mapellil 7:d79cbeda2982 24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
mapellil 7:d79cbeda2982 25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
mapellil 7:d79cbeda2982 26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
mapellil 7:d79cbeda2982 27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
mapellil 7:d79cbeda2982 28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
mapellil 7:d79cbeda2982 29 ********************************************************************************/
mapellil 7:d79cbeda2982 30
mapellil 7:d79cbeda2982 31 /*
mapellil 7:d79cbeda2982 32 * $Date$
mapellil 7:d79cbeda2982 33 * $Revision$
mapellil 7:d79cbeda2982 34 */
mapellil 7:d79cbeda2982 35
mapellil 7:d79cbeda2982 36 #include "tof_gestures.h"
mapellil 7:d79cbeda2982 37 #include "tof_gestures_SWIPE_1.h"
mapellil 7:d79cbeda2982 38
mapellil 7:d79cbeda2982 39 int tof_gestures_initSWIPE_1(Gesture_SWIPE_1_Data_t *data){
mapellil 7:d79cbeda2982 40 int status=0;
mapellil 7:d79cbeda2982 41 status |= (int)(RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE));
mapellil 7:d79cbeda2982 42 data->meanBackground = 0;
mapellil 7:d79cbeda2982 43 data->meanHand = 0;
mapellil 7:d79cbeda2982 44 data->gestureCode = GESTURES_NULL;
mapellil 7:d79cbeda2982 45 data->cpt = SWIPE_1_SENSITIVITY;
mapellil 7:d79cbeda2982 46 return status;
mapellil 7:d79cbeda2982 47 }
mapellil 7:d79cbeda2982 48
mapellil 7:d79cbeda2982 49 int tof_gestures_detectSWIPE_1(int32_t range_mm, Gesture_SWIPE_1_Data_t *data){
mapellil 7:d79cbeda2982 50 int time = GET_TIME_STAMP();
mapellil 7:d79cbeda2982 51
mapellil 7:d79cbeda2982 52 /* Default state */
mapellil 7:d79cbeda2982 53 if (data->gestureCode == GESTURES_NULL){
mapellil 7:d79cbeda2982 54 // Calculate background distance (averaging)
mapellil 7:d79cbeda2982 55 data->meanBackground = (data->rangeList.count==0) ? 0 : RB_mean(&(data->rangeList));
mapellil 7:d79cbeda2982 56 // Check current distance vs background distance
mapellil 7:d79cbeda2982 57 if (range_mm < (int)((1-SWIPE_1_BACKGROUND_VARIATION) * data->meanBackground)){
mapellil 7:d79cbeda2982 58 // big variation detected => hand entering in FoV
mapellil 7:d79cbeda2982 59 if (data->cpt == 0) {
mapellil 7:d79cbeda2982 60 // Potential spikes (duster ?) skipped
mapellil 7:d79cbeda2982 61 data->cpt = SWIPE_1_SENSITIVITY;
mapellil 7:d79cbeda2982 62 data->gestureCode = GESTURES_HAND_ENTERING;
mapellil 7:d79cbeda2982 63 RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE);
mapellil 7:d79cbeda2982 64 RB_push(&(data->rangeList), range_mm);
mapellil 7:d79cbeda2982 65 data->timestamp = time;
mapellil 7:d79cbeda2982 66 TOF_GESTURES_DEBUG(SWIPE_1,"(background=%d, current=%d) => HAND_ENTERING", data->meanBackground, range_mm);
mapellil 7:d79cbeda2982 67 } else {
mapellil 7:d79cbeda2982 68 data->cpt--;
mapellil 7:d79cbeda2982 69 TOF_GESTURES_DEBUG(SWIPE_1,"(background=%d, current=%d) => SKIP", data->meanBackground, range_mm);
mapellil 7:d79cbeda2982 70 }
mapellil 7:d79cbeda2982 71 }else{
mapellil 7:d79cbeda2982 72 // no variation => keep averaging background
mapellil 7:d79cbeda2982 73 RB_push(&(data->rangeList), range_mm);
mapellil 7:d79cbeda2982 74 }
mapellil 7:d79cbeda2982 75
mapellil 7:d79cbeda2982 76 /* Hand entering */
mapellil 7:d79cbeda2982 77 } else if (data->gestureCode == GESTURES_HAND_ENTERING){
mapellil 7:d79cbeda2982 78 // Calculate hand distance (averaging)
mapellil 7:d79cbeda2982 79 data->meanHand = (data->rangeList.count==0) ? 0 : RB_mean(&(data->rangeList));
mapellil 7:d79cbeda2982 80 // Check current distance vs hand mean distance
mapellil 7:d79cbeda2982 81 if ((range_mm < (data->meanHand * (1-SWIPE_1_HAND_VARIATION))) || (((data->meanHand * (1+SWIPE_1_HAND_VARIATION)) < range_mm))){
mapellil 7:d79cbeda2982 82 // Big variation => Hand leaving
mapellil 7:d79cbeda2982 83 data->gestureCode = GESTURES_HAND_LEAVING;
mapellil 7:d79cbeda2982 84 RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE);
mapellil 7:d79cbeda2982 85 RB_push(&(data->rangeList), range_mm);
mapellil 7:d79cbeda2982 86 data->timestamp = time;
mapellil 7:d79cbeda2982 87 TOF_GESTURES_DEBUG(SWIPE_1,"(meanHand=%d, current=%d) => HAND_LEAVING", data->meanHand, range_mm);
mapellil 7:d79cbeda2982 88 } else if ((time - data->timestamp) > SWIPE_1_MAX_SWIPE_DURATION) {
mapellil 7:d79cbeda2982 89 // Too slow => discard (swipe is too slow, this may be a background change for a long time)
mapellil 7:d79cbeda2982 90 data->gestureCode = GESTURES_DISCARDED_TOO_SLOW;
mapellil 7:d79cbeda2982 91 RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE);
mapellil 7:d79cbeda2982 92 TOF_GESTURES_DEBUG(SWIPE_1,"DISCARDED_TOO_SLOW");
mapellil 7:d79cbeda2982 93 } else {
mapellil 7:d79cbeda2982 94 // No big variation => remains in this state
mapellil 7:d79cbeda2982 95 RB_push(&(data->rangeList), range_mm);
mapellil 7:d79cbeda2982 96 TOF_GESTURES_DEBUG(SWIPE_1, "Wait (meanHand=%d, current=%d)", data->meanHand, range_mm);
mapellil 7:d79cbeda2982 97 }
mapellil 7:d79cbeda2982 98
mapellil 7:d79cbeda2982 99 /* Hand leaving */
mapellil 7:d79cbeda2982 100 } else if (data->gestureCode == GESTURES_HAND_LEAVING){
mapellil 7:d79cbeda2982 101 // Check current distance vs background distance (previously calculated)
mapellil 7:d79cbeda2982 102 if (((data->meanBackground * (1-SWIPE_1_BACKGROUND_VARIATION)) < range_mm) && (range_mm < ((data->meanBackground * (1+SWIPE_1_BACKGROUND_VARIATION))))){
mapellil 7:d79cbeda2982 103 // Almost same => Swipe detected/finished
mapellil 7:d79cbeda2982 104 data->gestureCode = GESTURES_SINGLE_SWIPE;
mapellil 7:d79cbeda2982 105 //data->cpt = 0;
mapellil 7:d79cbeda2982 106 RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE);
mapellil 7:d79cbeda2982 107 //return_code = GESTURES_SINGLE_SWIPE;
mapellil 7:d79cbeda2982 108 TOF_GESTURES_DEBUG(SWIPE_1, "(background=%d, current==%d) => SINGLE_SWIPE\n", data->meanBackground, range_mm);
mapellil 7:d79cbeda2982 109 //uart_printf("2 : Next state : %d, (background=%d), range=%d (SWIPE)\n", data->gestureCode, data->meanBackground, range_mm);
mapellil 7:d79cbeda2982 110 } else if ((time - data->timestamp) > SWIPE_1_BACKGROUND_TIMEOUT){
mapellil 7:d79cbeda2982 111 // To slow to converge back to background distance => discard (background may have change during the gesture)
mapellil 7:d79cbeda2982 112 data->gestureCode = GESTURES_DISCARDED;
mapellil 7:d79cbeda2982 113 RB_init(&(data->rangeList),SWIPE_1_BUFFER_SIZE);
mapellil 7:d79cbeda2982 114 TOF_GESTURES_DEBUG(SWIPE_1,"DISCARDED");
mapellil 7:d79cbeda2982 115 } else {
mapellil 7:d79cbeda2982 116 // Wait for convergence towards background
mapellil 7:d79cbeda2982 117 TOF_GESTURES_DEBUG(SWIPE_1,"Wait (background=%d, current=%d)", data->meanBackground, range_mm);
mapellil 7:d79cbeda2982 118 }
mapellil 7:d79cbeda2982 119
mapellil 7:d79cbeda2982 120 /* Swipe detected or discarded */
mapellil 7:d79cbeda2982 121 } else {
mapellil 7:d79cbeda2982 122 data->gestureCode = GESTURES_NULL;
mapellil 7:d79cbeda2982 123 }
mapellil 7:d79cbeda2982 124
mapellil 7:d79cbeda2982 125 return data->gestureCode;
mapellil 7:d79cbeda2982 126 }
mapellil 7:d79cbeda2982 127