view Discovery/Src/motion.c @ 530:c880907fd1d7

Bugfix: No need to consider surface custom view in count of enabled view: In early motion control versions the detection was also applied in surface mode (as development support). The condition check is no longer needed because motion control is only applied in dive mode.
author Ideenmodellierer
date Tue, 15 Sep 2020 20:54:54 +0200
parents 0cd862e501f6
children e3237f580ae9
line wrap: on
line source

/*
 * motion.c
 *
 *  Created on: 20.05.2019
 *      Author: Thorsten Sonntag
 */

#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "motion.h"
#include "data_central.h"
#include "t7.h"
#include "t3.h"
#include "settings.h"

#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_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 */

detectionState_t detectionState = DETECT_NOTHING;
SSector sectorDetection;


uint8_t GetSectorForPitch(float pitch)
{
	static uint8_t lastsector = 0;
	float newPitch;
	uint8_t sector = 0;

	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 */
	{
		newPitch = sectorDetection.window;
	}

	/* switch to other sector? */
	if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
	{
		sector = (uint16_t) newPitch / sectorDetection.size;
		sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
		sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
		lastsector = sector;
	}

	return lastsector;
}

void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
{
	if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
	{
		if(settingsGetPointer()->design == 3)		/* Big font view ? */
		{
			sectorDetection.count =  t3_GetEnabled_customviews();
		}
		else
		{
			sectorDetection.count =  t7_GetEnabled_customviews();
		}
		if(sectorDetection.count > 7)
		{
			sectorDetection.count = 7;	/* more views are hard to manually control */
		}
	}
	else
	if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
	{
		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);

}

void InitMotionDetection(void)
{
	sectorDetection.target = 0;
	sectorDetection.current = 0;
	sectorDetection.size = 0;
	sectorDetection.count = 0;

	switch(settingsGetPointer()->MotionDetection)
	{
		case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS);
			break;
		case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX);
			break;
		case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL);
			break;
		default:
			break;
	}

}

/* Map the current pitch value to a sector and create button event in case the sector is left */
detectionState_t detectSectorButtonEvent(float curPitch)
{
	static uint8_t lastTargetSector = 0;
	uint8_t newTargetSector;
	uint8_t PitchEvent = DETECT_NOTHING;

/* only change sector if reading is stable */
	newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
	if(lastTargetSector == newTargetSector)
	{
		sectorDetection.target = newTargetSector;
	}
	lastTargetSector = newTargetSector;
	if(sectorDetection.target != sectorDetection.current)
	{
		 if(sectorDetection.target > sectorDetection.current)
		 {
			 sectorDetection.current++;
			PitchEvent = DETECT_POS_PITCH;
		 }
		 else
		 {
			 sectorDetection.current--;
			 PitchEvent = DETECT_NEG_PITCH;
		 }
	}
	return PitchEvent;
}

/* Check if pitch is not in center position and trigger a button action if needed */
detectionState_t detectScrollButtonEvent(float curPitch)
{
	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 windoe is split into 6 sectors => set event accoring to the sector number*/
		switch(newSector)
		{
			case 0:
			case 1:	PitchEvent = DETECT_POS_PITCH;
				break;
			case 5:
			case 6:	PitchEvent = DETECT_NEG_PITCH;
				break;
			default:
				break;
		}
		if(PitchEvent != DETECT_NOTHING)
		{
			delayscroll = 5;
		}
	}
	else
	{
		delayscroll--;
	}
	return PitchEvent;
}


/* Detect if user is generating an pitch including return to starting position */
/* 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 curSector;

	if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH))	/* discard last detection */
	{
		detectionState = DETECT_NOTHING;
	}

	curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);

	/* 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;
									}

									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;
	}
	if(detectionState != DETECT_START)
	{
		stableCnt++;
	}
	lastSector = curSector;
	if(stableCnt > STABLE_STATE_TIMEOUT)
	{
		detectionState = DETECT_NOTHING;
		stableCnt = 0;
	}

	return detectionState;
}