Mercurial > public > ostc4

changeset 551:e3237f580ae9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added viewport functionality: The viewport shall estimate if the user is looking at the display (in focus) or not. This is done by vector calculation using roll, pitch and yaw angles. The first usecase is to enable the motion detection feature: It is only active if the user is looking at the display. Still in development state => deactivated by compile switch
author Ideenmodellierer
date Sun, 08 Nov 2020 18:43:19 +0100
parents af1c3e3abd5f
children 531e7818b737
files Discovery/Inc/base.h Discovery/Inc/motion.h Discovery/Src/motion.c
diffstat 3 files changed, 415 insertions(+), 91 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/base.h	Sun Nov 08 18:38:53 2020 +0100
+++ b/Discovery/Inc/base.h	Sun Nov 08 18:43:19 2020 +0100
@@ -108,6 +108,7 @@
 void set_globalState_Log_Page(uint8_t pageIsLine);
 void set_returnFromComm(void);
 uint8_t font_update_required(void);
+void set_Backlight_Boost(uint8_t level);
 
 #endif /* BASE_H */
 
--- a/Discovery/Inc/motion.h	Sun Nov 08 18:38:53 2020 +0100
+++ b/Discovery/Inc/motion.h	Sun Nov 08 18:43:19 2020 +0100
@@ -51,13 +51,39 @@
     uint8_t count;				/* number of sectors used for detection */
 } SSector;
 
+typedef struct
+{
+	float x;
+	float y;
+	float z;
+} SCoord;
 
+typedef enum
+{
+		MOTION_HISTORY_ROLL = 0,
+		MOTION_HISTORY_PITCH,
+		MOTION_HISTORY_YAW
+} MotionHistoryEntry_t;
 
+typedef struct
+{
+	uint8_t roll;
+	uint8_t pitch;
+	uint8_t yaw;
+} SDeltaHistory;
 
 void InitMotionDetection(void);
+void resetMotionDeltaHistory();
+void evaluateMotionDelta(float roll, float pitch, float yaw);
 void DefinePitchSectors(float centerAngle, uint8_t numOfSectors);
 detectionState_t detectPitch(float currentPitch);
 detectionState_t detectSectorButtonEvent(float curPitch);
 detectionState_t detectScrollButtonEvent(float curPitch);
 
+void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord);
+void calibrateViewport(float roll, float pitch, float yaw);
+float checkViewport(float roll, float pitch, float yaw);
+uint8_t viewInFocus(void);
+void resetFocusState(void);
+
 #endif /* INC_MOTION_H_ */
--- a/Discovery/Src/motion.c	Sun Nov 08 18:38:53 2020 +0100
+++ b/Discovery/Src/motion.c	Sun Nov 08 18:43:19 2020 +0100
@@ -18,7 +18,7 @@
 #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 SECTOR_WINDOW				80.0  	/* Pitch window which is used for custom view projection */
+#define SECTOR_WINDOW				40.0  	/* Pitch window which is used for custom view projection */
 #define SECTOR_WINDOW_MAX			120.0  	/* Pitch window which will be greater than the divers field of view */
 #define SECTOR_HYSTERY				2		/* 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 */
@@ -27,9 +27,108 @@
 #define SECTOR_MAX					24		/* maximum number of sectors */
 #define SECTOR_SCROLL				7		/* number of sectors used for scroll detection */
 
+#define MOTION_DELTA_STABLE			0
+#define MOTION_DELTA_JITTER			1
+#define MOTION_DELTA_RAISE			2
+#define MOTION_DELTA_FALL			3
+
+#define MOTION_DELTA_JITTER_LEVEL	3.0		/* lower values are considered as stable */
+#define MOTION_DELTA_RAISE_LEVEL	6.0	/* Movement causing a significant change detected */
+#define MOTION_DELTA_FALL_LEVEL		-6.0	/* Movement causing a significant change detected */
+
+#define MOTION_DELTA_HISTORY_SIZE	20		/* Number of history data sets */
+
 detectionState_t detectionState = DETECT_NOTHING;
 SSector sectorDetection;
 
