Revision 14:946e62f44f4f, committed 2015-10-27

Comitter:

gallonm

Date:

Tue Oct 27 15:54:05 2015 +0000

Parent:

13:ce9220e964bd

Child:

15:b94bc967fecd

Commit message:

Updated all files. Added midlewares

Changed in this revision

--- /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;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Middlewares/ST/STM32_HMI_HandGesture/HmiBBGesture.h	Tue Oct 27 15:54:05 2015 +0000
@@ -0,0 +1,86 @@
+/* ----------------------------------------------------------------------
+ * Copyright (C) 2014 STMicroelectronics. All rights reserved.
+ *
+ * Project: BB gesture recognition
+ * Title:   HmiBBGesture
+ *
+ * Description: Gesture recognition - left, right, tap
+ *
+ * -------------------------------------------------------------------- */
+
+#ifndef HMIBBGESTURE_H_
+#define HMIBBGESTURE_H_
+
+#ifdef __cplusplus      //c++
+extern "C"  
+{       
+#endif  
+
+#define HMI_BB_GESTURE_CODE_TAP 0
+#define HMI_BB_GESTURE_CODE_LEFT 1
+#define HMI_BB_GESTURE_CODE_RIGHT 2
+#define HMI_BB_GESTURE_CODE_NULL -1
+
+
+class MotionDetector
+{
+public:
+    MotionDetector() {};
+    ~MotionDetector() {};
+
+    void Init(int threshold);
+    int Update(int sample,int *duration);
+
+protected:
+    int m_prev_sample;
+    int m_threshold;
+    int m_timestamp;
+    bool m_runonce;
+};
+
+
+class TapDetector
+{
+public:
+    TapDetector() {};
+    ~TapDetector() {};
+    void Init( int threshold, int min_down_duration_ms, int min_hold_duration_ms, bool enable_double_tap, int double_tap_min_duration_ms);
+    int Update( int sample, int *duration );
+
+protected:
+    MotionDetector m_motion;
+    int m_threshold;
+    int m_min_down_duration_ms;
+    int m_min_hold_duration_ms;
+    int m_min_dtap_duration_ms;
+    int m_timestamp;
+    bool m_notify_hold;
+    bool m_notify_dtap;
+};
+
+
+class HmiBBGesture
+{
+public:
+    HmiBBGesture() {};
+    ~HmiBBGesture() {};
+
+    void Init(int threshold_mm, int tap_threshold_mm, int min_hold_duration_ms, int min_swipe_duration_ms, int max_gesture_duration_ms );
+    int Update(int range_r, int range_l,int *gesture_duration_ms);
+
+protected:
+    MotionDetector m_motion_r;
+    MotionDetector m_motion_l;
+    TapDetector m_tap_det;
+    int m_timestamp;
+    int m_fsm_state;
+    int m_min_swipe_ms;
+    int m_max_gesture_ms;
+    int m_var_max_gesture_ms;
+    bool m_gesture_starts_from_right;
+
+};
+#ifdef __cplusplus  //c++
+}                   
+#endif  
+#endif /* HMIBBGESTURE_H_ */

--- a/X_NUCLEO_6180XA1.lib	Tue Oct 27 15:30:25 2015 +0000
+++ b/X_NUCLEO_6180XA1.lib	Tue Oct 27 15:54:05 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/gallonm/code/X_NUCLEO_6180XA1/#454710d17358
+https://developer.mbed.org/users/gallonm/code/X_NUCLEO_6180XA1/#c98aa73dc4a5

--- a/main.cpp	Tue Oct 27 15:30:25 2015 +0000
+++ b/main.cpp	Tue Oct 27 15:54:05 2015 +0000
@@ -1,11 +1,14 @@
 #include "mbed.h"
 #include "x_nucleo_6180xa1.h"
-
+#include "HmiBBGesture.h"
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <assert.h>
 
+/* Polling operating modes don`t require callback function that handles IRQ 
+   Callback functions are used only for measure that require interrupt */
+
 #define VL6180X_I2C_SDA I2C_SDA
 #define VL6180X_I2C_SCL I2C_SCL
 
@@ -14,7 +17,7 @@
 //#define RANGE_CONTINUOUS_POLLING
 //#define ALS_CONTINUOUS_POLLING
 //#define RANGE_CONTINUOUS_INTERRUPT
-#define ALS_CONTINUOUS_INTERRUPT
+//#define ALS_CONTINUOUS_INTERRUPT
 //#define INTERLEAVED_MODE_INTERRUPT
 //#define RANGE_CONTINUOUS_POLLING_LOW_THRESHOLD
 //#define RANGE_CONTINUOUS_POLLING_HIGH_THRESHOLD
