Mercurial > public > ostc4

annotate 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
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 STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
14 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
15 #define MOVE_DELTA_SPEED 4 /* Delta speed needed to identify a valid movement */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
16 #define SHAKE_DELTA_COUNT 10 /* Delta count needed to identify a valid minima / maxima */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
17 #define SHAKE_DELTA_END 10 /* Delta allowed between start and end position */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
18
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
19 static detectionState_t detectionState = DETECT_NOTHING;
362
7b8c87a39c0e Parameter update
Ideenmodellierer
parents: 359
diff changeset
20
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
21 /* 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
22 /* 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
23 detectionState_t detectShake(float currentPitch)
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
24 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
25 static uint8_t stableCnt = 0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
26 static float lastPitch = 0.0;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
27 static float startPitch = 0.0;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
28 static float minmax = 0.0;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
29 float curSpeed;
359
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 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
32 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
33 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
34 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
35
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
36 curSpeed = currentPitch - lastPitch;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
37
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
38 /* feed value into state machine */
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
39 switch (detectionState)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
40 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
41 case DETECT_NOTHING: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) /* detect a stable condition before evaluating for the next move */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
42 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
43 stableCnt++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
44 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
45 if(stableCnt > STABLE_STATE_COUNT)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
46 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
47 detectionState = DETECT_START;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
48 stableCnt = 0;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
49 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
50 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
51 case DETECT_START: if(fabsf(curSpeed) > MOVE_DELTA_SPEED)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
52 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
53 if(curSpeed > 0)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
54 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
55 detectionState = DETECT_POS_MOVE;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
56 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
57 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
58 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
59 detectionState = DETECT_NEG_MOVE;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
60 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
61 stableCnt = 0;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
62 startPitch = lastPitch;
359
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 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
65 case DETECT_NEG_MOVE:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
66 case DETECT_POS_MOVE: if(fabsf(curSpeed) > MOVE_DELTA_SPEED )
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
67 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
68 stableCnt++;
359
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 else
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
71 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
72 if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
73 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
74 if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
75 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
76 detectionState++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
77 minmax = lastPitch;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
78 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
79 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
80 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
81 detectionState = DETECT_NOTHING;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
82 }
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
83 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
84 else
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
85 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
86 detectionState = DETECT_NOTHING;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
87 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
88 stableCnt = 0;
359
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 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
91 case DETECT_MINIMA:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
92 case DETECT_MAXIMA: if(fabsf(currentPitch - minmax ) < SHAKE_DELTA_COUNT) /* stay at maximum for short time to add a pattern for user interaction */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
93 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
94 stableCnt++;
359
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 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
98 if(stableCnt > 0)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
99 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
100 detectionState++;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
101 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
102 else
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
103 {
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
104 detectionState = DETECT_NOTHING;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
105 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
106 stableCnt = 0;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
107 }
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
108 break;
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
109 case DETECT_RISEBACK:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
110 case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
111 {
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
112 if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
113 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
114 detectionState++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
115 }
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
116 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
117 stableCnt++;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
118 break;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
119 default:
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
120 detectionState = DETECT_NOTHING;
359
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
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
123 }
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
124 if(stableCnt > STABLE_STATE_TIMEOUT)
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
125 {
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
126 detectionState = DETECT_NOTHING;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
127 stableCnt = 0;
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
128 }
359
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 lastPitch = currentPitch;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
131 return detectionState;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
132 }