Diff: Middlewares/ST/STM32_HMI_HandGesture/HmiBBGesture.cpp

Revision:

14:946e62f44f4f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Middlewares/ST/STM32_HMI_HandGesture/HmiBBGesture.cpp	Tue Oct 27 15:54:05 2015 +0000
@@ -0,0 +1,296 @@
+/* ----------------------------------------------------------------------
+ * Copyright (C) 2014 STMicroelectronics. All rights reserved.
+ *
+ * Project: BB gesture recognition
+ * Title:   HmiBBGesture
+ *
+ * Description: Gesture recognition - left, right, tap
+ *
+ * 20/10/14
+ * Changed from proximity to range
+ * Ultra simplified version
+ *
+ * -------------------------------------------------------------------- */
+
+//#define PRINT_DEBUG_
+#define USE_MICRO_
+
+#include "HmiBBGesture.h"
+#include "stm32f4xx_hal.h"
+#include <stdint.h>
+#include <stdio.h>
+
+#ifdef USE_MICRO_
+//#include "vl6180x_shield.h"
+//#include "platform.h"
+//#define GET_TIME_STAMP() ((int)timer_get_clock_time_msecs())  // for Mbed
+#define GET_TIME_STAMP()  (int32_t)HAL_GetTick()   // for Cube
+#else
+/* PC config */
+#include <windows.h>
+#define GET_TIME_STAMP() ((int)timeGetTime())
+#endif
+
+#ifdef PRINT_DEBUG_
+#ifdef USE_MICRO_
+static char str[80];
+//extern void serial_print(const char *str);
+#define serial_print(str)  printf(str)
+#endif
+#endif
+
+#define HMI_SIGNAL_CODE_DROP_DOWN 1
+#define HMI_SIGNAL_CODE_RAISE_UP 2
+#define HMI_SIGNAL_CODE_DOWN_STATE 3
+#define HMI_SIGNAL_CODE_UP_STATE 4
+#define HMI_SIGNAL_CODE_NULL -1
+
+#define HMI_TAP_CODE_SINGLE 0
+#define HMI_TAP_CODE_DOUBLE 1
+#define HMI_TAP_CODE_HOLD 2
+#define HMI_TAP_CODE_NULL -1
+
+
+
+
+/**********************************************************************************
+ *
+ *
+ *  Gesture recognition class
+ *
+ *
+ **********************************************************************************/
+
+
+void HmiBBGesture::Init(int threshold_mm, int tap_threshold_mm, int min_hold_duration_ms, int min_swipe_duration_ms, int max_gesture_duration_ms )
+{
+    m_motion_r.Init( threshold_mm );
+    m_motion_l.Init( threshold_mm );
+    m_tap_det.Init( tap_threshold_mm, 150, min_hold_duration_ms, false, 0 );
+    m_fsm_state = 0;
+    m_timestamp = GET_TIME_STAMP();
+    m_min_swipe_ms   = min_swipe_duration_ms;
+    m_max_gesture_ms = max_gesture_duration_ms;
+    m_gesture_starts_from_right = false;
+}
+
+int HmiBBGesture::Update(int range_r, int range_l, int *gesture_duration_ms)
+{
+    int r_code, r_ms;
+    int l_code, l_ms;
+    int duration;
+    int gesture_code = HMI_BB_GESTURE_CODE_NULL;
+    *gesture_duration_ms = 0;
+
+    r_code = m_motion_r.Update( range_r , &r_ms );
+    l_code = m_motion_l.Update( range_l , &l_ms );
+//        printf ("r_ms: %d l_ms: %d\n\r",r_ms, l_ms);
+    switch( m_fsm_state )
+    {
+    case 1: // gesture ends
+
+        duration = GET_TIME_STAMP() - m_timestamp;
+        if(duration > m_max_gesture_ms)  // gesture is too long - discard it
+        {
+            m_fsm_state = 0;
+#ifdef PRINT_DEBUG_
+
+#ifdef USE_MICRO_
+sprintf(str, "FSM 1 - Discarded %d %d %d mts %d\n",r_code,l_code,duration, m_timestamp);
+serial_print(str);
+#else
+printf("FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
+#endif
+
+#endif
+        }
+        else if( (m_gesture_starts_from_right && l_code == HMI_SIGNAL_CODE_RAISE_UP ) ||
+                 (m_gesture_starts_from_right==false && r_code == HMI_SIGNAL_CODE_RAISE_UP) )
+        {
+            m_fsm_state = 0;
+            if( duration > (m_min_swipe_ms/2) )
+            {
+                gesture_code = ( m_gesture_starts_from_right ) ? HMI_BB_GESTURE_CODE_RIGHT : HMI_BB_GESTURE_CODE_LEFT;
+                *gesture_duration_ms = duration;
+#ifdef PRINT_DEBUG_
+
+#ifdef USE_MICRO_
+sprintf(str,"FSM 2 - Recognized %d\n",gesture_code);
+serial_print(str);
+#else
+printf("FSM 2 - Recognized %d\n",gesture_code);
+#endif
+
+#endif
+            }
+#ifdef PRINT_DEBUG_
+            else
+            {
+#ifdef USE_MICRO_
+sprintf(str,"FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
+serial_print(str);
+#else
+printf("FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
+#endif
+            }
+#endif
+        }
+
+        break;
+
+    case 0: // gesture starts
+        if( l_code == HMI_SIGNAL_CODE_DOWN_STATE && r_code == HMI_SIGNAL_CODE_RAISE_UP && r_ms > m_min_swipe_ms )
+        {
+            m_gesture_starts_from_right = true;
+            m_fsm_state = 1;
+            m_timestamp = GET_TIME_STAMP();
+            m_var_max_gesture_ms = r_ms;
+#ifdef PRINT_DEBUG_
+
+#ifdef USE_MICRO_
+sprintf(str,"FSM 0 - Right started %d\n",r_ms);
+serial_print(str);
+#else
+printf("FSM 0 - Right started %d\n",r_ms);
+#endif
+
+#endif
+        }
+        else if( l_code == HMI_SIGNAL_CODE_RAISE_UP && r_code == HMI_SIGNAL_CODE_DOWN_STATE && l_ms > m_min_swipe_ms )
+        {
+            m_gesture_starts_from_right = false;
+            m_fsm_state = 1;
+            m_timestamp = GET_TIME_STAMP();
+            m_var_max_gesture_ms = l_ms;
+#ifdef PRINT_DEBUG_
+
+#ifdef USE_MICRO_
+sprintf(str,"FSM 0 - Left started %d\n",l_ms);
+serial_print(str);
+#else
+printf("FSM 0 - Left started %d\n",l_ms);
+#endif
+
+#endif
+        }
+        break;
+
+    default:
+        break;
+    };
+
+    if( m_tap_det.Update((range_r+range_l)/2,&duration) == HMI_TAP_CODE_HOLD )
+    {
+        gesture_code = HMI_BB_GESTURE_CODE_TAP;
+        *gesture_duration_ms = duration;
+        m_fsm_state = 0;
+    }
+
+    return gesture_code;
+}
+
+
+/**********************************************************************************
+ *
+ *
+ *  MotionDetector class
+ *
+ *
+ **********************************************************************************/
+
+void MotionDetector::Init(int threshold)
+{
+    m_threshold = threshold;
+    m_runonce = true;
+}
+
+
+int MotionDetector::Update(int sample,int *duration)
+{
+    int return_code;
+    bool belowCurrent, isContinuous;
+
+
+    // run once
+    if( m_runonce )
+    {
+        m_runonce = false;
+        m_timestamp = GET_TIME_STAMP();
+        m_prev_sample = sample;
+        belowCurrent  = (sample < m_threshold);
+        isContinuous  = true;
+        return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DOWN_STATE : HMI_SIGNAL_CODE_UP_STATE;
+        *duration = 1;
+        return return_code;
+    }
+
+    *duration = 0;
+    return_code = HMI_SIGNAL_CODE_NULL;
+    belowCurrent  = (sample < m_threshold);
+    isContinuous  = ( belowCurrent ==  (m_prev_sample < m_threshold));
+    m_prev_sample = sample;
+
+    // update
+    *duration = GET_TIME_STAMP() - m_timestamp;
+    if( isContinuous )
+    {
+        return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DOWN_STATE : HMI_SIGNAL_CODE_UP_STATE;
+    }
+    else
+    {
+        return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DROP_DOWN : HMI_SIGNAL_CODE_RAISE_UP;
+        m_timestamp = GET_TIME_STAMP();
+    }
+
+    return return_code;
+}
+
+
+
+/**********************************************************************************
+ *
+ *
+ *  TapDetector
+ *
+ *
+ **********************************************************************************/
+void TapDetector::Init( int threshold, int min_down_duration_ms, int min_hold_duration_ms, bool enable_double_tap, int double_tap_min_duration_ms)
+{
+    m_threshold = threshold;
+    m_motion.Init( threshold );
+    m_min_down_duration_ms = min_down_duration_ms;
+    m_min_hold_duration_ms = min_hold_duration_ms;
+    m_min_dtap_duration_ms = double_tap_min_duration_ms;
+    m_notify_hold = true;
+    m_notify_dtap = enable_double_tap;
+    m_timestamp = GET_TIME_STAMP();
+}
+
+int TapDetector::Update( int sample, int *duration )
+{
+    int return_code;
+    int motion_code,motion_duration;
+
+    return_code = HMI_TAP_CODE_NULL;
+    *duration = 0;
+    motion_code = m_motion.Update(sample, &motion_duration);
+
+    // hold condition - down_duration > hold_time
+    if( motion_code == HMI_SIGNAL_CODE_DOWN_STATE && motion_duration > m_min_hold_duration_ms )
+    {
+        if( m_notify_hold )
+        {
+            return_code = HMI_TAP_CODE_HOLD;
+            m_notify_hold = false; // to avoid continue notification ( anti flood )
+            *duration = motion_duration;
+        }
+    }
+    else if( motion_code == HMI_SIGNAL_CODE_RAISE_UP )
+    {
+        m_notify_hold = true;
+        /* TODO add TAP single/double code */
+    }
+
+    return return_code;
+}
+