Mercurial > public > ostc4
changeset 370:77cdfbdaca8c MotionDetection
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
author | ideenmodellierer |
---|---|
date | Tue, 13 Aug 2019 21:13:54 +0200 (2019-08-13) |
parents | 210bffc496a3 |
children | fca370f847f8 |
files | Discovery/Inc/motion.h Discovery/Src/motion.c |
diffstat | 2 files changed, 142 insertions(+), 13 deletions(-) [+] |
line wrap: on
line diff
--- a/Discovery/Inc/motion.h Tue Aug 13 21:12:17 2019 +0200 +++ b/Discovery/Inc/motion.h Tue Aug 13 21:13:54 2019 +0200 @@ -10,20 +10,37 @@ /* exported data types */ + +typedef enum +{ + MOTION_DETECT_OFF = 0, + MOTION_DETECT_SECTOR, + MOTION_DETECT_MOVE +} MotionDetectMethod_t; + typedef enum { DETECT_START = 0, DETECT_POS_MOVE, DETECT_MAXIMA, DETECT_FALLBACK, - DETECT_POS_SHAKE, + DETECT_POS_PITCH, DETECT_NEG_MOVE, DETECT_MINIMA, DETECT_RISEBACK, - DETECT_NEG_SHAKE, + DETECT_NEG_PITCH, DETECT_NOTHING } detectionState_t; -detectionState_t detectShake(float currentPitch); +typedef struct +{ + float upperlimit; + float lowerlimit; +} SSector; + +void InitMotionDetection(void); +void DefinePitchSectors(float centerAngle); +detectionState_t detectPitch(float currentPitch); +detectionState_t detectSectorButtonEvent(float curPitch); #endif /* INC_MOTION_H_ */
--- a/Discovery/Src/motion.c Tue Aug 13 21:12:17 2019 +0200 +++ b/Discovery/Src/motion.c Tue Aug 13 21:13:54 2019 +0200 @@ -9,26 +9,133 @@ #include <string.h> #include <math.h> #include "motion.h" +#include "data_central.h" +#include "t7.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 MOVE_DELTA_SPEED 4 /* Delta speed needed to identify a valid movement */ -#define SHAKE_DELTA_COUNT 10 /* Delta count needed to identify a valid minima / maxima */ -#define SHAKE_DELTA_END 10 /* Delta allowed between start and end position */ +#define PITCH_DELTA_COUNT 10 /* Delta count needed to identify a valid minima / maxima */ +#define PITCH_DELTA_END 10 /* Delta allowed between start and end position */ + + +#define SECTOR_WINDOW 40.0 /* Pitch window which is used for custom view projection */ +#define SECTOR_HYSTERY 3 /* 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 */ + static detectionState_t detectionState = DETECT_NOTHING; -/* Detect if user is generating an pitch including return to starting position (shake) */ +static uint8_t curSector; +static uint8_t targetSector; +static uint8_t sectorSize; +static uint8_t sectorCount; + +SSector PitchSector[10]; /* max number of enabled custom views */ + + +uint8_t GetSectorForPitch(float pitch) +{ + static float lastPitch = 1000; + float newPitch; + uint8_t index; + uint8_t sector = 0; + + if(lastPitch == 1000) /* init at first call */ + { + lastPitch = pitch; + } + + newPitch = lastPitch + (pitch / SECTOR_FILTER); + + for(index = 1; index < sectorCount; index++) + { + if((pitch < PitchSector[index].upperlimit) && (pitch > PitchSector[index].lowerlimit )) + { + sector = index; + break; + } + } + lastPitch = newPitch; + return sector; +} + +void DefinePitchSectors(float centerPitch) +{ + uint8_t index; + + sectorCount = t7_GetEnabled_customviews(); + sectorSize = SECTOR_WINDOW / sectorCount; + + PitchSector[0].upperlimit = centerPitch + (SECTOR_WINDOW / 2); + PitchSector[0].lowerlimit = PitchSector[0].upperlimit - sectorSize - SECTOR_HYSTERY; + + for(index = 1; index < sectorCount; index++) + { + PitchSector[index].upperlimit = PitchSector[0].upperlimit - index * sectorSize + SECTOR_HYSTERY; + PitchSector[index].lowerlimit = PitchSector[0].upperlimit - (index + 1) * sectorSize - SECTOR_HYSTERY; + } + + PitchSector[0].upperlimit = SECTOR_BORDER; + PitchSector[index - 1].lowerlimit = SECTOR_BORDER * -1.0; + +/* get the current sector */ + curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); + targetSector = curSector; +} + +void InitMotionDetection(void) +{ + targetSector = 0; + curSector = 0; + sectorSize = 0; + sectorCount = 0; + DefinePitchSectors(0); +} + +/* 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) + { + targetSector = newTargetSector; + } + lastTargetSector = newTargetSector; + if(targetSector != curSector) + { + if(targetSector > curSector) + { + curSector++; + PitchEvent = DETECT_POS_PITCH; + } + else + { + curSector--; + PitchEvent = DETECT_NEG_PITCH; + } + } + 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 detectShake(float currentPitch) +detectionState_t detectPitch(float currentPitch) { static uint8_t stableCnt = 0; static float lastPitch = 0.0; static float startPitch = 0.0; - static float minmax = 0.0; float curSpeed; - if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE)) /* discard last detection */ + + if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */ { detectionState = DETECT_NOTHING; } @@ -42,6 +149,11 @@ { stableCnt++; } + else + { + stableCnt=0; + } + if(stableCnt > STABLE_STATE_COUNT) { detectionState = DETECT_START; @@ -71,10 +183,9 @@ { if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */ { - if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT) + if(fabsf(startPitch - currentPitch) > PITCH_DELTA_COUNT) { detectionState++; - minmax = lastPitch; } else { @@ -89,7 +200,8 @@ } break; case DETECT_MINIMA: - case DETECT_MAXIMA: if(fabsf(currentPitch - minmax ) < SHAKE_DELTA_COUNT) /* stay at maximum for short time to add a pattern for user interaction */ + case DETECT_MAXIMA: /* stay at maximum for short time to add a pattern for user interaction */ + if(fabsf(curSpeed) < MOVE_DELTA_SPEED ) { stableCnt++; } @@ -109,7 +221,7 @@ case DETECT_RISEBACK: case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) { - if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END) + if(fabsf(startPitch - currentPitch) < PITCH_DELTA_END) { detectionState++; }