+static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE];			/* Change history of roll, pitch and yaw */
+static uint8_t motionDeltaHistoryIdx;										/* Current index of history data */
+
+static uint8_t focusCnt = 0;
+static uint8_t inFocus = 0;
+
+void resetMotionDeltaHistory()
+{
+	motionDeltaHistoryIdx = 0;
+	memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory));
+}
+
+void evaluateMotionDelta(float roll, float pitch, float yaw)
+{
+	static float lastValue[3] = {0.0,0.0,0.0};
+	uint8_t nextIndex = motionDeltaHistoryIdx + 1;
+	uint8_t axis;
+	float curValue;
+
+	if(nextIndex == MOTION_DELTA_HISTORY_SIZE)
+	{
+		nextIndex = 0;
+	}
+	for(axis=0; axis < 3; axis++)
+	{
+		switch(axis)
+		{
+			case MOTION_HISTORY_ROLL:	curValue = roll;
+				break;
+			case MOTION_HISTORY_PITCH:	curValue = pitch;
+				break;
+			default:
+			case MOTION_HISTORY_YAW:	if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0))		/* transition 360 => 0 */
+										{
+											lastValue[MOTION_HISTORY_YAW] -= 360;
+										}
+										else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0))	/* transition 0 => 360 */
+										{
+											lastValue[MOTION_HISTORY_YAW] += 360;
+										}
+										curValue = yaw;
+				break;
+		}
+		if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE;
+		}
+		if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER;
+		}
+		if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE;
+		}
+		if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
+		}
+		lastValue[axis] = curValue;
+	}
+	motionDeltaHistoryIdx = nextIndex;
+}
+
+SDeltaHistory GetDeltaHistory(uint8_t stepback)
+{
+	uint8_t loop = stepback;
+	uint8_t index = motionDeltaHistoryIdx;
+
+	SDeltaHistory result = {0,0,0};
+
+	if(stepback < MOTION_DELTA_HISTORY_SIZE)
+	{
+		while(loop != 0)			/* find requested entry */
+		{
+			loop--;
+			index--;
+			if(index == 0)
+			{
+				index = MOTION_DELTA_HISTORY_SIZE - 1;
+			}
+		}
+		result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index];
+		result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index];
+		result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
+	}
+	return result;
+}
 
 uint8_t GetSectorForPitch(float pitch)
 {
@@ -115,16 +214,17 @@
 
 	switch(settingsGetPointer()->MotionDetection)
 	{
-		case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS);
+		case MOTION_DETECT_SECTOR: DefinePitchSectors(settingsGetPointer()->viewPitch,CUSTOMER_DEFINED_VIEWS);
 			break;
-		case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX);
+		case MOTION_DETECT_MOVE: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_MAX);
 			break;
-		case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL);
+		case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL);
 			break;
 		default:
 			break;
 	}
 
+	resetMotionDeltaHistory();
 }
 
 /* Map the current pitch value to a sector and create button event in case the sector is left */
@@ -168,7 +268,7 @@
 	if(delayscroll == 0)
 	{
 		newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
-		/* for scroll detection the motion windoe is split into 6 sectors => set event accoring to the sector number*/
+		/* for scroll detection the motion window is split into 6 sectors => set event accoring to the sector number*/
 		switch(newSector)
 		{
 			case 0:
@@ -197,101 +297,298 @@
 /* This is done by feeding the past movements value per value into a state machine */
 detectionState_t detectPitch(float currentPitch)
 {
-	static uint8_t lastSector = 0;
-	static uint8_t startSector = 0;
-	static uint8_t stableCnt = 0;
+	uint8_t exit = 0;
+	uint8_t step = 0;
+	SDeltaHistory test;
 
-	uint8_t curSector;
-
-	if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH))	/* discard last detection */
+	detectionState = DETECT_NOTHING;
+	while((step != MOTION_DELTA_HISTORY_SIZE) && (!exit))		/* start backward evalution of pitch changes*/
+	{
+		test = GetDeltaHistory(step);
+		step++;
+		switch (detectionState)
+		{
+				case DETECT_NOTHING: 	if(test.pitch > MOTION_DELTA_STABLE)
+										{
+											exit = 1;
+										}
+										else
+										{
+											detectionState = DETECT_START;
+										}
+					break;
+				case DETECT_START:		if(test.pitch == MOTION_DELTA_RAISE)
+										{
+											detectionState = DETECT_POS_MOVE;
+										}
+										if(test.pitch == MOTION_DELTA_FALL)
+										{
+											detectionState = DETECT_NEG_MOVE;
+										}
+					break;
+				case DETECT_NEG_MOVE:
+				case DETECT_POS_MOVE:	if(test.pitch <= MOTION_DELTA_JITTER)
+										{
+											detectionState++;
+										}
+					break;
+				case DETECT_MAXIMA:		if(test.pitch == MOTION_DELTA_FALL)
+										{
+											detectionState = DETECT_FALLBACK;
+										}
+					break;
+				case DETECT_MINIMA:		if(test.pitch == MOTION_DELTA_RAISE)
+										{
+											detectionState = DETECT_RISEBACK;
+										}
+					break;
+				case DETECT_RISEBACK:
+				case DETECT_FALLBACK:	if(test.pitch == MOTION_DELTA_STABLE)
+										{
+											detectionState++;
+											exit = 1;
+										}
+									break;
+				default:
+					detectionState = DETECT_NOTHING;
+					exit = 1;
+				break;
+		}
+	}
+	if((detectionState != DETECT_POS_PITCH) && (detectionState != DETECT_NEG_PITCH))	/* nothing found */
 	{
 		detectionState = DETECT_NOTHING;
 	}
+	else																				/* dont detect the same event twice */
+	{
+		resetMotionDeltaHistory();
+	}
+	return detectionState;
+}
 
