# HG changeset patch
# User Ideenmodellierer
# Date 1613162607 -3600
# Node ID 189f945ae4baf5e1d1b5d16a6a104d84e458f5e6
# Parent  3e1a0e267f3816910aa58a5274270dd418b54137
Improve in / out of focus transitions:
Added a function which detects if the yaw value changed significant while in focus state. This event is typically pointing to a arm movment => diver is no longer focussing at the OSTC.

Improve custom view mapping:
Added a direction indicator to the focus distance value (now +/- values instead of absolut values before). With this modification custom view sectors may have the double size and are threrfore easier to be controlled.

diff -r 3e1a0e267f38 -r 189f945ae4ba Discovery/Src/motion.c
--- a/Discovery/Src/motion.c	Fri Feb 12 21:37:50 2021 +0100
+++ b/Discovery/Src/motion.c	Fri Feb 12 21:43:27 2021 +0100
@@ -23,17 +23,28 @@
 #define SECTOR_SCROLL				7		/* number of sectors used for scroll detection */
 #define SECTOR_MAX_CNT				5		/* max number of views used for sector control */
 
-#define MOTION_DELTA_STABLE			0
-#define MOTION_DELTA_JITTER			1
-#define MOTION_DELTA_RAISE			2
-#define MOTION_DELTA_FALL			3
+
+typedef enum
+{
+	MOTION_DELTA_STABLE = 0,
+	MOTION_DELTA_JITTER,
+	MOTION_DELTA_RAISE,
+	MOTION_DELTA_RAISE_FAST,
+	MOTION_DELTA_FALL,
+	MOTION_DELTA_FALL_FAST
+} MotionDeltaState_t;
 
 #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_FAST_LEVEL		6.0		/* Movement causing a fast change detected */
 
 #define MOTION_DELTA_HISTORY_SIZE	20		/* Number of history data sets */
 
+#define MOTION_FOCUS_LIMIT			0.5		/* +/- value which defines the border of the focus area */
+#define MOTION_FOCUS_USE_SECTOR		0.4		/* +/- value for the focus area used to map secors to views */
+#define MOTION_FOCUS_SCROLL_IDLE	0.3		/* +/- value for starting generation of scroll events */
+
 detectionState_t detectionState = DETECT_NOTHING;
 SSector sectorDetection;
 
@@ -99,6 +110,12 @@
 		{
 			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
 		}
+
+		if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex]++;
+		}
+
 		lastValue[axis] = curValue;
 	}
 	motionDeltaHistoryIdx = nextIndex;
@@ -111,8 +128,7 @@
 
 	SDeltaHistory result = {0,0,0};
 
