Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 370:77cdfbdaca8c MotionDetection

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
author ideenmodellierer
date Tue, 13 Aug 2019 21:13:54 +0200
parents bdf978d2a5d4
children fca370f847f8
comparison
equal deleted inserted replaced
369:210bffc496a3 370:77cdfbdaca8c
7 7
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 #include "data_central.h"
13 #include "t7.h"
12 14
13 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ 15 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
14 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */ 16 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
15 #define MOVE_DELTA_SPEED 4 /* Delta speed needed to identify a valid movement */ 17 #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 */ 18 #define PITCH_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 */ 19 #define PITCH_DELTA_END 10 /* Delta allowed between start and end position */
20
21
22 #define SECTOR_WINDOW 40.0 /* Pitch window which is used for custom view projection */
23 #define SECTOR_HYSTERY 3 /* Additional offset to avoid fast changing displays */
24 #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */
25 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
26
18 27
19 static detectionState_t detectionState = DETECT_NOTHING; 28 static detectionState_t detectionState = DETECT_NOTHING;
20 29
21 /* Detect if user is generating an pitch including return to starting position (shake) */ 30 static uint8_t curSector;
31 static uint8_t targetSector;
32 static uint8_t sectorSize;
33 static uint8_t sectorCount;
34
35 SSector PitchSector[10]; /* max number of enabled custom views */
36
37
38 uint8_t GetSectorForPitch(float pitch)
39 {
40 static float lastPitch = 1000;
41 float newPitch;
42 uint8_t index;
43 uint8_t sector = 0;
44
45 if(lastPitch == 1000) /* init at first call */
46 {
47 lastPitch = pitch;
48 }
49
50 newPitch = lastPitch + (pitch / SECTOR_FILTER);
51
52 for(index = 1; index < sectorCount; index++)
53 {
54 if((pitch < PitchSector[index].upperlimit) && (pitch > PitchSector[index].lowerlimit ))
55 {
56 sector = index;
57 break;
58 }
59 }
60 lastPitch = newPitch;
61 return sector;
62 }
63
64 void DefinePitchSectors(float centerPitch)
65 {
66 uint8_t index;
67
68 sectorCount = t7_GetEnabled_customviews();
69 sectorSize = SECTOR_WINDOW / sectorCount;
70
71 PitchSector[0].upperlimit = centerPitch + (SECTOR_WINDOW / 2);
72 PitchSector[0].lowerlimit = PitchSector[0].upperlimit - sectorSize - SECTOR_HYSTERY;
73
74 for(index = 1; index < sectorCount; index++)
75 {
76 PitchSector[index].upperlimit = PitchSector[0].upperlimit - index * sectorSize + SECTOR_HYSTERY;
77 PitchSector[index].lowerlimit = PitchSector[0].upperlimit - (index + 1) * sectorSize - SECTOR_HYSTERY;
78 }
79
80 PitchSector[0].upperlimit = SECTOR_BORDER;
81 PitchSector[index - 1].lowerlimit = SECTOR_BORDER * -1.0;
82
83 /* get the current sector */
84 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
85 targetSector = curSector;
86 }
87
88 void InitMotionDetection(void)
89 {
90 targetSector = 0;
91 curSector = 0;
92 sectorSize = 0;
93 sectorCount = 0;
94 DefinePitchSectors(0);
95 }
96
97 /* Map the current pitch value to a sector and create button event in case the sector is left */
98 detectionState_t detectSectorButtonEvent(float curPitch)
99 {
100 static uint8_t lastTargetSector = 0;
101 uint8_t newTargetSector;
102 uint8_t PitchEvent = DETECT_NOTHING;
103
104 /* only change sector if reading is stable */
105 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
106 if(lastTargetSector == newTargetSector)
107 {
108 targetSector = newTargetSector;
109 }
110 lastTargetSector = newTargetSector;
111 if(targetSector != curSector)
112 {
113 if(targetSector > curSector)
114 {
115 curSector++;
116 PitchEvent = DETECT_POS_PITCH;
117 }
118 else
119 {
120 curSector--;
121 PitchEvent = DETECT_NEG_PITCH;
122 }
123 }
124 return PitchEvent;
125 }
126
127
128 /* Detect if user is generating an pitch including return to starting position */
22 /* This is done by feeding the past movements value per value into a state machine */ 129 /* This is done by feeding the past movements value per value into a state machine */
23 detectionState_t detectShake(float currentPitch) 130 detectionState_t detectPitch(float currentPitch)
24 { 131 {
25 static uint8_t stableCnt = 0; 132 static uint8_t stableCnt = 0;
26 static float lastPitch = 0.0; 133 static float lastPitch = 0.0;
27 static float startPitch = 0.0; 134 static float startPitch = 0.0;
28 static float minmax = 0.0;
29 float curSpeed; 135 float curSpeed;
30 136
31 if((detectionState == DETECT_NEG_SHAKE) || (detectionState == DETECT_POS_SHAKE)) /* discard last detection */ 137
138 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */
32 { 139 {
33 detectionState = DETECT_NOTHING; 140 detectionState = DETECT_NOTHING;
34 } 141 }
35 142
36 curSpeed = currentPitch - lastPitch; 143 curSpeed = currentPitch - lastPitch;
40 { 147 {
41 case DETECT_NOTHING: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) /* detect a stable condition before evaluating for the next move */ 148 case DETECT_NOTHING: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) /* detect a stable condition before evaluating for the next move */
42 { 149 {
43 stableCnt++; 150 stableCnt++;
44 } 151 }
152 else
153 {
154 stableCnt=0;
155 }
156
45 if(stableCnt > STABLE_STATE_COUNT) 157 if(stableCnt > STABLE_STATE_COUNT)
46 { 158 {
47 detectionState = DETECT_START; 159 detectionState = DETECT_START;
48 stableCnt = 0; 160 stableCnt = 0;
49 } 161 }
69 } 181 }
70 else 182 else
71 { 183 {
72 if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */ 184 if(stableCnt >= STABLE_STATE_COUNT) /* debounce movement */
73 { 185 {
74 if(fabsf(startPitch - currentPitch) > SHAKE_DELTA_COUNT) 186 if(fabsf(startPitch - currentPitch) > PITCH_DELTA_COUNT)
75 { 187 {
76 detectionState++; 188 detectionState++;
77 minmax = lastPitch;
78 } 189 }
79 else 190 else
80 { 191 {
81 detectionState = DETECT_NOTHING; 192 detectionState = DETECT_NOTHING;
82 } 193 }
87 } 198 }
88 stableCnt = 0; 199 stableCnt = 0;
89 } 200 }
90 break; 201 break;
91 case DETECT_MINIMA: 202 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 */ 203 case DETECT_MAXIMA: /* stay at maximum for short time to add a pattern for user interaction */
204 if(fabsf(curSpeed) < MOVE_DELTA_SPEED )
93 { 205 {
94 stableCnt++; 206 stableCnt++;
95 } 207 }
96 else 208 else
97 { 209 {
107 } 219 }
108 break; 220 break;
109 case DETECT_RISEBACK: 221 case DETECT_RISEBACK:
110 case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED) 222 case DETECT_FALLBACK: if(fabsf(curSpeed) < MOVE_DELTA_SPEED)
111 { 223 {
112 if(fabsf(startPitch - currentPitch) < SHAKE_DELTA_END) 224 if(fabsf(startPitch - currentPitch) < PITCH_DELTA_END)
113 { 225 {
114 detectionState++; 226 detectionState++;
115 } 227 }
116 } 228 }
117 stableCnt++; 229 stableCnt++;