-	curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord)
+{
+	pCoord->x = ((cosf(yaw) * cosf(pitch)) * pCoord->x + (cosf(yaw)*sinf(pitch)*sinf(roll) - (sinf(yaw)* cosf(roll))) * pCoord->y + (cosf(yaw)*sinf(pitch)*cosf(roll) + sinf(yaw)*sinf(roll)) * pCoord->z);
+	pCoord->y = ((sinf(yaw) * cosf(pitch)) * pCoord->x + (sinf(yaw)*sinf(pitch)*sinf(roll) + cosf(yaw) * cosf(roll)) * pCoord->y + ( sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll))* pCoord->z);
+	pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z);
+}
+
+SCoord CoordAdd(SCoord cA, SCoord cB)
+{
+	SCoord result;
+
+	result.x = cA.x + cB.x;
+	result.y = cA.y + cB.y;
+	result.z = cA.z + cB.z;
+	return result;
+}
+
+SCoord CoordSub(SCoord cA, SCoord cB)
+{
+	SCoord result;
 
-	/* feed value into state machine */
-	switch (detectionState)
-	{
-			case DETECT_NOTHING: 	if(curSector != lastSector)	/* detect a stable condition before evaluating for the next move */
-									{
-										stableCnt=0;
-									}
+	result.x = cA.x - cB.x;
+	result.y = cA.y - cB.y;
+	result.z = cA.z - cB.z;
+	return result;
+}
+
+SCoord CoordCross(SCoord cA, SCoord cB)
+{
+	SCoord result;
+
+	result.x = (cA.y * cB.z) - (cA.z * cB.y);
+	result.y = (cA.z * cB.x) - (cA.x * cB.z);
+	result.z = (cA.x * cB.y) - (cA.y * cB.x);
+
+	return result;
+
+}
+
+SCoord CoordMulF(SCoord op, float factor)
+{
+	SCoord result;
+	result.x = (op.x * factor);
+	result.y = (op.y * factor);
+	result.z = (op.z * factor);
+
+	return result;
+}
 
-									if(stableCnt > STABLE_STATE_COUNT)
-									{
-										detectionState = DETECT_START;
-										stableCnt = 0;
-										startSector = lastSector;
-									}
-				break;
-			case DETECT_START:		if(curSector != lastSector)
-									{
-										if(abs(curSector - startSector) > 1)
-										{
-											if(curSector > lastSector)
-											{
-												detectionState = DETECT_POS_MOVE;
-											}
-											else
-											{
-												detectionState = DETECT_NEG_MOVE;
-											}
-											stableCnt = 0;
-											startSector = lastSector;
-										}
-									}
-				break;
-			case DETECT_NEG_MOVE:
-			case DETECT_POS_MOVE:	if(curSector == lastSector)		/* Moved to a max? */
-									{
-										if(abs(startSector - curSector) > 2)
-										{
-											detectionState++;
-											stableCnt = 0;
-										}
-										if(stableCnt > 2)
-										{
-											detectionState = DETECT_NOTHING;
-											stableCnt = 0;
-										}
-									}
-				break;
-			case DETECT_MAXIMA:
-			case DETECT_MINIMA:		if(curSector != lastSector)		/* reset timeout detection */
-									{
-										detectionState++;
-										stableCnt = 0;
-									}
-				break;
-			case DETECT_RISEBACK:
-			case DETECT_FALLBACK:
-									if(curSector == lastSector)		/* check if we are back at start position at end of movement */
-									{
-										if(abs(startSector - curSector) <= 1)
-										{
-											if(stableCnt > 2)
-											{
-												detectionState++;
-												stableCnt = 0;
-											}
-										}
-									}
-								break;
-			default:
-				detectionState = DETECT_NOTHING;
-				break;
+SCoord CoordDivF(SCoord op, float factor)
+{
+	SCoord result;
+	result.x = (op.x / factor);
+	result.y = (op.y / factor);
+	result.z = (op.z / factor);
+
+	return result;
+}
+
+float CoordDot(SCoord cA, SCoord cB)
+{
+	float result;
+
+	result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z;
+	return result;
+}
+
+void calibrateViewport(float roll, float pitch, float yaw)
+{
+    SSettings* pSettings = settingsGetPointer();
+
+    pSettings->viewPitch = pitch;
+	pSettings->viewRoll = roll;
+	pSettings->viewYaw = yaw;
+}
+
+
+float checkViewport(float roll, float pitch, float yaw)
+{
+	uint8_t retval = 0;
+	float angleYaw;
+	float anglePitch;
+	float angleRoll;
+	float distance = 0;
+	float _a, _b;
+	SCoord u,v,n;
+	float r;
+
+	SCoord refVec;
+	SCoord axis_1;
+	SCoord axis_2;
+	SCoord curVec;
+	SCoord resultVec;
+
+	SSettings* pSettings = settingsGetPointer();
+
+	/* calculate base vector taking calibration delta into account yaw (heading) */
+	float compYaw = yaw + pSettings->viewYaw;
+	if (compYaw < 0.0)
+	{
+		compYaw = 360.0 + compYaw;
 	}
-	if(detectionState != DETECT_START)
+
+	if (compYaw > 360.0)
 	{
-		stableCnt++;
-	}
-	lastSector = curSector;
-	if(stableCnt > STABLE_STATE_TIMEOUT)
-	{
-		detectionState = DETECT_NOTHING;
-		stableCnt = 0;
+		compYaw = compYaw - 360.0;;
 	}
 
-	return detectionState;
+	angleYaw = compYaw * M_PI / 180.0;
+	anglePitch = pSettings->viewPitch * M_PI / 180.0;
+	angleRoll = pSettings->viewRoll * M_PI / 180.0;
+
+	refVec.x = 0;
+	refVec.y = 0;
+	refVec.z = 1.0;
+
+    anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
+
+	anglePitch = pitch * M_PI / 180.0;
+	angleRoll = roll * M_PI / 180.0;
+    angleYaw = yaw * M_PI / 180.0;
+
+    /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
+    axis_1.x = 0;
+    if(refVec.y >=0)
+    {
+    	axis_2.y = 1; /* => Spawn y == refVec y */
+    }
+    else axis_1.y = -1;
+    axis_1.z = -1.0 * refVec.y / refVec.z;
+    axis_2 = CoordCross(refVec, axis_1);	/* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */
+
+    /* check if detection plane is correct */
+	u = CoordSub(axis_1,refVec);
+    v = CoordSub(axis_2,refVec);
+    n = CoordCross(u,v);
+
+    if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001))
+    {
+    	retval = 2;
+    }
+    else
+    {
+    	angleYaw = yaw * M_PI / 180.0;
+    	anglePitch = pitch * M_PI / 180.0;
+    	angleRoll = roll * M_PI / 180.0;
+    	curVec.x = 0;
+    	curVec.y = 0;
+    	curVec.z = 1.0;
+		anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
+
+		_a = CoordDot(curVec,n);
+		_b = CoordDot(refVec,n);
+
+		if(_b>=(-0.0001)&&_b<=0.0001)		/* Check if view port is parallel (no matchpoint) */
+		{
+			retval = 3;
+		}
+		else
+		{
+			r=_a/_b;
+			if(r<0.00||r>1.40)				/* are we looking into wrong direction? */
+			{
+				retval = 4;
+			}
+		}
+		distance = retval * 1.0;			/* just for debugging */
+		if(retval == 0)
+		{
+
+			/* start calculating the matchpoint */
+			curVec = CoordMulF(curVec,r);
+			resultVec = CoordSub(refVec,curVec);
+
+			/* calculate the distance between reference and actual vector */
+			resultVec.x = resultVec.x * resultVec.x;
+			resultVec.y = resultVec.y * resultVec.y;
+			resultVec.z = resultVec.z * resultVec.z;
+
+			if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0))
+			{
+				distance = 0.0;
+			}
+			else
+			{
+				distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
+			}
+		}
+    }
+
+    if(distance < 0.5)		/* handle focus counter to avoid fast in/out focus changes */
+    {
+		if(focusCnt < 10)
+		{
+			if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
+			{
+				resetMotionDeltaHistory();
+			}
+			focusCnt++;
+		}
+		if(focusCnt == 10)
+		{
+			inFocus = 1;
+		}
+	}
+	else
+	{
+		if(focusCnt)
+		{
+			focusCnt--;
+		}
+		else
+		{
+			inFocus = 0;
+		}
+	}
+    return distance;
 }
+uint8_t viewInFocus(void)
+{
+	return inFocus;
+}
+void resetFocusState(void)
+{
+	inFocus = 0;
+}