-	stepback++;						/* motionDeltaHistoryIdx is pointing to future entry => step back one to get the latest */
-	loop = stepback;
+	loop = stepback + 1;			/* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */
 	if(stepback < MOTION_DELTA_HISTORY_SIZE)
 	{
 		while(loop != 0)			/* find requested entry */
@@ -134,9 +150,9 @@
 uint8_t GetSectorForFocus(float focusOffset)
 {
 	uint8_t sector = 0;
-	float compare = 0.1;
+	float compare = -1.0 * MOTION_FOCUS_USE_SECTOR + sectorDetection.size ;		/* start with first sector upper limit */
 
-	while(compare <= 0.5)
+	while(compare <= MOTION_FOCUS_USE_SECTOR)
 	{
 		if(focusOffset > compare)
 		{
@@ -146,11 +162,11 @@
 		{
 			break;
 		}
-		compare += 0.1;
+		compare += sectorDetection.size;
 	}
-	if(sector > sectorDetection.count)
+	if(sector >= sectorDetection.count)
 	{
-		sector = sectorDetection.count;
+		sector = sectorDetection.count - 1;
 	}
 	return sector;
 }
@@ -177,6 +193,7 @@
 	{
 		sectorDetection.count = numOfSectors;
 	}
+	sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count;
 }
 
 
@@ -195,8 +212,12 @@
 void MapCVToSector()
 {
 	uint8_t ViewIndex = 0;
+	memset(sectorMap, 0, sizeof(sectorMap));
 
-	memset(sectorMap, 0, sizeof(sectorMap));
+	while(ViewIndex < (sectorDetection.count / 2))		/* define center sector */
+	{
+		ViewIndex++;
+	}
 
 	if(settingsGetPointer()->design == 3)		/* Big font view ? */
 	{
@@ -211,7 +232,7 @@
 	}
 
 	ViewIndex++;
-	while(ViewIndex < sectorDetection.count)
+	while(sectorMap[ViewIndex] == 0)
 	{
 		if(settingsGetPointer()->design == 3)		/* Big font view ? */
 		{
@@ -222,6 +243,10 @@
 			sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
 		}
 		ViewIndex++;
+		if(ViewIndex == sectorDetection.count)
+		{
+			ViewIndex = 0;
+		}
 	}
 
 }
@@ -277,11 +302,16 @@
 
 	if(delayscroll == 0)
 	{
-		if(focusOffset > 0.3)
+		if(focusOffset > MOTION_FOCUS_SCROLL_IDLE)
 		{
 			PitchEvent = DETECT_POS_PITCH;
 			delayscroll = 7;
 		}
+		if(focusOffset < (-1.0 * MOTION_FOCUS_SCROLL_IDLE))
+		{
+			PitchEvent = DETECT_NEG_PITCH;
+			delayscroll = 7;
+		}
 	}
 	else
 	{
@@ -345,12 +375,12 @@
 										}
 										duration = 0;
 					break;
-				case DETECT_NEG_MOVE:	if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE))
+				case DETECT_NEG_MOVE:	if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE) || (test.pitch == MOTION_DELTA_RAISE_FAST))
 										{
 											detectionState++;
 										}
 					break;
-				case DETECT_POS_MOVE:	if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL))
+				case DETECT_POS_MOVE:	if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL) || (test.pitch == MOTION_DELTA_FALL_FAST))
 										{
 											detectionState++;
 										}
@@ -489,7 +519,8 @@
 	float distance = 0;
 	float _a, _b;
 	SCoord u,v,n;
-	float r;
+	float r = 0.0;
+	float focusLimit = 0;
 
 	SCoord refVec;
 	SCoord axis_1;
@@ -497,6 +528,8 @@
 	SCoord curVec;
 	SCoord resultVec;
 
+	SDeltaHistory movementDelta;
+
 	SSettings* pSettings = settingsGetPointer();
 
 	roll += 180;
@@ -644,7 +677,18 @@
 		}
     }
 
-    if(distance < 0.5)		/* handle focus counter to avoid fast in/out focus changes */
+    movementDelta = GetDeltaHistory(0);
+
+    if(inFocus == 0)							/* consider option to use smaller spot to detect focus state */
+    {
+    	focusLimit = MOTION_FOCUS_LIMIT - (((pSettings->viewPortMode >> 5) & 0x03) / 10.0);
+    }
+    else
+    {
+    	focusLimit = MOTION_FOCUS_LIMIT;		/* use standard spot to detect diver interactions */
+    }
+
+    if((distance <= focusLimit) && (movementDelta.yaw != MOTION_DELTA_RAISE_FAST) && (movementDelta.yaw != MOTION_DELTA_FALL_FAST))		/* handle focus counter to avoid fast in/out focus changes */
     {
 		if(focusCnt < 10)
 		{
@@ -661,6 +705,10 @@
 	}
 	else
 	{
+		if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5))
+		{
+			focusCnt--;
+		}
 		if(focusCnt >= 5)						/* Reset focus faster then setting focus */
 		{
 			focusCnt--;
@@ -671,6 +719,10 @@
 			inFocus = 0;
 		}
 	}
+	if ((r<1) && (retval == 0))		/* add direction information to distance */
+	{
+		distance *= -1.0;
+	}
     return distance;
 }
 uint8_t viewInFocus(void)
@@ -715,7 +767,7 @@
 					break;
 				case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset);
 					break;
-				case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(focusOffset);
+				case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(fabs(focusOffset));
 					 break;
 				default:
 					pitchstate = DETECT_NOTHING;