@@ -28,297 +31,306 @@
 //#define ALS_CONTINUOUS_INTERRUPT_LOW_THRESHOLD
 //#define ALS_CONTINUOUS_INTERRUPT_HIGH_THRESHOLD
 //#define ALS_CONTINUOUS_INTERRUPT_OUT_OF_WINDOW
+#define HAND_GESTURE
 
-/* Polling operating modes (1,2,3 and 4) don`t require callback function that handles IRQ 
-   Callback functions are used only for measure that require interrupt */
+/* timer and digital out pin used for debugging */
+//Timer timer;
+//int start, end; 
+//DigitalOut pin(PA_15); 
 
-void OnErrLog(void){}; // sistemare la funzione per stampare il messaggio che gli viene passato
-void DISP_ExecLoopBody(void){};
+/* Create a serial object */
+Serial pc(USBTX, USBRX);
 
 DevI2C device_i2c(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
 static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&device_i2c);
 
-MeasureData_t data_sensor_top, data_sensor_left, data_sensor_right;
+HmiBBGesture gesture_reco;
 
-Timer timer; // timer to debug
-int start, end;	
+MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
 	
 /* flags that handle the call to HandleIRQ function */
 bool flag_sensor_top=false, flag_sensor_bottom=false, flag_sensor_left=false, flag_sensor_right=false;	
 
 /* callback functions of the sensors */ 
-void SensorTopIRQ(void)
+static void SensorTopIRQ(void)
 {
    flag_sensor_top=true;
-	 board->sensor_top->DisableInterruptMeasureDetectionIRQ();
+   board->sensor_top->DisableInterruptMeasureDetectionIRQ();
 }	
 
-void SensorBottomIRQ(void)
+static void SensorBottomIRQ(void)
 {
    flag_sensor_bottom=true;	
-	 board->sensor_bottom->DisableInterruptMeasureDetectionIRQ();
+   board->sensor_bottom->DisableInterruptMeasureDetectionIRQ();
 }	
 
-void SensorLeftIRQ(void)
+static void SensorLeftIRQ(void)
 {
    flag_sensor_left=true;	
-	 board->sensor_left->DisableInterruptMeasureDetectionIRQ();
+   board->sensor_left->DisableInterruptMeasureDetectionIRQ();
 }
 
-void SensorRightIRQ(void)
+static void SensorRightIRQ(void)
 {
    flag_sensor_right=true;	
-	 board->sensor_right->DisableInterruptMeasureDetectionIRQ();
+   board->sensor_right->DisableInterruptMeasureDetectionIRQ();
 }
 
