Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 359:4258ea9b67fa MotionDetection

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