Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 363:bdf978d2a5d4 MotionDetection

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