+void OnErrLog(void){}; // FIXME sistemare la funzione per stampare il messaggio che gli viene passato
+                       // posso ridirigere la OnErrLog come pc.printf(__VA_ARGS__), ma deve essere OnErrLog(...)
+void DISP_ExecLoopBody(void){}; //FIXME sistemare questa funzione
+
 int main()
 {   
-	 int status, status_t, status_b, status_l, status_r;
-	
+   int status, status_t, status_b, status_l, status_r;
+   
+   //device_i2c.frequency(300000); //change i2c frequncy from 100kHz to 400kHz
+   pc.baud(115200); //change baudrate of the printf
+   
    status=board->InitBoard();
-	 if(status)
-		  printf("Failed to init the board!\n\r");
+   if(status)
+      VL6180x_ErrLog("Failed to init the board!\n\r");
 
 #ifdef RANGE_SINGLE_SHOT_POLLING	 
-	 status_t=board->sensor_top->StartMeasurement(range_single_shot_polling, NULL, &data_sensor_top, NULL, NULL);
-	 if(!status_t)
-	    printf("Top Range: %dmm\n\r",data_sensor_top.range_mm);
-	 else if(status==INVALID_PARAMS)
-		  printf("Failed to start measurement!\n\r");
-	 else
-		  printf("Invalid range value!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(range_single_shot_polling);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StartMeasurement(range_single_shot_polling, NULL, &data_sensor_top, NULL, NULL);
+   if(!status_t)
+      printf("Top Range: %dmm\n\r",data_sensor_top.range_mm);
+   else if(status==INVALID_PARAMS)
+      printf("Failed to start measurement!\n\r");
+   else
+      printf("Invalid range value!\n\r");
+   status_t=board->sensor_top->StopMeasurement(range_single_shot_polling);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif
 	 
 #ifdef ALS_SINGLE_SHOT_POLLING
-	 status_t=board->sensor_top->StartMeasurement(als_single_shot_polling, NULL, &data_sensor_top, NULL, NULL);
-	 if(!status_t)
-	    printf("Top Light: %dlux\n\r",data_sensor_top.lux);
-	 else if(status==INVALID_PARAMS)
-		  printf("Failed to start measurement!\n\r");
-	 else
-		  printf("Invalid light value!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(als_single_shot_polling);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StartMeasurement(als_single_shot_polling, NULL, &data_sensor_top, NULL, NULL);
+   if(!status_t)
+      printf("Top Light: %dlux\n\r",data_sensor_top.lux);
+   else if(status==INVALID_PARAMS)
+      printf("Failed to start measurement!\n\r");
+   else
+      printf("Invalid light value!\n\r");
+   status_t=board->sensor_top->StopMeasurement(als_single_shot_polling);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif
 
 #ifdef RANGE_CONTINUOUS_POLLING
-	 status_t=board->sensor_top->StartMeasurement(range_continuous_polling, NULL, &data_sensor_top, NULL, NULL);
+   status_t=board->sensor_top->StartMeasurement(range_continuous_polling, NULL, &data_sensor_top, NULL, NULL);
+   if(!status_t)
+   {
+      int i;
+      for(i=0;i<10;i++)
+      {
+         status_t=board->sensor_top->GetMeasurement(range_continuous_polling, &data_sensor_top);
 	 if(!status_t)
-	 {
-		  int i;
-		  for(i=0;i<10;i++)
-		  {
-				 status_t=board->sensor_top->GetMeasurement(range_continuous_polling, &data_sensor_top);
-	       if(!status_t)
-				    printf("Top Range measure %d: %dmm\n\r",i+1,data_sensor_top.range_mm);
-	       else
-	          printf("Invalid range value!\n\r");
-			}
+	    printf("Top Range measure %d: %dmm\n\r",i+1,data_sensor_top.range_mm);
+	 else
+	    printf("Invalid range value!\n\r");
+      }
    }
-	 else
-	    printf("Failed to start measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(range_continuous_polling);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+   else
+      printf("Failed to start measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(range_continuous_polling);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif 
 	
 #ifdef ALS_CONTINUOUS_POLLING	 
-	 status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL);
+   status_t=board->sensor_top->StartMeasurement(als_continuous_polling, NULL, &data_sensor_top, NULL, NULL);
+   if(!status_t)
+   {
+      int i;
+      for(i=0;i<10;i++)
+      {
+         status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top);
 	 if(!status_t)
-	 {
-	    int i;
-		  for(i=0;i<10;i++)
-		  {
-				 status=board->sensor_top->GetMeasurement(als_continuous_polling, &data_sensor_top);
-	       if(!status_t)
-				    printf("Top Light measure %d: %dlux\n\r",i,data_sensor_top.lux);
-	       else
-	          printf("Invalid light value!\n\r");
-			}
+	    printf("Top Light measure %d: %dlux\n\r",i,data_sensor_top.lux);
+	 else
+	    printf("Invalid light value!\n\r");
+      }
    }
-	 else
-	    printf("Failed to start measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(als_continuous_polling);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+   else
+      printf("Failed to start measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(als_continuous_polling);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif
-	 
-/*#ifdef RANGE_CONTINUOUS_INTERRUPT
-	 status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
-	 status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL);
-	 if((!status_t)&&(!status_l))
-	 {
-		  while(1)
-	    {
-	      if(flag_sensor_top)
-        {
- 		      flag_sensor_top=false;
-		      status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top);
-			    if(!status_t)
-				    printf("Top Range: %dmm\n\r",data_sensor_top.range_mm);
-			    else
-				    printf("Invalid range value!\n\r");
-	      }
-				if(flag_sensor_left)
-        {
- 		      flag_sensor_left=false;
-		      status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left);
-			    if(!status_l)
-				    printf("Left Range: %dmm\n\r",data_sensor_left.range_mm);
-			    else
-				    printf("Invalid range value!\n\r");
-	      }
-	    }
-	 }
-	 else 
-		  printf("Failed to start measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt);
-	 if(status_l)
-		  printf("Failed to stop sensor_top measurement!\n\r");
-	 status_l=board->sensor_left->StopMeasurement(range_continuous_interrupt);
-	 if(status_l)
-		  printf("Failed to stop sensor_left measurement!\n\r");
-#endif*/
 
 #ifdef RANGE_CONTINUOUS_INTERRUPT
-     status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
-     if(!status)
-     {
-        while(1)
-        {
-          if(flag_sensor_top)
-          {
-             flag_sensor_top=false;
-             status=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top);
-             if(!status)
-                printf("Range: %dmm\n\r",data_sensor_top.range_mm);
-             else
-                printf("Invalid range value!\n\r");
-          }
-        }
-     }
-     else 
-        printf("Failed to start measurement!\n\r");
-     status=board->sensor_top->StopMeasurement(range_continuous_interrupt);
-     if(status)
-          printf("Failed to stop measurement!\n\r");
+   status=board->sensor_top->StartMeasurement(range_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
+   if(!status)
+   {
+      while(1)
+      {
+         if(flag_sensor_top)
+         {
+            flag_sensor_top=false;
+            status=board->sensor_top->HandleIRQ(range_continuous_interrupt, &data_sensor_top);
+            if(!status)
+               printf("Range: %dmm\n\r",data_sensor_top.range_mm);
+            else
+               printf("Invalid range value!\n\r");
+         }
+      }
+   }
+   else 
+      printf("Failed to start measurement!\n\r");
+   status=board->sensor_top->StopMeasurement(range_continuous_interrupt); //FIXME per questo modo operativo e per i successivi ad interrupt si deve sistemare la funzione di StopMeasurement in quanto il main sta sempre dentro il ciclo while e quindi non viene mai chiamata StopMeasurement
+   if(status)
+      printf("Failed to stop measurement!\n\r");
 #endif
 
 #ifdef ALS_CONTINUOUS_INTERRUPT
-	 status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
+   status_t=board->sensor_top->StartMeasurement(als_continuous_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
    if(!status_t)
-	 {
-	    while(1)
-	    {
-	      if(flag_sensor_top)
-        {
- 		      flag_sensor_top=false;
-		      status_t=board->sensor_top->HandleIRQ(als_continuous_interrupt, &data_sensor_top);
-	        if(!status_t)
-				    printf("Top Light: %dlux\n\r",data_sensor_top.lux);
-			    else
-				    printf("Invalid light value!\n\r");
-	      }
-	    }
+   {
+      while(1)
+      {
+         if(flag_sensor_top)
+         {
+ 	    flag_sensor_top=false;
+            status_t=board->sensor_top->HandleIRQ(als_continuous_interrupt, &data_sensor_top);
+	    if(!status_t)
+	       printf("Top Light: %dlux\n\r",data_sensor_top.lux);
+	    else
+               printf("Invalid light value!\n\r");
 	 }
-	 else
-		  printf("Failed to stop measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+      }
+   }
+   else
+      printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif
 	 
 #ifdef INTERLEAVED_MODE_INTERRUPT	 
-	 status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
-	 if(!status_t)
-	 {
-	    while(1)
-	    {
-	      if(flag_sensor_top)
-        {
- 		      flag_sensor_top=false;
-		      status_t=board->sensor_top->HandleIRQ(interleaved_mode_interrupt, &data_sensor_top);
-			    if(!status_t)
-					{
-				    printf("Top Range: %dmm     ",data_sensor_top.range_mm);
-						printf("Top Light: %dlux\n\r",data_sensor_top.lux);
-					}
-			    else
-				    printf("Invalid range or light value!\n\r");
-	      }
-		 }
+   status_t=board->sensor_top->StartMeasurement(interleaved_mode_interrupt, SensorTopIRQ, &data_sensor_top, NULL, NULL);
+   if(!status_t)
+   {
+      while(1)
+      {
+         if(flag_sensor_top)
+         {
+            flag_sensor_top=false;
+            status_t=board->sensor_top->HandleIRQ(interleaved_mode_interrupt, &data_sensor_top);
+            if(!status_t)
+	       printf("Top Range: %dmm\tTop Light: %dlux\n\r",data_sensor_top.range_mm, data_sensor_top.lux);
+	    else
+	       printf("Invalid range or light value!\n\r");
 	 }
-	 else
-	    printf("Failed to stop measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(interleaved_mode_interrupt);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r"); 
+      }
+   }
+   else
+      printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(interleaved_mode_interrupt);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r"); 
 #endif
 
 #ifdef RANGE_CONTINUOUS_INTERRUPT_LOW_THRESHOLD
-	 status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL);
-	 if(!status_t)
-	 {
-	    while(1)
-	    {
-	      if(flag_sensor_top)
-        {
- 		      flag_sensor_top=false;
-		      status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt_low_threshold, &data_sensor_top);
-			    if(!status_t)
-				    printf("Range int low threshold: %dmm\n\r",data_sensor_top.range_mm);
-			    else
-				    printf("Invalid range value!\n\r");
-	      }
-	    }
-	 }
-	 else 
-		  printf("Failed to start measurement!\n\r");
-	 status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold);
-	 if(status_t)
-		  printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StartMeasurement(range_continuous_interrupt_low_threshold, SensorTopIRQ, &data_sensor_top, 80, NULL);
+   if(!status_t)
+   {
+      while(1)
+      {
+         if(flag_sensor_top)
+         {
+            flag_sensor_top=false;
+	    status_t=board->sensor_top->HandleIRQ(range_continuous_interrupt_low_threshold, &data_sensor_top);
+            if(!status_t)
+	       printf("Range int low threshold: %dmm\n\r",data_sensor_top.range_mm);
+            else
+	       printf("Invalid range value!\n\r");
+         }
+      }
+   }
+   else 
+      printf("Failed to start measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
 #endif
 
-/*
-status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL);	
-status_r=board->sensor_right->StartMeasurement(als_continuous_interrupt, SensorRightIRQ, &data_sensor_right, NULL, NULL);
-if((!status_l)&&(!status_r))
-{
-   while(1)
-	 {
-	   if(flag_sensor_left)
-     {
- 		   flag_sensor_left=false;
-		   status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left);
-			 if(!status_l)
-				 printf("Left Range: %dmm\n\r",data_sensor_left.range_mm);
-			 else
-				 printf("Invalid range value!\n\r");
-	   }
-		 if(flag_sensor_right)
-     {
- 		   flag_sensor_right=false;
-		   status_r=board->sensor_right->HandleIRQ(als_continuous_interrupt, &data_sensor_right);
-			 if(!status_r)
-				 printf("Right Light: %dlux\n\r",data_sensor_right.lux);
-			 else
-				 printf("Invalid light value!\n\r");
-		 }
-	 }
-}
-else 
-	 printf("Failed to start measurement!\n\r");
-status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold);
-if(status_t)
-	 printf("Failed to stop measurement!\n\r");
-status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt);
-if(status_t)
-	 printf("Failed to stop measurement!\n\r");
-*/ 
+/* Test application Hand Gesture */
+#ifdef HAND_GESTURE
+   int gest_code, prev_gest_code, gest_duration;
+   char str[5];
+   char valLeft[] = {'E'};
+   char valRight[] = {'3'};		 
+   char valTap[] = {'0'};	
+   status_l=board->sensor_left->StartMeasurement(range_continuous_interrupt, SensorLeftIRQ, &data_sensor_left, NULL, NULL);	
+   status_r=board->sensor_right->StartMeasurement(range_continuous_interrupt, SensorRightIRQ, &data_sensor_right, NULL, NULL);
+   gesture_reco.Init(150, 100, 200, 60, 500);
+   if((!status_l)&&(!status_r))
+   {
+      while(1)
+      {
+         if(flag_sensor_left)
+         {
+            flag_sensor_left=false;
+	    status_l=board->sensor_left->HandleIRQ(range_continuous_interrupt, &data_sensor_left);
+            //printf("Left range=%dmm\n\r",data_sensor_left.range_mm);
+         }
+         if(flag_sensor_right)
+         {
+            flag_sensor_right=false;
+            status_r=board->sensor_right->HandleIRQ(range_continuous_interrupt, &data_sensor_right);
+            //printf("Right range=%dmm\n\r",data_sensor_right.range_mm);
+         }
+         if(data_sensor_left.range_mm>400) 
+            data_sensor_left.range_mm=400;
+    	 if(data_sensor_right.range_mm>400)
+            data_sensor_right.range_mm=400;
+         gest_code=gesture_reco.Update(data_sensor_right.range_mm, data_sensor_left.range_mm, &gest_duration);
+         switch(gest_code)
+         {
+            case -1:
+              break;
+            case 0: //tap and start timer
+              if(gest_code!=prev_gest_code)
+              {
+                 prev_gest_code=gest_code;
+                 sprintf(str,"%s","0-");
+                 board->display->DisplayDigit(valTap, 1); 
+              }
+              else
+              {
+                 if(strcmp(str,"0-")==0)
+                 {
+                   sprintf(str,"-0");
+                   board->display->DisplayDigit(valTap, 2);
+                 }
+                 else
+                 {
+                   sprintf(str,"0-");
+                   board->display->DisplayDigit(valTap, 1); 
+                 }
+              }
+              break;
+            case 1: //left to right
+              board->display->DisplayDigit(valRight, 3); 
+              break;
+            case 2: //right to left
+              board->display->DisplayDigit(valLeft, 0);
+              break;
+            default:
+              printf("Errore!\n\r");
+         }
+      }
+   }
+   else 
+      printf("Failed to start measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(range_continuous_interrupt_low_threshold);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
+   status_t=board->sensor_top->StopMeasurement(als_continuous_interrupt);
+   if(status_t)
+      printf("Failed to stop measurement!\n\r");
+#endif
 
 }