prova
Dependencies: X_NUCLEO_53L0A1 mbed
Fork of 53L0A1_HandGestureRecognition by
Middlewares/ST/Gestures/GesturesDetect/Src/tof_gestures_TAP_1.c@9:6205ef056c8a, 2018-02-22 (annotated)
- Committer:
- mapellil
- Date:
- Thu Feb 22 15:50:29 2018 +0000
- Revision:
- 9:6205ef056c8a
- Parent:
- 7:d79cbeda2982
AStDay
Who changed what in which revision?
User | Revision | Line number | New 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_TAP_1.h" |
mapellil | 7:d79cbeda2982 | 38 | |
mapellil | 7:d79cbeda2982 | 39 | int tof_gestures_initTAP_1(int MinBackGroundDistance, Gesture_TAP_1_Data_t *data){ |
mapellil | 7:d79cbeda2982 | 40 | int status=0; |
mapellil | 7:d79cbeda2982 | 41 | status |= (int)(RB_init(&(data->rangeList),TAP_1_BUFFER_SIZE)); |
mapellil | 7:d79cbeda2982 | 42 | data->MinBackGroundDistance = MinBackGroundDistance; |
mapellil | 7:d79cbeda2982 | 43 | data->timestampLastDetectedTap = 0; |
mapellil | 7:d79cbeda2982 | 44 | data->nbOfDataToDecide = TAP_1_NB_OF_DATA_TO_DECIDE; |
mapellil | 7:d79cbeda2982 | 45 | data->meanBackground = 0; |
mapellil | 7:d79cbeda2982 | 46 | data->gestureCode = GESTURES_NULL; |
mapellil | 7:d79cbeda2982 | 47 | data->timestamp = 0; |
mapellil | 7:d79cbeda2982 | 48 | return status; |
mapellil | 7:d79cbeda2982 | 49 | } |
mapellil | 7:d79cbeda2982 | 50 | |
mapellil | 7:d79cbeda2982 | 51 | int tof_gestures_detectTAP_1(int32_t range_mm, Gesture_TAP_1_Data_t *data){ |
mapellil | 7:d79cbeda2982 | 52 | int time = GET_TIME_STAMP(); |
mapellil | 7:d79cbeda2982 | 53 | int madRange = 0; |
mapellil | 7:d79cbeda2982 | 54 | int dir; |
mapellil | 7:d79cbeda2982 | 55 | int delta_time = 0; |
mapellil | 7:d79cbeda2982 | 56 | int background_threshold = 0; |
mapellil | 7:d79cbeda2982 | 57 | |
mapellil | 7:d79cbeda2982 | 58 | // Default state |
mapellil | 7:d79cbeda2982 | 59 | if (data->gestureCode == GESTURES_NULL){ |
mapellil | 7:d79cbeda2982 | 60 | // Calculate background distance (averaging) |
mapellil | 7:d79cbeda2982 | 61 | data->meanBackground = (data->rangeList.count==0) ? 0 : RB_mean(&(data->rangeList)); |
mapellil | 7:d79cbeda2982 | 62 | background_threshold = (int)((1-TAP_1_BACKGROUND_VARIATION) * data->meanBackground); |
mapellil | 7:d79cbeda2982 | 63 | // Check current distance vs background distance |
mapellil | 7:d79cbeda2982 | 64 | if (range_mm < background_threshold){ |
mapellil | 7:d79cbeda2982 | 65 | // big variation detected => hand entering in FoV |
mapellil | 7:d79cbeda2982 | 66 | data->gestureCode = GESTURES_HAND_ENTERING; |
mapellil | 7:d79cbeda2982 | 67 | RB_init(&(data->rangeList),TAP_1_BUFFER_SIZE); |
mapellil | 7:d79cbeda2982 | 68 | RB_push(&(data->rangeList), range_mm); |
mapellil | 7:d79cbeda2982 | 69 | data->timestamp = time; |
mapellil | 7:d79cbeda2982 | 70 | TOF_GESTURES_DEBUG(TAP_1,"(background_thd=%d, current=%d, hand_entering=1)", background_threshold, range_mm); |
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 | TOF_GESTURES_DEBUG(TAP_1,"(background_thd=%d, current=%d, hand_entering=0)", background_threshold, range_mm); |
mapellil | 7:d79cbeda2982 | 75 | } |
mapellil | 7:d79cbeda2982 | 76 | |
mapellil | 7:d79cbeda2982 | 77 | |
mapellil | 7:d79cbeda2982 | 78 | // Hand entering |
mapellil | 7:d79cbeda2982 | 79 | } else if (data->gestureCode == GESTURES_HAND_ENTERING){ |
mapellil | 7:d79cbeda2982 | 80 | // Push the range value in the buffer |
mapellil | 7:d79cbeda2982 | 81 | RB_push(&(data->rangeList), range_mm); |
mapellil | 7:d79cbeda2982 | 82 | |
mapellil | 7:d79cbeda2982 | 83 | // Try to detect a Tap gesture as soon as possible (4 data are enough for normal/fast gestures speed) |
mapellil | 7:d79cbeda2982 | 84 | if(data->rangeList.count == data->nbOfDataToDecide){ |
mapellil | 7:d79cbeda2982 | 85 | // Calculate MAD |
mapellil | 7:d79cbeda2982 | 86 | madRange = RB_mad(&(data->rangeList)); |
mapellil | 7:d79cbeda2982 | 87 | dir = RB_dir(&(data->rangeList)); |
mapellil | 7:d79cbeda2982 | 88 | delta_time = time - data->timestampLastDetectedTap; |
mapellil | 7:d79cbeda2982 | 89 | // Try to detect a TAP |
mapellil | 7:d79cbeda2982 | 90 | // MAD is higher than a threshold, ranging elements stored in the buffer are decreasing and last TAP has been |
mapellil | 7:d79cbeda2982 | 91 | // detected more than 250 ms ago (too avoid 2 TAPS being reported in case of a slow tap) |
mapellil | 7:d79cbeda2982 | 92 | if((madRange > 12) && (dir<0) && (delta_time>250)){ |
mapellil | 7:d79cbeda2982 | 93 | // Big negative range variation |
mapellil | 7:d79cbeda2982 | 94 | if ((data->meanBackground > data->MinBackGroundDistance)){ |
mapellil | 7:d79cbeda2982 | 95 | // Background estimated just before hand entering is above the min distance of possible background => Skip TAP as this is a false one |
mapellil | 7:d79cbeda2982 | 96 | // Relax constraints to detect the next TAP which can come very soon |
mapellil | 7:d79cbeda2982 | 97 | RB_init(&(data->rangeList),16); |
mapellil | 7:d79cbeda2982 | 98 | data->meanBackground = range_mm; |
mapellil | 7:d79cbeda2982 | 99 | data->nbOfDataToDecide = 3; |
mapellil | 7:d79cbeda2982 | 100 | }else{ |
mapellil | 7:d79cbeda2982 | 101 | // Estimated background is now in the gesture area so this was the hand => TAP |
mapellil | 7:d79cbeda2982 | 102 | data->timestampLastDetectedTap = time; |
mapellil | 7:d79cbeda2982 | 103 | //RB_trace(&(data->rangeList)); |
mapellil | 7:d79cbeda2982 | 104 | RB_init(&(data->rangeList),16); |
mapellil | 7:d79cbeda2982 | 105 | data->nbOfDataToDecide = 4; |
mapellil | 7:d79cbeda2982 | 106 | data->gestureCode = GESTURES_SINGLE_TAP; |
mapellil | 7:d79cbeda2982 | 107 | //TOF_GESTURES_DEBUG(TAP_1,"(madRange=%d, delta_time=%d, dir=%d) => TAP", madRange, delta_time, dir); |
mapellil | 7:d79cbeda2982 | 108 | } |
mapellil | 7:d79cbeda2982 | 109 | }else{ |
mapellil | 7:d79cbeda2982 | 110 | // No TAP detected |
mapellil | 7:d79cbeda2982 | 111 | RB_init(&(data->rangeList),16); |
mapellil | 7:d79cbeda2982 | 112 | data->nbOfDataToDecide = 4; |
mapellil | 7:d79cbeda2982 | 113 | data->gestureCode = GESTURES_NULL; |
mapellil | 7:d79cbeda2982 | 114 | //TOF_GESTURES_DEBUG(TAP_1,"(madRange=%d, delta_time=%d, dir=%d) => no TAP", madRange, delta_time, dir); |
mapellil | 7:d79cbeda2982 | 115 | } |
mapellil | 7:d79cbeda2982 | 116 | |
mapellil | 7:d79cbeda2982 | 117 | TOF_GESTURES_DEBUG(TAP_1,"(hand_entering=1, current=%d, decide=1, madRange=%d, dir=%d, delta_time=%d, meanBackground=%d, tap=%d)", range_mm, madRange, dir, delta_time, data->meanBackground, (int)(data->gestureCode == GESTURES_SINGLE_TAP)); |
mapellil | 7:d79cbeda2982 | 118 | |
mapellil | 7:d79cbeda2982 | 119 | } else { |
mapellil | 7:d79cbeda2982 | 120 | // Wait for more data before deciding |
mapellil | 7:d79cbeda2982 | 121 | TOF_GESTURES_DEBUG(TAP_1,"(hand_entering=1, current=%d) wait", range_mm); |
mapellil | 7:d79cbeda2982 | 122 | } |
mapellil | 7:d79cbeda2982 | 123 | // Tap detected |
mapellil | 7:d79cbeda2982 | 124 | } else { |
mapellil | 7:d79cbeda2982 | 125 | // Reset gesture code state machine |
mapellil | 7:d79cbeda2982 | 126 | data->gestureCode = GESTURES_NULL; |
mapellil | 7:d79cbeda2982 | 127 | } |
mapellil | 7:d79cbeda2982 | 128 | |
mapellil | 7:d79cbeda2982 | 129 | return data->gestureCode; |
mapellil | 7:d79cbeda2982 | 130 | } |
mapellil | 7:d79cbeda2982 | 131 | |
mapellil | 7:d79cbeda2982 | 132 |