Mercurial > public > ostc4

changeset 359:4258ea9b67fa MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added new files for motion detection (shaking) detection
author ideenmodellierer
date Fri, 24 May 2019 22:00:38 +0200
parents b70c26be71a0
children fc5e9fdcb156
files Discovery/Inc/motion.h Discovery/Src/motion.c
diffstat 2 files changed, 288 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Discovery/Inc/motion.h	Fri May 24 22:00:38 2019 +0200
@@ -0,0 +1,31 @@
+/*
+ * 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_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_ */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Discovery/Src/motion.c	Fri May 24 22:00:38 2019 +0200
@@ -0,0 +1,257 @@
+/*
+ * motion.c
+ *
+ *  Created on: 20.05.2019
+ *      Author: Thorsten Sonntag
+ */
+
+#include <stdint.h>
+#include <string.h>
+#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			5	/* 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;
+
+
+
+
+
+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;
+
+}
+
+/* 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;
+	static detectionState_t detectionState = DETECT_NOTHING;
+
+	pitchHistory[pitchWriteIdx] = currentPitch;
+	runningIdx = pitchWriteIdx;
+#if 0
+	runningIdx = pitchWriteIdx + 1;
+
+	if(runningIdx == PITCH_HISTORY_ENTRIES)
+	{
+		runningIdx = 0;
+	}
+#endif
+
+	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 */
+			}
+		}
+
+	/* feed value into statemachine */
+		switch (detectionState)
+		{
+			case DETECT_NOTHING: 	if(relativeMove == RELATIVE_MOVE_STATIC)
+									{
+										if(stableCnt > 4)
+										{
+											detectionState = DETECT_START;
+											startPitch = lastPitch;
+										}
+									}
+				break;
+			case DETECT_START:		switch(relativeMove)
+									{
+										case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
+											break;
+										case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
+											break;
+										default:
+											break;
+									}
+				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 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;
+									}
+									else
+									{
+										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										{
+											detectionState = DETECT_FALLBACK;
+										}
+										if(relativeMove == RELATIVE_MOVE_INCREASE)
+										{
+											detectionState = DETECT_POS_MOVE;
+										}
+									}
+				break;
+			case DETECT_MINIMA:		if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
+									{
+										detectionState = DETECT_NOTHING;
+									}
+									else
+									{
+										if(relativeMove == RELATIVE_MOVE_DECREASE)
+										{
+											detectionState = DETECT_NEG_MOVE;
+										}
+										if(relativeMove == RELATIVE_MOVE_INCREASE)
+										{
+											detectionState = DETECT_RISEBACK;
+										}
+									}
+				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:
+											detectionState = DETECT_NOTHING;
+											break;
+									}
+				break;
+			case DETECT_RISEBACK:	switch(relativeMove)
+									{
+										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;
+									}
+				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));
+
+	lastPitch = currentPitch;
+	pitchWriteIdx++;
+	if(pitchWriteIdx == PITCH_HISTORY_ENTRIES)
+	{
+		pitchWriteIdx = 0;
+	}
+	return detectionState;
+}