Mercurial > public > ostc4

changeset 625:028d8f3a9410

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Switch Sector / Scroll detection to vector implementation: The vector implementation is mor robust against influences of roll and yaw to the pitch angle. To improve the performance the old sector based implementation has been replaced. In addition after entering focus state the roll angle is ignored. To improve readability and due to removal of some no longer needed function codes, function names have been updated
author Ideenmodellierer
date Mon, 08 Feb 2021 21:35:46 +0100
parents 930f1bbe0ac2
children 3e1a0e267f38
files Discovery/Inc/gfx_engine.h Discovery/Inc/motion.h Discovery/Src/base.c Discovery/Src/motion.c Discovery/Src/tMenuEditCustom.c
diffstat 5 files changed, 122 insertions(+), 154 deletions(-) [+]
line wrap: on
line diff
--- a/Discovery/Inc/gfx_engine.h	Sun Feb 07 22:05:32 2021 +0100
+++ b/Discovery/Inc/gfx_engine.h	Mon Feb 08 21:35:46 2021 +0100
@@ -173,7 +173,7 @@
 uint8_t releaseFrame(uint8_t callerId, uint32_t frameStartAddress);
 void GFX_forceReleaseFramesWithId(uint8_t callerId);
 void releaseAllFramesExcept(uint8_t callerId, uint32_t frameStartAddress);
-void housekeepingFrame(void);
+uint8_t housekeepingFrame(void);
 uint16_t blockedFramesCount(void);
 uint8_t getFrameCount(uint8_t frameId);
 
--- a/Discovery/Inc/motion.h	Sun Feb 07 22:05:32 2021 +0100
+++ b/Discovery/Inc/motion.h	Mon Feb 08 21:35:46 2021 +0100
@@ -13,6 +13,11 @@
 #define CUSTOMER_DEFINED_VIEWS	(100u)	/* value will cause the function to detect the number of selected views */
 #define CUSTOMER_KEEP_LAST_SECTORS	(200u)	/* do not update number of sectors, just define the new center position */
 
+#define MOTION_ENABLE_ROLL	(0x01u)
+#define MOTION_ENABLE_PITCH	(0x02u)
+#define MOTION_ENABLE_YAW	(0x04u)
+#define MOTION_ENABLE_ALL	(MOTION_ENABLE_ROLL | MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW)
+
 typedef enum
 {
 		MOTION_DETECT_OFF = 0,
@@ -77,15 +82,15 @@
 void HandleMotionDetection(void);
 void resetMotionDeltaHistory();
 void evaluateMotionDelta(float roll, float pitch, float yaw);
-void DefinePitchSectors(float centerAngle, uint8_t numOfSectors);
+void DefineSectorCount(uint8_t numOfSectors);
 void MapCVToSector(void);
 detectionState_t detectPitch(float currentPitch);
-detectionState_t detectSectorButtonEvent(float curPitch);
-detectionState_t detectScrollButtonEvent(float curPitch);
+detectionState_t detectSectorButtonEvent(float focusOffset);
+detectionState_t detectScrollButtonEvent(float focusOffset);
 
 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);
+float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis);
 uint8_t viewInFocus(void);
 void resetFocusState(void);
 
--- a/Discovery/Src/base.c	Sun Feb 07 22:05:32 2021 +0100
+++ b/Discovery/Src/base.c	Mon Feb 08 21:35:46 2021 +0100
@@ -757,7 +757,7 @@
 							pSettings->design = 7;
 							if(pSettings->MotionDetection == MOTION_DETECT_SECTOR)
 							{
-								DefinePitchSectors((float)(pSettings->viewPitch - 180.0),CUSTOMER_DEFINED_VIEWS);
+								DefineSectorCount(CUSTOMER_DEFINED_VIEWS);
 								MapCVToSector();
 							}
 						}
@@ -773,7 +773,7 @@
 						pSettings->design = 3;
 						if(pSettings->MotionDetection == MOTION_DETECT_SECTOR)
 						{
-							DefinePitchSectors((float)(pSettings->viewPitch - 180.0),CUSTOMER_DEFINED_VIEWS);
+							DefineSectorCount(CUSTOMER_DEFINED_VIEWS);
 							MapCVToSector();
 						}
 					}
--- a/Discovery/Src/motion.c	Sun Feb 07 22:05:32 2021 +0100
+++ b/Discovery/Src/motion.c	Mon Feb 08 21:35:46 2021 +0100
@@ -19,12 +19,6 @@
 #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				30.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 */
-#define SECTOR_FILTER				10		/* Define speed for calculated angle to follow real value */
-
 #define SECTOR_MAX					24		/* maximum number of sectors */
 #define SECTOR_SCROLL				7		/* number of sectors used for scroll detection */
 #define SECTOR_MAX_CNT				5		/* max number of views used for sector control */
