Mercurial > public > ostc4

changeset 370:77cdfbdaca8c MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
author ideenmodellierer
date Tue, 13 Aug 2019 21:13:54 +0200
parents 210bffc496a3
children fca370f847f8
files Discovery/Inc/motion.h Discovery/Src/motion.c
diffstat 2 files changed, 142 insertions(+), 13 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/motion.h	Tue Aug 13 21:12:17 2019 +0200
+++ b/Discovery/Inc/motion.h	Tue Aug 13 21:13:54 2019 +0200
@@ -10,20 +10,37 @@
 
 
 /* exported data types */
+
+typedef enum
+{
+		MOTION_DETECT_OFF = 0,
+		MOTION_DETECT_SECTOR,
+		MOTION_DETECT_MOVE
+} MotionDetectMethod_t;
+
 typedef enum
 {
 		DETECT_START = 0,
 		DETECT_POS_MOVE,
 		DETECT_MAXIMA,
 		DETECT_FALLBACK,
-		DETECT_POS_SHAKE,
+		DETECT_POS_PITCH,
 		DETECT_NEG_MOVE,
 		DETECT_MINIMA,
 		DETECT_RISEBACK,
-		DETECT_NEG_SHAKE,
+		DETECT_NEG_PITCH,
 		DETECT_NOTHING
 } detectionState_t;
 
-detectionState_t detectShake(float currentPitch);
+typedef struct
+{
+    float upperlimit;
+    float lowerlimit;
+} SSector;
+
+void InitMotionDetection(void);
+void DefinePitchSectors(float centerAngle);
+detectionState_t detectPitch(float currentPitch);
+detectionState_t detectSectorButtonEvent(float curPitch);
 
 #endif /* INC_MOTION_H_ */
--- a/Discovery/Src/motion.c	Tue Aug 13 21:12:17 2019 +0200
+++ b/Discovery/Src/motion.c	Tue Aug 13 21:13:54 2019 +0200
@@ -9,26 +9,133 @@
 #include <string.h>
 #include <math.h>
 #include "motion.h"
+#include "data_central.h"
+#include "t7.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 */
+#define PITCH_DELTA_COUNT			10	/* Delta count needed to identify a valid minima / maxima */
+#define PITCH_DELTA_END				10	/* Delta allowed between start and end position */
+
+
+#define SECTOR_WINDOW				40.0  	/* Pitch window which is used for custom view projection */
+#define SECTOR_HYSTERY				3		/* Additional offset to avoid fast changing displays */
+#define SECTOR_BORDER				400.0	/* Define a value which is out of limit to avoid not wanted key events */
+#define SECTOR_FILTER				10		/* Define speed for calculated angle to follow real value */
+
 
 static detectionState_t detectionState = DETECT_NOTHING;
 
-/* Detect if user is generating an pitch including return to starting position (shake) */
+static uint8_t curSector;
+static uint8_t targetSector;
+static uint8_t sectorSize;
+static uint8_t sectorCount;
+
+SSector PitchSector[10];			/* max number of enabled custom views */
+
+
+uint8_t GetSectorForPitch(float pitch)
+{
+	static float lastPitch = 1000;
+	float newPitch;
+	uint8_t index;
+	uint8_t sector = 0;
+
+	if(lastPitch == 1000)						/* init at first call */
+	{
+		lastPitch = pitch;
+	}
+
+	newPitch = lastPitch + (pitch / SECTOR_FILTER);
+
+	for(index = 1; index < sectorCount; index++)
+	{
+		if((pitch < PitchSector[index].upperlimit) && (pitch > PitchSector[index].lowerlimit ))
+		{
+			sector = index;
+			break;
+		}
+	}
+	lastPitch = newPitch;
+	return sector;
+}
+
+void DefinePitchSectors(float centerPitch)
+{
+	uint8_t index;
+
+	sectorCount =  t7_GetEnabled_customviews();
+	sectorSize = SECTOR_WINDOW / sectorCount;
+
+	PitchSector[0].upperlimit = centerPitch + (SECTOR_WINDOW / 2);
+	PitchSector[0].lowerlimit = PitchSector[0].upperlimit - sectorSize - SECTOR_HYSTERY;
+
+	for(index = 1; index < sectorCount; index++)
+	{
+		PitchSector[index].upperlimit = PitchSector[0].upperlimit - index * sectorSize + SECTOR_HYSTERY;
+		PitchSector[index].lowerlimit = PitchSector[0].upperlimit - (index + 1) * sectorSize - SECTOR_HYSTERY;
+	}
+
+	PitchSector[0].upperlimit = SECTOR_BORDER;
+	PitchSector[index - 1].lowerlimit = SECTOR_BORDER * -1.0;
+
+/* get the current sector */
+	curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+	targetSector = curSector;
+}
+
+void InitMotionDetection(void)
+{
+	targetSector = 0;
+	curSector = 0;
+	sectorSize = 0;
+	sectorCount = 0;
+	DefinePitchSectors(0);
+}
+
+/* Map the current pitch value to a sector and create button event in case the sector is left */
+detectionState_t detectSectorButtonEvent(float curPitch)
+{
+	static uint8_t lastTargetSector = 0;
+	uint8_t newTargetSector;
+	uint8_t PitchEvent = DETECT_NOTHING;
+
+/* only change sector if reading is stable */
+	newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+	if(lastTargetSector == newTargetSector)
+	{
+		targetSector = newTargetSector;
+	}
+	lastTargetSector = 	newTargetSector;
+	if(targetSector != curSector)
+	{
+		 if(targetSector > curSector)
+		 {
+			 curSector++;
+			PitchEvent = DETECT_POS_PITCH;
+		 }
+		 else
+		 {
+			 curSector--;
+			 PitchEvent = DETECT_NEG_PITCH;
+		 }
+	}
+	return PitchEvent;
+}
+
+
+/* Detect if user is generating an pitch including return to starting position */
 /* This is done by feeding the past movements value per value into a state machine */
-detectionState_t detectShake(float currentPitch)
+detectionState_t detectPitch(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 */
+
+	if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH))	/* discard last detection */
 	{
 		detectionState = DETECT_NOTHING;
 	}
@@ -42,6 +149,11 @@
 									{
 										stableCnt++;
 									}
+									else
+									{
+										stableCnt=0;
+									}
+
 									if(stableCnt > STABLE_STATE_COUNT)
 									{
 										detectionState = DETECT_START;
@@ -71,10 +183,9 @@
 									{
 										if(stableCnt >= STABLE_STATE_COUNT)	/* debounce movement */
 										{
-											if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT)
+											if(fabsf(startPitch - currentPitch) > PITCH_DELTA_COUNT)
 											{
 												detectionState++;
-												minmax = lastPitch;
 											}
 											else
 											{
@@ -89,7 +200,8 @@
 									}
 				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 */
+			case DETECT_MAXIMA:		/* stay at maximum for short time to add a pattern for user interaction */
+									if(fabsf(curSpeed) < MOVE_DELTA_SPEED )
 									{
 										stableCnt++;
 									}
@@ -109,7 +221,7 @@
 			case DETECT_RISEBACK:
 			case DETECT_FALLBACK:	if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
 									{
-										if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END)
+										if(fabsf(startPitch - currentPitch) < PITCH_DELTA_END)
 										{
 											detectionState++;
 										}