Mercurial > public > ostc4

changeset 363:bdf978d2a5d4 MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Reworked detection function The first implementation was not stable enough during underwater tests => redesign taking speed deltas into account
author ideenmodellierer
date Mon, 17 Jun 2019 19:46:18 +0200
parents 7b8c87a39c0e
children 3014a698f46d
files Discovery/Inc/motion.h Discovery/Src/motion.c
diffstat 2 files changed, 74 insertions(+), 203 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/motion.h	Tue Jun 11 05:31:07 2019 +0200
+++ b/Discovery/Inc/motion.h	Mon Jun 17 19:46:18 2019 +0200
@@ -14,18 +14,16 @@
 {
 		DETECT_START = 0,
 		DETECT_POS_MOVE,
+		DETECT_MAXIMA,
+		DETECT_FALLBACK,
+		DETECT_POS_SHAKE,
 		DETECT_NEG_MOVE,
-		DETECT_MAXIMA,
 		DETECT_MINIMA,
-		DETECT_FALLBACK,
 		DETECT_RISEBACK,
-		DETECT_POS_SHAKE,
 		DETECT_NEG_SHAKE,
 		DETECT_NOTHING
 } detectionState_t;
 
-
-void InitMotion(void);
 detectionState_t detectShake(float currentPitch);
 
 #endif /* INC_MOTION_H_ */
--- a/Discovery/Src/motion.c	Tue Jun 11 05:31:07 2019 +0200
+++ b/Discovery/Src/motion.c	Mon Jun 17 19:46:18 2019 +0200
@@ -10,250 +10,123 @@
 #include <math.h>
 #include "motion.h"
 
-#define PITCH_HISTORY_ENTRIES		20	/* number of pitch value stored in buffer */
 #define	STABLE_STATE_COUNT			2	/* number of count to declare a state as stable (at the moment based on 100ms) */
-#define MOVE_DELTA_COUNT			10	/* Delta count needed to identify a valid movement */
-#define SHAKE_DELTA_COUNT			30	/* Delta count needed to identify a valid minima / maxima */
-
-
-typedef enum
-{
-		RELATIVE_MOVE_STATIC = 0,
-		RELATIVE_MOVE_INCREASE,
-		RELATIVE_MOVE_DECREASE,
-		RELATIVE_MOVE_INVALID
-} relativeMove_t;
-
-
-
-
+#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 */
 
-float pitchHistory[PITCH_HISTORY_ENTRIES];			/* Ringbuffer of last read pich values */
-static uint8_t pitchWriteIdx;			/* Index of current write slot */
-
-/* Init data structures */
-void InitMotion()
-{
-	uint8_t tmp;
-
-	for(tmp = 0; tmp < PITCH_HISTORY_ENTRIES; tmp++)
-	{
-		pitchHistory[tmp] = 0;
-	}
-	pitchWriteIdx = 0;
-
-}
-
-detectionState_t detectionState = DETECT_NOTHING;
+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 runningIdx = 0;
 	static uint8_t stableCnt = 0;
 	static float lastPitch = 0.0;
 	static float startPitch = 0.0;
-
-	relativeMove_t relativeMove = RELATIVE_MOVE_INVALID;
-
-
-	pitchHistory[pitchWriteIdx] = currentPitch;
-	runningIdx = pitchWriteIdx;
-#if 0
-	runningIdx = pitchWriteIdx + 1;
-
-	if(runningIdx == PITCH_HISTORY_ENTRIES)
-	{
-		runningIdx = 0;
-	}
-#endif
+	static float minmax = 0.0;
+	float curSpeed;
 
 	if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE))	/* discard last detection */
 	{
 		detectionState = DETECT_NOTHING;
 	}
-//	do
-//	{
-//		lastPitch = pitchHistory[runningIdx];
-#if 0
-		runningIdx++;
-		if(runningIdx == PITCH_HISTORY_ENTRIES)
-		{
-			runningIdx = 0;
-		}
-#endif
-	/* define relative movement compared to last position */
-		if(fabsf(lastPitch -  pitchHistory[runningIdx]) < MOVE_DELTA_COUNT )		/* more or less no movement */
-		{
-			relativeMove = RELATIVE_MOVE_STATIC;
-			stableCnt++;
-		}
-		else
-		{
-			if(lastPitch > pitchHistory[runningIdx])				/* decreasing */
-			{
-				relativeMove = RELATIVE_MOVE_DECREASE;
-			}
-			else
-			{
-				relativeMove = RELATIVE_MOVE_INCREASE;				/* increasing */
-			}
-		}
+
+	curSpeed = currentPitch - lastPitch;
 
