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