Mercurial > public > ostc4

changeset 367:e309f78f89a5 MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merge default
author Ideenmodellierer
date Sat, 20 Jul 2019 21:42:45 +0200
parents 3c7030d6d67a (diff) be1f74d5b3cb (current diff)
children 50ea68c7a153
files Current build/OSTC4update_190506.bin Discovery/Src/base.c Discovery/Src/t7.c Small_CPU/Inc/compass_LSM303DLHC.h
diffstat 8 files changed, 281 insertions(+), 66 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/base.h	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Inc/base.h	Sat Jul 20 21:42:45 2019 +0200
@@ -86,6 +86,8 @@
 	ACTION_BUTTON_NEXT,
 	ACTION_BUTTON_ENTER,
 	ACTION_BUTTON_ENTER_FINAL,
+	ACTION_SHAKE_POS,
+	ACTION_SHAKE_NEG,
 	ACTION_END
 } SAction;
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Discovery/Inc/motion.h	Sat Jul 20 21:42:45 2019 +0200
@@ -0,0 +1,29 @@
+/*
+ * motion.h
+ *
+ *  Created on: 20.05.2019
+ *      Author: Thorsten Sonntag
+ */
+
+#ifndef INC_MOTION_H_
+#define INC_MOTION_H_
+
+
+/* exported data types */
+typedef enum
+{
+		DETECT_START = 0,
+		DETECT_POS_MOVE,
+		DETECT_MAXIMA,
+		DETECT_FALLBACK,
+		DETECT_POS_SHAKE,
+		DETECT_NEG_MOVE,
+		DETECT_MINIMA,
+		DETECT_RISEBACK,
+		DETECT_NEG_SHAKE,
+		DETECT_NOTHING
+} detectionState_t;
+
+detectionState_t detectShake(float currentPitch);
+
+#endif /* INC_MOTION_H_ */
--- a/Discovery/Inc/t7.h	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Inc/t7.h	Sat Jul 20 21:42:45 2019 +0200
@@ -40,7 +40,7 @@
 void t7_refresh_customview_old(void);
 
 void t7_change_field(void);
-void t7_change_customview(void);
+void t7_change_customview(uint8_t action);
 
 void t7_set_field_to_primary(void);
 void t7_set_customview_to_primary(void);
--- a/Discovery/Inc/tHome.h	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Inc/tHome.h	Sat Jul 20 21:42:45 2019 +0200
@@ -116,7 +116,7 @@
 void tHome_sleepmode_fun(void);
 void set_globalState_tHome(void);
 void tHome_change_field_button_pressed(void);
-void tHome_change_customview_button_pressed(void);
+void tHome_change_customview_button_pressed(uint8_t action);
 
 void tHome_findNextStop(const uint16_t *list, uint8_t *depthOut, uint16_t *lengthOut);
 void tHomeDiveMenuControl(uint8_t sendAction);
--- a/Discovery/Src/base.c	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Src/base.c	Sat Jul 20 21:42:45 2019 +0200
@@ -229,12 +229,14 @@
 #include "logbook_miniLive.h"
 #include "test_vpm.h"
 #include "tDebug.h"
+#include "motion.h"
 
 #ifdef DEMOMODE
 #include "demo.h"
 static void TIM_DEMO_init(void);
 #endif
 
+
 //#include "lodepng.h"
 //#include <stdlib.h> // for malloc and free
 
@@ -325,6 +327,14 @@
 #define MEASURECNT 60	/* number of measuremets to be stored */
 static uint32_t loopcnt[MEASURECNT];
 #endif
+
+static uint8_t ButtonAction = ACTION_END;
+
+static void StoreButtonAction(uint8_t action)
+{
+	ButtonAction = action;
+}
+
 //  ===============================================================================
 //	main
 /// @brief	This function makes initializations and has the nonIRQ endless loop
@@ -341,6 +351,7 @@
     uint8_t lastsecond = 0xFF;
 #endif
 
