annotate Discovery/Src/motion.c @ 366:3c7030d6d67a MotionDetection

Fix misalignment of do while loop
author ideenmodellierer
date Mon, 17 Jun 2019 19:56:38 +0200
parents bdf978d2a5d4
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 }