-	/* feed value into statemachine */
-		switch (detectionState)
-		{
-			case DETECT_NOTHING: 	if(relativeMove == RELATIVE_MOVE_STATIC)
+	/* 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 */
 									{
-										if(stableCnt > 4)
-										{
-											detectionState = DETECT_START;
-											startPitch = lastPitch;
-										}
+										stableCnt++;
+									}
+									if(stableCnt > STABLE_STATE_COUNT)
+									{
+										detectionState = DETECT_START;
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_START:		switch(relativeMove)
+			case DETECT_START:		if(fabsf(curSpeed) > MOVE_DELTA_SPEED)
 									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
-											break;
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
-											break;
-										default:
-											break;
+										if(curSpeed > 0)
+										{
+											detectionState = DETECT_POS_MOVE;
+										}
+										else
+										{
+											detectionState = DETECT_NEG_MOVE;
+										}
+										stableCnt = 0;
+										startPitch = lastPitch;
 									}
 				break;
-			case DETECT_POS_MOVE:	switch(relativeMove)
-									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
-											break;
-										case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)
-																	{
-																		detectionState = DETECT_MAXIMA;
-																	}
-																	else
-																	{
-																		detectionState = DETECT_NOTHING;
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
-									}
-				break;
-			case DETECT_NEG_MOVE:	switch(relativeMove)
+			case DETECT_NEG_MOVE:
+			case DETECT_POS_MOVE:	if(fabsf(curSpeed) > MOVE_DELTA_SPEED )
 									{
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
-											break;
-										case RELATIVE_MOVE_STATIC:	if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)   /* significant movment */
-																	{
-																		detectionState = DETECT_MINIMA;
-																	}
-																	else
-																	{
-																		detectionState = DETECT_NOTHING;
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
-									}
-				break;
-			case DETECT_MAXIMA:		if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
-									{
-										detectionState = DETECT_NOTHING;
+										stableCnt++;
 									}
 									else
 									{
-										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										if(stableCnt >= STABLE_STATE_COUNT)	/* debounce movement */
 										{
-											detectionState = DETECT_FALLBACK;
+											if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT)
+											{
+												detectionState++;
+												minmax = lastPitch;
+											}
+											else
+											{
+												detectionState = DETECT_NOTHING;
+											}
 										}
-										if(relativeMove == RELATIVE_MOVE_INCREASE)
+										else
 										{
-											detectionState = DETECT_POS_MOVE;
+											detectionState = DETECT_NOTHING;
 										}
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_MINIMA:		if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
+			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 */
 									{
-										detectionState = DETECT_NOTHING;
+										stableCnt++;
 									}
 									else
 									{
-										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										if(stableCnt > 0)
 										{
-											detectionState = DETECT_NEG_MOVE;
-										}
-										if(relativeMove == RELATIVE_MOVE_INCREASE)
-										{
-											detectionState = DETECT_RISEBACK;
+											detectionState++;
 										}
-									}
-				break;
-
-			case DETECT_FALLBACK:	switch(relativeMove)
-									{
-										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_FALLBACK;
-											break;
-										case RELATIVE_MOVE_STATIC:  if( stableCnt >= STABLE_STATE_COUNT)
-																	{
-																		if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT) 		/* are we where started, again? */
-																		{
-																			detectionState = DETECT_POS_SHAKE;
-																			memset(pitchHistory, 0, sizeof(pitchHistory));
-																		}
-																		else
-																		{
-																			detectionState = DETECT_START;							/* start new detection */
-																			startPitch = lastPitch;
-																		}
-																	}
-											break;
-										default:
+										else
+										{
 											detectionState = DETECT_NOTHING;
-											break;
+										}
+										stableCnt = 0;
 									}
 				break;
-			case DETECT_RISEBACK:	switch(relativeMove)
+			case DETECT_RISEBACK:
+			case DETECT_FALLBACK:	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
 									{
-										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_RISEBACK;
-											break;
-										case RELATIVE_MOVE_STATIC:  if(stableCnt >= STABLE_STATE_COUNT)
-																	{
-																		if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT)
-																		{
-																			detectionState = DETECT_NEG_SHAKE;
-																			memset(pitchHistory, 0, sizeof(pitchHistory));
-																		}
-																		else
-																		{
-																			detectionState = DETECT_START;
-																			startPitch = lastPitch;
-																		}
-																	}
-											break;
-										default:
-											detectionState = DETECT_NOTHING;
-											break;
+										if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END)
+										{
+											detectionState++;
+										}
 									}
+									stableCnt++;
+				break;
+			default:
+				detectionState = DETECT_NOTHING;
 				break;
 
-			default:
-				break;
-
-		}
-		if(relativeMove != RELATIVE_MOVE_STATIC)		/* reset counter for stable time detection */
-		{
-			stableCnt = 0;
-		}
-//	} while ((runningIdx != pitchWriteIdx) && (detectionState != DETECT_NEG_SHAKE) && (detectionState != DETECT_POS_SHAKE));
+	}
+	if(stableCnt > STABLE_STATE_TIMEOUT)
+	{
+		detectionState = DETECT_NOTHING;
+		stableCnt = 0;
+	}
 
 	lastPitch = currentPitch;
-	pitchWriteIdx++;
-	if(pitchWriteIdx == PITCH_HISTORY_ENTRIES)
-	{
-		pitchWriteIdx = 0;
-	}
 	return detectionState;
 }