+    detectionState_t shakestate;
     set_globalState( StBoot0 );
     LastButtonPressed = 0;
 
@@ -476,6 +487,16 @@
 	        DoDisplayRefresh = 0;
         	RefreshDisplay();
 
+        	shakestate = detectShake(stateRealGetPointer()->lifeData.compass_pitch);
+            if(DETECT_NEG_SHAKE == shakestate)
+           	{
+            	StoreButtonAction((uint8_t)ACTION_SHAKE_NEG);
+           	}
+            if(DETECT_POS_SHAKE == shakestate)
+           	{
+            	StoreButtonAction((uint8_t)ACTION_SHAKE_POS);
+           	}
+
 // Enable this to make the simulator write a logbook entry
 // #define SIM_WRITES_LOGBOOK 1
 
@@ -510,6 +531,9 @@
     }
 }
 
+
+
+
 //  ===============================================================================
 //	timer IRQ
 /// @brief	this is called periodically
@@ -817,12 +841,7 @@
 		break;
 	}
 }
-static uint8_t ButtonAction = ACTION_END;
 
-static void StoreButtonAction(uint8_t action)
-{
-	ButtonAction = action;
-}
 
 static void TriggerButtonAction()
 {
@@ -866,14 +885,16 @@
 					set_globalState(StD);
 				} else
 					tHome_change_field_button_pressed();
-			} else if (action == ACTION_BUTTON_ENTER) {
-				if ((status.page == PageDive) && (status.line == 0))
-					tHome_change_customview_button_pressed();
-				else if (status.page == PageSurface)
-					tHome_change_customview_button_pressed();
-				else
-					tHomeDiveMenuControl(action);
-			}
+			} else if ((action == ACTION_BUTTON_ENTER) || (action == ACTION_SHAKE_NEG) || (action == ACTION_SHAKE_POS))
+					{
+
+						if ((status.page == PageDive) && (status.line == 0))
+							tHome_change_customview_button_pressed(action);
+						else if (status.page == PageSurface)
+							tHome_change_customview_button_pressed(action);
+						else
+							tHomeDiveMenuControl(action);
+					}
 			break;
 
 		case BaseMenu:
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Discovery/Src/motion.c	Sat Jul 20 21:42:45 2019 +0200
@@ -0,0 +1,132 @@
+/*
+ * motion.c
+ *
+ *  Created on: 20.05.2019
+ *      Author: Thorsten Sonntag
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include "motion.h"
+
+#define	STABLE_STATE_COUNT			2	/* number of count to declare a state as stable (at the moment based on 100ms) */
+#define STABLE_STATE_TIMEOUT		5	/* Detection shall be aborted if a movement state is stable for more than 500ms */
+#define MOVE_DELTA_SPEED			4	/* Delta speed needed to identify a valid movement */
+#define SHAKE_DELTA_COUNT			10	/* Delta count needed to identify a valid minima / maxima */
+#define SHAKE_DELTA_END				10	/* Delta allowed between start and end position */
+
+static detectionState_t detectionState = DETECT_NOTHING;
+
+/* Detect if user is generating an pitch including return to starting position (shake) */
+/* This is done by feeding the past movements value per value into a state machine */
+detectionState_t detectShake(float currentPitch)
+{
+	static uint8_t stableCnt = 0;
+	static float lastPitch = 0.0;
+	static float startPitch = 0.0;
+	static float minmax = 0.0;
+	float curSpeed;
+
+	if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE))	/* discard last detection */
+	{
+		detectionState = DETECT_NOTHING;
+	}
+
+	curSpeed = currentPitch - lastPitch;
+
+	/* feed value into state machine */
+	switch (detectionState)
+	{
+			case DETECT_NOTHING: 	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)	/* detect a stable condition before evaluating for the next move */
+									{
+										stableCnt++;
+									}
+									if(stableCnt > STABLE_STATE_COUNT)
+									{
+										detectionState = DETECT_START;
+										stableCnt = 0;
+									}
+				break;
+			case DETECT_START:		if(fabsf(curSpeed) > MOVE_DELTA_SPEED)
+									{
+										if(curSpeed > 0)
+										{
+											detectionState = DETECT_POS_MOVE;
+										}
+										else
+										{
+											detectionState = DETECT_NEG_MOVE;
+										}
+										stableCnt = 0;
+										startPitch = lastPitch;
+									}
+				break;
+			case DETECT_NEG_MOVE:
+			case DETECT_POS_MOVE:	if(fabsf(curSpeed) > MOVE_DELTA_SPEED )
+									{
+										stableCnt++;
+									}
+									else
+									{
+										if(stableCnt >= STABLE_STATE_COUNT)	/* debounce movement */
+										{
+											if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT)
+											{
+												detectionState++;
+												minmax = lastPitch;
+											}
+											else
+											{
+												detectionState = DETECT_NOTHING;
+											}
+										}
+										else
+										{
+											detectionState = DETECT_NOTHING;
+										}
+										stableCnt = 0;
+									}
+				break;
+			case DETECT_MINIMA:
+			case DETECT_MAXIMA:		if(fabsf(currentPitch - minmax ) < SHAKE_DELTA_COUNT)		/* stay at maximum for short time to add a pattern for user interaction */
+									{
+										stableCnt++;
+									}
+									else
+									{
+										if(stableCnt > 0)
+										{
+											detectionState++;
+										}
+										else
+										{
+											detectionState = DETECT_NOTHING;
+										}
+										stableCnt = 0;
+									}
+				break;
+			case DETECT_RISEBACK:
+			case DETECT_FALLBACK:	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
+									{
+										if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END)
+										{
+											detectionState++;
+										}
+									}
+									stableCnt++;
+				break;
+			default:
+				detectionState = DETECT_NOTHING;
+				break;
+
+	}
+	if(stableCnt > STABLE_STATE_TIMEOUT)
+	{
+		detectionState = DETECT_NOTHING;
+		stableCnt = 0;
+	}
+
+	lastPitch = currentPitch;
+	return detectionState;
+}
--- a/Discovery/Src/t7.c	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Src/t7.c	Sat Jul 20 21:42:45 2019 +0200
@@ -1479,9 +1479,10 @@
             selection_customview = settingsGetPointer()->tX_customViewPrimary;
 }
 
