Mercurial > public > ostc4

annotate Discovery/Src/motion.c @ 362:7b8c87a39c0e MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Parameter update
author Ideenmodellierer
date Tue, 11 Jun 2019 05:31:07 +0200
parents 4258ea9b67fa
children bdf978d2a5d4
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) */
362
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
15 #define MOVE_DELTA_COUNT 10 /* Delta count needed to identify a valid movement */
359
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
362
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
47 detectionState_t detectionState = DETECT_NOTHING;
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
48
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
49 /* 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
50 /* 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
51 detectionState_t detectShake(float currentPitch)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
52 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
53 static uint8_t runningIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
54 static uint8_t stableCnt = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
55 static float lastPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
56 static float startPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
57
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
58 relativeMove_t relativeMove = RELATIVE_MOVE_INVALID;
362
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
59
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
60
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
61 pitchHistory[pitchWriteIdx] = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
62 runningIdx = pitchWriteIdx;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
63 #if 0
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
64 runningIdx = pitchWriteIdx + 1;
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 if(runningIdx == PITCH_HISTORY_ENTRIES)
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 runningIdx = 0;
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 #endif
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 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
73 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
74 detectionState = DETECT_NOTHING;
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 // do
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
77 // {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
78 // lastPitch = pitchHistory[runningIdx];
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
79 #if 0
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
80 runningIdx++;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
81 if(runningIdx == PITCH_HISTORY_ENTRIES)
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 runningIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
84 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
85 #endif
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
86 /* define relative movement compared to last position */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
87 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
88 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
89 relativeMove = RELATIVE_MOVE_STATIC;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
90 stableCnt++;
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 else
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 if(lastPitch > pitchHistory[runningIdx]) /* decreasing */
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 relativeMove = RELATIVE_MOVE_DECREASE;
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 else
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 relativeMove = RELATIVE_MOVE_INCREASE; /* increasing */
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 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
103
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
104 /* feed value into statemachine */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
105 switch (detectionState)
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 case DETECT_NOTHING: if(relativeMove == RELATIVE_MOVE_STATIC)
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 if(stableCnt > 4)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
110 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
111 detectionState = DETECT_START;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
112 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
113 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
114 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
115 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
116 case DETECT_START: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
117 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
118 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_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 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
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 default:
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 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
125 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
126 case DETECT_POS_MOVE: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
127 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
128 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_POS_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
129 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
130 case RELATIVE_MOVE_STATIC: if(fabsf(startPitch - lastPitch) > SHAKE_DELTA_COUNT)
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 detectionState = DETECT_MAXIMA;
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 else
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 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
137 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
138 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
139 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
140 detectionState = DETECT_NOTHING;
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 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
143 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
144 case DETECT_NEG_MOVE: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
145 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
146 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_NEG_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
147 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
148 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
149 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
150 detectionState = DETECT_MINIMA;
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 else
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 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
155 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
156 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
157 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
158 detectionState = DETECT_NOTHING;
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 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
161 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
162 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
163 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
164 detectionState = DETECT_NOTHING;
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 else
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 if(relativeMove == RELATIVE_MOVE_DECREASE)
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 detectionState = DETECT_FALLBACK;
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 if(relativeMove == RELATIVE_MOVE_INCREASE)
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 detectionState = DETECT_POS_MOVE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
175 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
176 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
177 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
178 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
179 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
180 detectionState = DETECT_NOTHING;
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 else
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 if(relativeMove == RELATIVE_MOVE_DECREASE)
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 detectionState = DETECT_NEG_MOVE;
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 if(relativeMove == RELATIVE_MOVE_INCREASE)
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 detectionState = DETECT_RISEBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
191 }
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 break;
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 DETECT_FALLBACK: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
196 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
197 case RELATIVE_MOVE_DECREASE: detectionState = DETECT_FALLBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
198 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
199 case RELATIVE_MOVE_STATIC: if( stableCnt >= STABLE_STATE_COUNT)
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 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
202 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
203 detectionState = DETECT_POS_SHAKE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
204 memset(pitchHistory, 0, sizeof(pitchHistory));
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 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
207 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
208 detectionState = DETECT_START; /* start new detection */
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
209 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
210 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
211 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
212 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
213 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
214 detectionState = DETECT_NOTHING;
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 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
217 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
218 case DETECT_RISEBACK: switch(relativeMove)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
219 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
220 case RELATIVE_MOVE_INCREASE: detectionState = DETECT_RISEBACK;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
221 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
222 case RELATIVE_MOVE_STATIC: if(stableCnt >= STABLE_STATE_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 if(fabsf(startPitch - lastPitch) < MOVE_DELTA_COUNT)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
225 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
226 detectionState = DETECT_NEG_SHAKE;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
227 memset(pitchHistory, 0, sizeof(pitchHistory));
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 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
230 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
231 detectionState = DETECT_START;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
232 startPitch = lastPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
233 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
234 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
235 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
236 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
237 detectionState = DETECT_NOTHING;
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 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
241
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
242 default:
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
243 break;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
244
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 if(relativeMove != RELATIVE_MOVE_STATIC) /* reset counter for stable time detection */
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 stableCnt = 0;
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 // } while ((runningIdx != pitchWriteIdx) && (detectionState != DETECT_NEG_SHAKE) && (detectionState != DETECT_POS_SHAKE));
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
251
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
252 lastPitch = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
253 pitchWriteIdx++;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
254 if(pitchWriteIdx == PITCH_HISTORY_ENTRIES)
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 pitchWriteIdx = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
257 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
258 return detectionState;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
259 }