ostc4: Discovery/Src/motion.c comparison

comparison Discovery/Src/motion.c @ 627:189f945ae4ba

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.

author	Ideenmodellierer
date	Fri, 12 Feb 2021 21:43:27 +0100
parents	028d8f3a9410
children	c737cf5d9067

comparison

equal deleted inserted replaced

-:3e1a0e267f38
+:189f945ae4ba
 #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 */
-#define MOTION_DELTA_STABLE			0
-#define MOTION_DELTA_JITTER			1
+typedef enum
-#define MOTION_DELTA_RAISE			2
+{
-#define MOTION_DELTA_FALL			3
+	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;
 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE];			/* Change history of roll, pitch and yaw */
 		}
 		if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
 		{
 			motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
 		}
+		if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL)
+		{
+			motionDeltaHistory[axis][nextIndex]++;
+		}
 		lastValue[axis] = curValue;
 	}
 	motionDeltaHistoryIdx = nextIndex;
 }
 	uint8_t loop;
 	uint8_t index = motionDeltaHistoryIdx;
 	SDeltaHistory result = {0,0,0};
-	stepback++;						/* motionDeltaHistoryIdx is pointing to future entry => step back one to get the latest */
+	loop = stepback + 1;			/* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */
-	loop = stepback;
 	if(stepback < MOTION_DELTA_HISTORY_SIZE)
 	{
 		while(loop != 0)			/* find requested entry */
 		{
 			loop--;
 }
 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)
 		{
 			sector++;
 		}
 		else
 		{
 			break;
 		}
-		compare += 0.1;
+		compare += sectorDetection.size;
 	}
-	if(sector > sectorDetection.count)
+	if(sector >= sectorDetection.count)
 	{
-		sector = sectorDetection.count;
+		sector = sectorDetection.count - 1;
 	}
 	return sector;
 }
 void DefineSectorCount(uint8_t numOfSectors)
 	else
 	if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
 	{
 		sectorDetection.count = numOfSectors;
 	}
+	sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count;
 }
 uint8_t GetCVForSector(uint8_t selSector)
 {
 }
 void MapCVToSector()
 {
 	uint8_t ViewIndex = 0;
 	memset(sectorMap, 0, sizeof(sectorMap));
+	while(ViewIndex < (sectorDetection.count / 2))		/* define center sector */
+	{
+		ViewIndex++;
+	}
 	if(settingsGetPointer()->design == 3)		/* Big font view ? */
 	{
 		t3_set_customview_to_primary();
 		sectorMap[ViewIndex] = t3_change_customview(ACTION_END);
 		sectorMap[ViewIndex] = t7_change_customview(ACTION_END);
 	}
 	ViewIndex++;
-	while(ViewIndex < sectorDetection.count)
+	while(sectorMap[ViewIndex] == 0)
 	{
 		if(settingsGetPointer()->design == 3)		/* Big font view ? */
 		{
 			sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER);
 		}
 		else
 		{
 			sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER);
 		}
 		ViewIndex++;
+		if(ViewIndex == sectorDetection.count)
+		{
+			ViewIndex = 0;
+		}
 	}
 }
 void InitMotionDetection(void)
 	uint8_t PitchEvent = DETECT_NOTHING;
 	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
 	{
 										{
 											lastStart = -1;
 										}
 										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++;
 										}
 					break;
 				case DETECT_MAXIMA:		if(test.pitch == MOTION_DELTA_FALL)
 	float anglePitch;
 	float angleRoll;
 	float distance = 0;
 	float _a, _b;
 	SCoord u,v,n;
-	float r;
+	float r = 0.0;
+	float focusLimit = 0;
 	SCoord refVec;
 	SCoord axis_1;
 	SCoord axis_2;
 	SCoord curVec;
 	SCoord resultVec;
+	SDeltaHistory movementDelta;
 	SSettings* pSettings = settingsGetPointer();
 	roll += 180;
 	pitch += 180;
 				distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
 			}
 		}
 }
-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)
 		{
 			if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
 			{
 			inFocus = 1;
 		}
 	}
 	else
 	{
+		if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5))
+		{
+			focusCnt--;
+		}
 		if(focusCnt >= 5)						/* Reset focus faster then setting focus */
 		{
 			focusCnt--;
 		}
 		else
 		{
 			focusCnt = 0;
 			inFocus = 0;
 		}
+	}
+	if ((r<1) && (retval == 0))		/* add direction information to distance */
+	{
+		distance *= -1.0;
 	}
 return distance;
 }
 uint8_t viewInFocus(void)
 {
 			{
 				case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch);
 					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;
 					break;
 			}

Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 627:189f945ae4ba