-void t7_change_customview(void)
+void t7_change_customview(uint8_t action)
 {
-    const uint8_t *pViews;
+    uint8_t *pViews;
+    uint8_t *pStartView,*pCurView, *pLastView;
     _Bool cv_disabled = 0;
 
     if(stateUsed->mode == MODE_DIVE)
@@ -1489,54 +1490,83 @@
     else
         pViews = customviewsSurface;
 
-    while((*pViews != CVIEW_END) && (*pViews != selection_customview))
-        {pViews++;}
-
-    if(*pViews < CVIEW_END)
-        pViews++;
-
-
-    if(*pViews == CVIEW_END)
+    pStartView = pViews;
+    /* set pointer to currently selected view and count number of entries */
+    while((*pViews != CVIEW_END))
+    {
+    	if (*pViews == selection_customview)
+    	{
+    		pCurView = pViews;
+    	}
+    	pViews++;
+    }
+    pLastView = pViews;
+    pViews = pCurView;
+
+    if((action == ACTION_BUTTON_ENTER) || (action == ACTION_SHAKE_POS))
     {
-        if(stateUsed->mode == MODE_DIVE)
-            pViews = customviewsDive;
-        else
-            pViews = customviewsSurface;
+		if(*pViews < CVIEW_END)
+			pViews++;
+
+		if(*pViews == CVIEW_END)
+		{
+			pViews = pStartView;
+		}
     }
-
-    if(stateUsed->mode == MODE_DIVE)
+    else
     {
-        do
+		if(pViews == pStartView)
+		{
+			pViews = pLastView - 1;
+		}
+		else
+		{
+			pViews--;
+		}
+    }
+
+    do
+    {
+        cv_disabled = 0;
+        for(int i=0;i<6;i++)
         {
-            cv_disabled = 0;
-            for(int i=0;i<6;i++)
-            {
-                if((*pViews == cv_changelist[i]) && !CHECK_BIT_THOME(settingsGetPointer()->cv_configuration, cv_changelist[i]))
-                {
-                    cv_disabled = 1;
-                    break;
-                }
-            }
-
-            if ((*pViews == CVIEW_sensors || *pViews == CVIEW_sensors_mV) &&
-            	stateUsed->diveSettings.ppo2sensors_deactivated)
-            {
-            	cv_disabled = 1;
-            }
-
-            if(cv_disabled)
-            {
-                if(*pViews < CVIEW_END)
-                {
-                    pViews++;
-                }
-                else
-                {
-                    pViews = customviewsDive;
-                }
-            }
-        } while(cv_disabled);
-    }
+             if((*pViews == cv_changelist[i]) && !CHECK_BIT_THOME(settingsGetPointer()->cv_configuration, cv_changelist[i]))
+             {
+            	 cv_disabled = 1;
+                   break;
+             }
+        }
+
+        if (((*pViews == CVIEW_sensors) || (*pViews == CVIEW_sensors_mV)) &&
+           	((stateUsed->diveSettings.ppo2sensors_deactivated) || (stateUsed->diveSettings.ccrOption == 0)))
+        {
+	      	cv_disabled = 1;
+        }
+
+        if(cv_disabled)		/* view is disabled => jump to next view */
+        {
+          	if((action == ACTION_BUTTON_ENTER) || (action == ACTION_SHAKE_POS))
+          	{
+           		pViews++;
+				if(*pViews == CVIEW_END)
+				{
+					pViews = pStartView;
+				}
+           	}
+           	else
+           	{
+           		if(pViews == pStartView)
+           		{
+           			pViews = pLastView - 1;
+           		}
+           		else
+           		{
+           			pViews--;
+           		}
+           	}
+        }
+    } while(cv_disabled);
+
     selection_customview = *pViews;
 }
 