@@ -34,9 +28,9 @@
 #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_JITTER_LEVEL	2.0		/* lower values are considered as stable */
+#define MOTION_DELTA_RAISE_LEVEL	4.0		/* Movement causing a significant change detected */
+#define MOTION_DELTA_FALL_LEVEL		-4.0	/* Movement causing a significant change detected */
 
 #define MOTION_DELTA_HISTORY_SIZE	20		/* Number of history data sets */
 
@@ -137,35 +131,31 @@
 	return result;
 }
 
-uint8_t GetSectorForPitch(float pitch)
+uint8_t GetSectorForFocus(float focusOffset)
 {
-	static uint8_t lastsector = 0;
-	float newPitch;
 	uint8_t sector = 0;
+	float compare = 0.1;
 
-	newPitch = pitch + sectorDetection.offset + sectorDetection.center;		/* do not use negative values and consider offset to center position */
-	if (newPitch < 0.0)							/* clip value */
-	{
-		newPitch = 0.0;
-	}
-	if (newPitch > sectorDetection.window)							/* clip value */
+	while(compare <= 0.5)
 	{
-		newPitch = sectorDetection.window;
+		if(focusOffset > compare)
+		{
+			sector++;
+		}
+		else
+		{
+			break;
+		}
+		compare += 0.1;
 	}
-
-	/* switch to other sector? */
-	if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
+	if(sector > sectorDetection.count)
 	{
-		sector = (uint16_t) newPitch / sectorDetection.size;
-		sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
-		sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
-		lastsector = sector;
+		sector = sectorDetection.count;
 	}
-
-	return lastsector;
+	return sector;
 }
 
