# HG changeset patch # User ideenmodellierer # Date 1560793578 -7200 # Node ID bdf978d2a5d425182153fc973ae664ab4d61d4a9 # Parent 7b8c87a39c0eb426f2339f442f9187982bd287b9 Reworked detection function The first implementation was not stable enough during underwater tests => redesign taking speed deltas into account diff -r 7b8c87a39c0e -r bdf978d2a5d4 Discovery/Inc/motion.h --- a/Discovery/Inc/motion.h Tue Jun 11 05:31:07 2019 +0200 +++ b/Discovery/Inc/motion.h Mon Jun 17 19:46:18 2019 +0200 @@ -14,18 +14,16 @@ { DETECT_START = 0, DETECT_POS_MOVE, + DETECT_MAXIMA, + DETECT_FALLBACK, + DETECT_POS_SHAKE, DETECT_NEG_MOVE, - DETECT_MAXIMA, DETECT_MINIMA, - DETECT_FALLBACK, DETECT_RISEBACK, - DETECT_POS_SHAKE, DETECT_NEG_SHAKE, DETECT_NOTHING } detectionState_t; - -void InitMotion(void); detectionState_t detectShake(float currentPitch); #endif /* INC_MOTION_H_ */ diff -r 7b8c87a39c0e -r bdf978d2a5d4 Discovery/Src/motion.c --- a/Discovery/Src/motion.c Tue Jun 11 05:31:07 2019 +0200 +++ b/Discovery/Src/motion.c Mon Jun 17 19:46:18 2019 +0200 @@ -10,250 +10,123 @@ #include #include "motion.h" -#define PITCH_HISTORY_ENTRIES 20 /* number of pitch value stored in buffer */ #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ -#define MOVE_DELTA_COUNT 10 /* Delta count needed to identify a valid movement */ -#define SHAKE_DELTA_COUNT 30 /* Delta count needed to identify a valid minima / maxima */ - - -typedef enum -{ - RELATIVE_MOVE_STATIC = 0, - RELATIVE_MOVE_INCREASE, - RELATIVE_MOVE_DECREASE, - RELATIVE_MOVE_INVALID -} relativeMove_t; - - - - +#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 */ -float pitchHistory[PITCH_HISTORY_ENTRIES]; /* Ringbuffer of last read pich values */ -static uint8_t pitchWriteIdx; /* Index of current write slot */ - -/* Init data structures */ -void InitMotion() -{ - uint8_t tmp; - - for(tmp = 0; tmp < PITCH_HISTORY_ENTRIES; tmp++) - { - pitchHistory[tmp] = 0; - } - pitchWriteIdx = 0; - -} - -detectionState_t detectionState = DETECT_NOTHING; +static detectionState_t detectionState = DETECT_NOTHING; /* Detect if user is generating an pitch including return to starting position (shake) */ /* This is done by feeding the past movements value per value into a state machine */ detectionState_t detectShake(float currentPitch) { - static uint8_t runningIdx = 0; static uint8_t stableCnt = 0; static float lastPitch = 0.0; static float startPitch = 0.0; - - relativeMove_t relativeMove = RELATIVE_MOVE_INVALID; - - - pitchHistory[pitchWriteIdx] = currentPitch; - runningIdx = pitchWriteIdx; -#if 0 - runningIdx = pitchWriteIdx + 1; - - if(runningIdx == PITCH_HISTORY_ENTRIES) - { - runningIdx = 0; - } -#endif + static float minmax = 0.0; + float curSpeed; if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE)) /* discard last detection */ { detectionState = DETECT_NOTHING; } -// do -// { -// lastPitch = pitchHistory[runningIdx]; -#if 0 - runningIdx++; - if(runningIdx == PITCH_HISTORY_ENTRIES) - { - runningIdx = 0; - } -#endif - /* define relative movement compared to last position */ - if(fabsf(lastPitch - pitchHistory[runningIdx]) < MOVE_DELTA_COUNT ) /* more or less no movement */ - { - relativeMove = RELATIVE_MOVE_STATIC; - stableCnt++; - } - else - { - if(lastPitch > pitchHistory[runningIdx]) /* decreasing */ - { - relativeMove = RELATIVE_MOVE_DECREASE; - } - else - { - relativeMove = RELATIVE_MOVE_INCREASE; /* increasing */ - } - } + + curSpeed = currentPitch - lastPitch; - /* feed value into statemachine */ - switch (detectionState) - { - case DETECT_NOTHING: if(relativeMove == RELATIVE_MOVE_STATIC) + /* feed value into state machine */ + switch (detectionState) + { + case DETECT_NOTHING: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) /* detect a stable condition before evaluating for the next move */ { - if(stableCnt > 4) - { - detectionState = DETECT_START; - startPitch = lastPitch; - } + stableCnt++; + } + if(stableCnt > STABLE_STATE_COUNT) + { + detectionState = DETECT_START; + stableCnt = 0; } break; - case DETECT_START: switch(relativeMove) + case DETECT_START: if(fabsf(curSpeed) > MOVE_DELTA_SPEED) { - case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE; - break; - case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE; - break; - default: - break; + if(curSpeed > 0) + { + detectionState = DETECT_POS_MOVE; + } + else + { + detectionState = DETECT_NEG_MOVE; + } + stableCnt = 0; + startPitch = lastPitch; } break; - case DETECT_POS_MOVE: switch(relativeMove) - { - case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE; - break; - case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT) - { - detectionState = DETECT_MAXIMA; - } - else - { - detectionState = DETECT_NOTHING; - } - break; - default: - detectionState = DETECT_NOTHING; - break; - } - break; - case DETECT_NEG_MOVE: switch(relativeMove) + case DETECT_NEG_MOVE: + case DETECT_POS_MOVE: if(fabsf(curSpeed) > MOVE_DELTA_SPEED ) { - case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE; - break; - case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT) /* significant movment */ - { - detectionState = DETECT_MINIMA; - } - else - { - detectionState = DETECT_NOTHING; - } - break; - default: - detectionState = DETECT_NOTHING; - break; - } - break; - case DETECT_MAXIMA: if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT )) - { - detectionState = DETECT_NOTHING; + stableCnt++; } else { - if(relativeMove == RELATIVE_MOVE_DECREASE) + if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */ { - detectionState = DETECT_FALLBACK; + if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT) + { + detectionState++; + minmax = lastPitch; + } + else + { + detectionState = DETECT_NOTHING; + } } - if(relativeMove == RELATIVE_MOVE_INCREASE) + else { - detectionState = DETECT_POS_MOVE; + detectionState = DETECT_NOTHING; } + stableCnt = 0; } break; - case DETECT_MINIMA: if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT )) + 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 */ { - detectionState = DETECT_NOTHING; + stableCnt++; } else { - if(relativeMove == RELATIVE_MOVE_DECREASE) + if(stableCnt > 0) { - detectionState = DETECT_NEG_MOVE; - } - if(relativeMove == RELATIVE_MOVE_INCREASE) - { - detectionState = DETECT_RISEBACK; + detectionState++; } - } - break; - - case DETECT_FALLBACK: switch(relativeMove) - { - case RELATIVE_MOVE_DECREASE: detectionState = DETECT_FALLBACK; - break; - case RELATIVE_MOVE_STATIC: if( stableCnt >= STABLE_STATE_COUNT) - { - if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT) /* are we where started, again? */ - { - detectionState = DETECT_POS_SHAKE; - memset(pitchHistory, 0, sizeof(pitchHistory)); - } - else - { - detectionState = DETECT_START; /* start new detection */ - startPitch = lastPitch; - } - } - break; - default: + else + { detectionState = DETECT_NOTHING; - break; + } + stableCnt = 0; } break; - case DETECT_RISEBACK: switch(relativeMove) + case DETECT_RISEBACK: + case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) { - case RELATIVE_MOVE_INCREASE: detectionState = DETECT_RISEBACK; - break; - case RELATIVE_MOVE_STATIC: if(stableCnt >= STABLE_STATE_COUNT) - { - if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT) - { - detectionState = DETECT_NEG_SHAKE; - memset(pitchHistory, 0, sizeof(pitchHistory)); - } - else - { - detectionState = DETECT_START; - startPitch = lastPitch; - } - } - break; - default: - detectionState = DETECT_NOTHING; - break; + if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END) + { + detectionState++; + } } + stableCnt++; + break; + default: + detectionState = DETECT_NOTHING; break; - default: - break; - - } - if(relativeMove != RELATIVE_MOVE_STATIC) /* reset counter for stable time detection */ - { - stableCnt = 0; - } -// } while ((runningIdx != pitchWriteIdx) && (detectionState != DETECT_NEG_SHAKE) && (detectionState != DETECT_POS_SHAKE)); + } + if(stableCnt > STABLE_STATE_TIMEOUT) + { + detectionState = DETECT_NOTHING; + stableCnt = 0; + } lastPitch = currentPitch; - pitchWriteIdx++; - if(pitchWriteIdx == PITCH_HISTORY_ENTRIES) - { - pitchWriteIdx = 0; - } return detectionState; }