Mercurial > public > ostc4

annotate Discovery/Src/motion.c @ 360:fc5e9fdcb156 MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added trigger of middle button action in case of a detected shake
author ideenmodellierer
date Fri, 24 May 2019 22:01:46 +0200
parents 4258ea9b67fa
children 7b8c87a39c0e
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
1 /*
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
2 * motion.c
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
3 *
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
4 * Created on: 20.05.2019
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
5 * Author: Thorsten Sonntag
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
6 */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
7
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
8 #include <stdint.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
9 #include <string.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
10 #include <math.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
11 #include "motion.h"
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
12
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
13 #define PITCH_HISTORY_ENTRIES 20 /* number of pitch value stored in buffer */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
14 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
15 #define MOVE_DELTA_COUNT 5 /* Delta count needed to identify a valid movement */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
16 #define SHAKE_DELTA_COUNT 30 /* Delta count needed to identify a valid minima / maxima */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
17
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
18
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
19 typedef enum
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
20 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
21 RELATIVE_MOVE_STATIC = 0,
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
22 RELATIVE_MOVE_INCREASE,
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
23 RELATIVE_MOVE_DECREASE,
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
24 RELATIVE_MOVE_INVALID
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
25 } relativeMove_t;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
26
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
27
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
28
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
29
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
30
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
31 float pitchHistory[PITCH_HISTORY_ENTRIES]; /* Ringbuffer of last read pich values */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
32 static uint8_t pitchWriteIdx; /* Index of current write slot */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
33
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
34 /* Init data structures */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
35 void InitMotion()
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
36 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
37 uint8_t tmp;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
38
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
39 for(tmp = 0; tmp < PITCH_HISTORY_ENTRIES; tmp++)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
40 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
41 pitchHistory[tmp] = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
42 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
43 pitchWriteIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
44
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
45 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
46
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
47 /* Detect if user is generating an pitch including return to starting position (shake) */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
48 /* This is done by feeding the past movements value per value into a state machine */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
49 detectionState_t detectShake(float currentPitch)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
50 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
51 static uint8_t runningIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
52 static uint8_t stableCnt = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
53 static float lastPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
54 static float startPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
55
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
56 relativeMove_t relativeMove = RELATIVE_MOVE_INVALID;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
57 static detectionState_t detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
58
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
59 pitchHistory[pitchWriteIdx] = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
60 runningIdx = pitchWriteIdx;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
61 #if 0
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
62 runningIdx = pitchWriteIdx + 1;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
63
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
64 if(runningIdx == PITCH_HISTORY_ENTRIES)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
65 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
66 runningIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
67 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
68 #endif
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
69
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
70 if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE)) /* discard last detection */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
71 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
72 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
73 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
74 // do
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
75 // {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
76 // lastPitch = pitchHistory[runningIdx];
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
77 #if 0
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
78 runningIdx++;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
79 if(runningIdx == PITCH_HISTORY_ENTRIES)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
80 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
81 runningIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
82 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
83 #endif
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
84 /* define relative movement compared to last position */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
85 if(fabsf(lastPitch - pitchHistory[runningIdx]) < MOVE_DELTA_COUNT ) /* more or less no movement */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
86 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
87 relativeMove = RELATIVE_MOVE_STATIC;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
88 stableCnt++;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
89 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
90 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
91 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
92 if(lastPitch > pitchHistory[runningIdx]) /* decreasing */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
93 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
94 relativeMove = RELATIVE_MOVE_DECREASE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
95 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
96 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
97 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
98 relativeMove = RELATIVE_MOVE_INCREASE; /* increasing */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
99 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
100 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
101
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
102 /* feed value into statemachine */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
103 switch (detectionState)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
104 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
105 case DETECT_NOTHING: if(relativeMove == RELATIVE_MOVE_STATIC)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
106 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
107 if(stableCnt > 4)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
108 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
109 detectionState = DETECT_START;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
110 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
111 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
112 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
113 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
114 case DETECT_START: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
115 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
116 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
117 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
118 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
119 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
120 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
121 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
122 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
123 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
124 case DETECT_POS_MOVE: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
125 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
126 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
127 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
128 case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
129 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
130 detectionState = DETECT_MAXIMA;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
131 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
132 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
133 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
134 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
135 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
136 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
137 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
138 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
139 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
140 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
141 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
142 case DETECT_NEG_MOVE: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
143 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
144 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
145 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
146 case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT) /* significant movment */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
147 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
148 detectionState = DETECT_MINIMA;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
149 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
150 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
151 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
152 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
153 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
154 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
155 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
156 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
157 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
158 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
159 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
160 case DETECT_MAXIMA: if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
161 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
162 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
163 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
164 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
165 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
166 if(relativeMove == RELATIVE_MOVE_DECREASE)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
167 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
168 detectionState = DETECT_FALLBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
169 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
170 if(relativeMove == RELATIVE_MOVE_INCREASE)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
171 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
172 detectionState = DETECT_POS_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
173 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
174 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
175 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
176 case DETECT_MINIMA: if((relativeMove != RELATIVE_MOVE_STATIC) && (stableCnt < STABLE_STATE_COUNT ))
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
177 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
178 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
179 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
180 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
181 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
182 if(relativeMove == RELATIVE_MOVE_DECREASE)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
183 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
184 detectionState = DETECT_NEG_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
185 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
186 if(relativeMove == RELATIVE_MOVE_INCREASE)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
187 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
188 detectionState = DETECT_RISEBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
189 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
190 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
191 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
192
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
193 case DETECT_FALLBACK: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
194 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
195 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_FALLBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
196 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
197 case RELATIVE_MOVE_STATIC: if( stableCnt >= STABLE_STATE_COUNT)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
198 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
199 if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT) /* are we where started, again? */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
200 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
201 detectionState = DETECT_POS_SHAKE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
202 memset(pitchHistory, 0, sizeof(pitchHistory));
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
203 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
204 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
205 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
206 detectionState = DETECT_START; /* start new detection */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
207 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
208 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
209 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
210 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
211 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
212 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
213 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
214 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
215 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
216 case DETECT_RISEBACK: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
217 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
218 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_RISEBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
219 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
220 case RELATIVE_MOVE_STATIC: if(stableCnt >= STABLE_STATE_COUNT)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
221 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
222 if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
223 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
224 detectionState = DETECT_NEG_SHAKE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
225 memset(pitchHistory, 0, sizeof(pitchHistory));
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
226 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
227 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
228 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
229 detectionState = DETECT_START;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
230 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
231 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
232 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
233 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
234 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
235 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
236 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
237 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
238 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
239
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
240 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
241 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
242
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
243 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
244 if(relativeMove != RELATIVE_MOVE_STATIC) /* reset counter for stable time detection */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
245 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
246 stableCnt = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
247 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
248 // } while ((runningIdx != pitchWriteIdx) && (detectionState != DETECT_NEG_SHAKE) && (detectionState != DETECT_POS_SHAKE));
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
249
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
250 lastPitch = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
251 pitchWriteIdx++;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
252 if(pitchWriteIdx == PITCH_HISTORY_ENTRIES)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
253 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
254 pitchWriteIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
255 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
256 return detectionState;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
257 }