-void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
+void DefineSectorCount(uint8_t numOfSectors)
 {
 	if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
 	{
@@ -187,29 +177,6 @@
 	{
 		sectorDetection.count = numOfSectors;
 	}
-
-	if(sectorDetection.count == SECTOR_MAX)
-	{
-		sectorDetection.window = SECTOR_WINDOW_MAX;
-	}
-	else
-	{
-		sectorDetection.window = SECTOR_WINDOW;
-	}
-
-	sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
-	sectorDetection.size = sectorDetection.window / sectorDetection.count;
-	sectorDetection.center = 0;
-
-/* reset border values */
-	sectorDetection.lowerborder = SECTOR_BORDER;
-	sectorDetection.upperborder = SECTOR_BORDER * -1.0;
-/* get the current sector */
-	sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
-	sectorDetection.target = sectorDetection.current;
-/* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
-	sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
-
 }
 
 
@@ -224,57 +191,43 @@
 		return 0;
 	}
 }
+
 void MapCVToSector()
 {
-	uint8_t centerView = 0;
+	uint8_t ViewIndex = 0;
 
 	memset(sectorMap, 0, sizeof(sectorMap));
 
-	switch(sectorDetection.count)
-	{
-		case 1: centerView = 0; break;
-		case 2: centerView = 0; break;
-		case 3: centerView = 1; break;
-		case 4: centerView = 1; break;
-		case 5: centerView = 2; break;
-		default: centerView = sectorDetection.count / 2 - 1;
-			break;
-	}
 	if(settingsGetPointer()->design == 3)		/* Big font view ? */
 	{
 		t3_set_customview_to_primary();
-		sectorMap[centerView] = t3_change_customview(ACTION_END);
+		sectorMap[ViewIndex] = t3_change_customview(ACTION_END);
 	}
 	else
 	{
 		t7_set_customview_to_primary();
-		sectorMap[centerView] = t7_change_customview(ACTION_END);
+		sectorMap[ViewIndex] = t7_change_customview(ACTION_END);
 
 	}
 
-	centerView++;
-	while(sectorMap[centerView] == 0)
+	ViewIndex++;
+	while(ViewIndex < sectorDetection.count)
 	{
 		if(settingsGetPointer()->design == 3)		/* Big font view ? */
 		{
-			sectorMap[centerView] = t3_change_customview(ACTION_BUTTON_ENTER);
+			sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER);
 		}
 		else
 		{
-			sectorMap[centerView] = t7_change_customview(ACTION_BUTTON_ENTER);
+			sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
 		}
-		centerView++;
-		if(centerView == sectorDetection.count)
-		{
-			centerView = 0;
-		}
+		ViewIndex++;
 	}
 
 }
 
 void InitMotionDetection(void)
 {
-	float sensorPitch = settingsGetPointer()->viewPitch - 180.0;	/* calib values are stored as 360° values. Sensor uses +/- 180° */
 	sectorDetection.target = 0;
 	sectorDetection.current = 0;
 	sectorDetection.size = 0;
@@ -282,12 +235,12 @@
 
 	switch(settingsGetPointer()->MotionDetection)
 	{
-		case MOTION_DETECT_SECTOR: DefinePitchSectors(sensorPitch,CUSTOMER_DEFINED_VIEWS);
+		case MOTION_DETECT_SECTOR: DefineSectorCount(CUSTOMER_DEFINED_VIEWS);
 									MapCVToSector();
 			break;
-		case MOTION_DETECT_MOVE: DefinePitchSectors(sensorPitch,SECTOR_MAX);
+		case MOTION_DETECT_MOVE: DefineSectorCount(SECTOR_MAX);
 			break;
-		case MOTION_DETECT_SCROLL: DefinePitchSectors(sensorPitch,SECTOR_SCROLL);
+		case MOTION_DETECT_SCROLL: DefineSectorCount(SECTOR_SCROLL);
 			break;
 		default:
 			break;
@@ -297,11 +250,11 @@
 }
 
 /* Map the current pitch value to a sector and create button event in case the sector is left */
-detectionState_t detectSectorButtonEvent(float curPitch)
+detectionState_t detectSectorButtonEvent(float focusOffset)
 {
 	uint8_t newTargetSector;
 
-	newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
+	newTargetSector = GetSectorForFocus(focusOffset);
 
 	if(settingsGetPointer()->design == 3)		/* Big font view ? */
 	{
@@ -316,28 +269,17 @@
 }
 
 /* Check if pitch is not in center position and trigger a button action if needed */
-detectionState_t detectScrollButtonEvent(float curPitch)
+detectionState_t detectScrollButtonEvent(float focusOffset)
 {
 	static uint8_t	delayscroll = 0;		/* slow down the number of scroll events */
 
 	uint8_t PitchEvent = DETECT_NOTHING;
-	uint8_t newSector;
 
 	if(delayscroll == 0)
 	{
-		newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
-		/* for scroll detection the motion window is split into 6 sectors => set event accoring to the sector number*/
-		switch(newSector)
+		if(focusOffset > 0.3)
 		{
-			case 0: PitchEvent = DETECT_POS_PITCH;
-				break;
-			case 6:	PitchEvent = DETECT_NEG_PITCH;
-				break;
-			default:
-				break;
-		}
-		if(PitchEvent != DETECT_NOTHING)
-		{
+			PitchEvent = DETECT_POS_PITCH;
 			delayscroll = 7;
 		}
 	}
@@ -538,11 +480,8 @@
 }
 
 
-float checkViewport(float roll, float pitch, float yaw)
+float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis)
 {
-	static float freezeRoll = 0;
-	static float freezeYaw = 0;
-
 	uint8_t retval = 0;
 	float angleYaw;
 	float anglePitch;
@@ -557,7 +496,6 @@
 	SCoord axis_2;
 	SCoord curVec;
 	SCoord resultVec;
-	SDeltaHistory test;
 
 	SSettings* pSettings = settingsGetPointer();
 
@@ -567,26 +505,49 @@
 	/* calculate base vector taking calibration delta into account yaw (heading) */
 	float compYaw;
 
-	compYaw = 360.0 - yaw; 				/* turn to 0° */
-	compYaw +=  pSettings->viewYaw; 	/* consider calib yaw value */
-	compYaw += yaw;
+	if(enableAxis & MOTION_ENABLE_YAW)
+	{
+		compYaw = 360.0 - yaw; 				/* turn to 0° */
+		compYaw +=  pSettings->viewYaw; 	/* consider calib yaw value */
+		compYaw += yaw;
+
+		if (compYaw < 0.0)
+		{
+			compYaw = 360.0 + compYaw;
+		}
 
-	if (compYaw < 0.0)
+		if (compYaw > 360.0)
+		{
+			compYaw = compYaw - 360.0;
+		}
+		if (compYaw > 360.0)
+		{
+			compYaw = compYaw - 360.0;
+		}
+		angleYaw = pSettings->viewYaw * M_PI / 180.0;
+	}
+	else
 	{
-		compYaw = 360.0 + compYaw;
+		compYaw = 0.0;
+		angleYaw = 0.0;
 	}
 
-	if (compYaw > 360.0)
+	if(enableAxis & MOTION_ENABLE_PITCH)
 	{
-		compYaw = compYaw - 360.0;
+		anglePitch = pSettings->viewPitch * M_PI / 180.0;
 	}
-	if (compYaw > 360.0)
+	else
 	{
-		compYaw = compYaw - 360.0;
+		anglePitch = 0;
 	}
-	angleYaw = pSettings->viewYaw * M_PI / 180.0;
-	anglePitch = pSettings->viewPitch * M_PI / 180.0;
-	angleRoll = pSettings->viewRoll * M_PI / 180.0;
+	if(enableAxis & MOTION_ENABLE_ROLL)
+	{
+		angleRoll = pSettings->viewRoll * M_PI / 180.0;
+	}
+	else
+	{
+		angleRoll = 0;
+	}
 
 	refVec.x = 0;
 	refVec.y = 0;
@@ -594,10 +555,6 @@
 
     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)
@@ -619,9 +576,31 @@
     }
     else
     {
-    	angleYaw = compYaw * M_PI / 180.0;
-    	anglePitch = pitch * M_PI / 180.0;
-    	angleRoll = roll * M_PI / 180.0;
+    	if(enableAxis & MOTION_ENABLE_PITCH)
+    	{
+    		anglePitch = pitch * M_PI / 180.0;
+    	}
+    	else
+    	{
+    		anglePitch = 0.0;
+    	}
+    	if(enableAxis & MOTION_ENABLE_ROLL)
+    	{
+    		angleRoll = roll * M_PI / 180.0;
+    	}
+    	else
+    	{
+    		angleRoll = 0.0;
+    	}
+    	if(enableAxis & MOTION_ENABLE_YAW)
+    	{
+    		angleYaw = compYaw * M_PI / 180.0;
+    	}
+    	else
+    	{
+    		angleYaw = 0.0;
+    	}
+
     	curVec.x = 0;
     	curVec.y = 0;
     	curVec.z = 1.0;
@@ -645,7 +624,6 @@
 		distance = retval * 1.0;			/* just for debugging */
 		if(retval == 0)
 		{
-
 			/* start calculating the matchpoint */
 			curVec = CoordMulF(curVec,r);
 			resultVec = CoordSub(refVec,curVec);
@@ -679,40 +657,18 @@
 		if((focusCnt == 10) && (inFocus == 0))
 		{
 			inFocus = 1;
-			freezeRoll = roll;
-			freezeYaw = yaw;
 		}
 	}
 	else
 	{
-		if(focusCnt >= 5)												/* Reset focus faster then setting focus */
+		if(focusCnt >= 5)						/* Reset focus faster then setting focus */
 		{
-			if(pSettings->MotionDetection != MOTION_DETECT_MOVE)		/* only apply extended focus for methods using absolute pitch values */
-			{
-				test = GetDeltaHistory(0);
-				if((test.yaw == MOTION_DELTA_STABLE) && (test.roll == MOTION_DELTA_STABLE)) 
-				{
-					if((fabsf(freezeRoll - roll) < MOTION_DELTA_JITTER_LEVEL) && (fabsf(freezeYaw - yaw) < MOTION_DELTA_JITTER_LEVEL))
-					{
-						focusCnt++;
-					}
-				}
-				else
-				{
-					if(freezeRoll != 0.0)
-					{
-						focusCnt = 1;
-					}
-				}
-			}
 			focusCnt--;
 		}
 		else
 		{
 			focusCnt = 0;
 			inFocus = 0;
-			freezeRoll = 0;
-			freezeYaw = 0;
 		}
 	}
     return distance;
@@ -735,10 +691,17 @@
 {
     detectionState_t pitchstate = DETECT_NOTHING;
     static uint8_t wasInFocus = 0;
+    float focusOffset = 0.0;
 
 	evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
-	checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
-
+	if(viewInFocus())
+	{
+		focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, (MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW));
+	}
+	else
+	{
+		focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL);
+	}
 	if(viewInFocus())
 	{
 		wasInFocus = 1;
@@ -750,9 +713,9 @@
 			{
 				case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch);
 					break;
-				case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(stateRealGetPointer()->lifeData.compass_pitch);
+				case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset);
 					break;
-				case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(stateRealGetPointer()->lifeData.compass_pitch);
+				case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(focusOffset);
 					 break;
 				default:
 					pitchstate = DETECT_NOTHING;
--- a/Discovery/Src/tMenuEditCustom.c	Sun Feb 07 22:05:32 2021 +0100
+++ b/Discovery/Src/tMenuEditCustom.c	Mon Feb 08 21:35:46 2021 +0100
@@ -217,7 +217,7 @@
     /* test if we are in focus */
     if((pSettings->viewPitch != 0.0) || (pSettings->viewRoll != 0.0) || (pSettings->viewYaw != 0.0))
     {
-    	distance = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
+    	distance = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL);
 
     	/* show "bar graph" indicating the distance to the center point */
         textIndex = 0;