ostc4: Discovery/Src/motion.c comparison

comparison Discovery/Src/motion.c @ 574:01ee21dd311f

Improved focus detection: In previous version th calibration window was turned into the current one. At all two translations were used causing a jitter. Now the current view is translated into the calibration windows by one step increasing the quality of the prediction. Reworked detection of pitch/sector/scroll events: Sector and scroll uses pitch angles to calculate the view to show. Movment detection uses movment, which may also be out of focus for a short time. To improve all detections these two use cases are now handled individual. Sector and scroll have increased action angle in case yaw and roll stay stable.

author	Ideenmodellierer
date	Wed, 25 Nov 2020 20:26:01 +0100
parents	e3237f580ae9
children	beb4d47542f1

comparison

equal deleted inserted replaced

-:f9c6ef098275
+:01ee21dd311f
 #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_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;
 	{
 		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)
 		{
-			case 0:
+			case 0: PitchEvent = DETECT_POS_PITCH;
-			case 1:	PitchEvent = DETECT_POS_PITCH;
 				break;
-			case 5:
 			case 6:	PitchEvent = DETECT_NEG_PITCH;
 				break;
 			default:
 				break;
 		}
 		if(PitchEvent != DETECT_NOTHING)
 		{
-			delayscroll = 5;
+			delayscroll = 7;
 		}
 	}
 	else
 	{
 		delayscroll--;
 /* This is done by feeding the past movements value per value into a state machine */
 detectionState_t detectPitch(float currentPitch)
 {
 	uint8_t exit = 0;
 	uint8_t step = 0;
+	uint8_t duration = 0;
 	SDeltaHistory test;
 	detectionState = DETECT_NOTHING;
 	while((step != MOTION_DELTA_HISTORY_SIZE) && (!exit))		/* start backward evalution of pitch changes*/
 	{
 		test = GetDeltaHistory(step);
 		step++;
+		duration++;
 		switch (detectionState)
 		{
 				case DETECT_NOTHING: 	if(test.pitch > MOTION_DELTA_STABLE)
 										{
 											exit = 1;
 										}
 										if(test.pitch == MOTION_DELTA_FALL)
 										{
 											detectionState = DETECT_NEG_MOVE;
 										}
+										duration = 0;
 					break;
 				case DETECT_NEG_MOVE:
 				case DETECT_POS_MOVE:	if(test.pitch <= MOTION_DELTA_JITTER)
 										{
 											detectionState++;
 										}
 					break;
 				case DETECT_RISEBACK:
 				case DETECT_FALLBACK:	if(test.pitch == MOTION_DELTA_STABLE)
 										{
-											detectionState++;
+											if(duration > 5)	/* avoid detection triggered by short moves */
-											exit = 1;
+											{
+												detectionState++;
+												exit = 1;
+											}
+											else
+											{
+												detectionState = DETECT_NOTHING;
+											}
 										}
 									break;
 				default:
 					detectionState = DETECT_NOTHING;
 					exit = 1;
 }
 float checkViewport(float roll, float pitch, float yaw)
 {
+	static float freezeRoll = 0;
+	static float freezeYaw = 0;
 	uint8_t retval = 0;
 	float angleYaw;
 	float anglePitch;
 	float angleRoll;
 	float distance = 0;
 	SCoord refVec;
 	SCoord axis_1;
 	SCoord axis_2;
 	SCoord curVec;
 	SCoord resultVec;
+	SDeltaHistory test;
 	SSettings* pSettings = settingsGetPointer();
 	/* calculate base vector taking calibration delta into account yaw (heading) */
 	float compYaw = yaw + pSettings->viewYaw;
+	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 > 360.0)
 	{
-		compYaw = compYaw - 360.0;;
+		compYaw = compYaw - 360.0;
 	}
+	if (compYaw > 360.0)
-	angleYaw = compYaw * M_PI / 180.0;
+	{
+		compYaw = compYaw - 360.0;
+	}
+	angleYaw = pSettings->viewYaw * M_PI / 180.0;
 	anglePitch = pSettings->viewPitch * M_PI / 180.0;
 	angleRoll = pSettings->viewRoll * M_PI / 180.0;
 	refVec.x = 0;
 	refVec.y = 0;
 {
 	retval = 2;
 }
 else
 {
-	angleYaw = yaw * M_PI / 180.0;
+	angleYaw = compYaw * 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;
 			{
 				resetMotionDeltaHistory();
 			}
 			focusCnt++;
 		}
-		if(focusCnt == 10)
+		if((focusCnt == 10) && (inFocus == 0))
 		{
 			inFocus = 1;
+			freezeRoll = roll;
+			freezeYaw = yaw;
 		}
 	}
 	else
 	{
-		if(focusCnt)
+		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;
 }
 uint8_t viewInFocus(void)

Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 574:01ee21dd311f