@@ -1570,11 +1600,11 @@
 	pSettings = settingsGetPointer();
 
     if((selection_customview == CVIEW_sensors) &&(stateUsed->diveSettings.ccrOption == 0))
-        t7_change_customview();
+        t7_change_customview(ACTION_BUTTON_ENTER);
     if((selection_customview == CVIEW_sensors_mV) &&(stateUsed->diveSettings.ccrOption == 0))
-        t7_change_customview();
+        t7_change_customview(ACTION_BUTTON_ENTER);
     if((selection_customview == CVIEW_sensors) &&(stateUsed->diveSettings.ccrOption == 0))
-        t7_change_customview();
+        t7_change_customview(ACTION_BUTTON_ENTER);
 
     switch(selection_customview)
     {
--- a/Discovery/Src/tHome.c	Sat Jul 20 10:44:01 2019 +0200
+++ b/Discovery/Src/tHome.c	Sat Jul 20 21:42:45 2019 +0200
@@ -42,6 +42,7 @@
 #include "tMenuEditGasOC.h" // for openEdit_DiveSelectBetterGas()
 #include "tMenuEditSetpoint.h" // for openEdit_DiveSelectBetterSetpoint()
 #include "simulation.h"
+#include "motion.h"
 
 /* Private types -------------------------------------------------------------*/
 
@@ -330,11 +331,11 @@
 }
 
 
-void tHome_change_customview_button_pressed(void)
+void tHome_change_customview_button_pressed(uint8_t action)
 {
     tHome_tick_count_cview = 0;
     if(settingsGetPointer()->design == 7)
-        t7_change_customview();
+        t7_change_customview(action);
     else
     if(settingsGetPointer()->design == 3)
         t3_change_customview();