Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 387:0dbb74be972f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merged in Ideenmodellierer/ostc4/MotionDetection (pull request #34) MotionDetection
author heinrichsweikamp <bitbucket@heinrichsweikamp.com>
date Sun, 24 Nov 2019 15:46:58 +0000
parents 0cd862e501f6
children e3237f580ae9
comparison
equal deleted inserted replaced
358:c6a084d1433f 387:0dbb74be972f
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 <stdlib.h>
11 #include <math.h>
12 #include "motion.h"
13 #include "data_central.h"
14 #include "t7.h"
15 #include "t3.h"
16 #include "settings.h"
17
18 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */
19 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
20
21 #define SECTOR_WINDOW 80.0 /* Pitch window which is used for custom view projection */
22 #define SECTOR_WINDOW_MAX 120.0 /* Pitch window which will be greater than the divers field of view */
23 #define SECTOR_HYSTERY 2 /* 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
27 #define SECTOR_MAX 24 /* maximum number of sectors */
28 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
29
30 detectionState_t detectionState = DETECT_NOTHING;
31 SSector sectorDetection;
32
33
34 uint8_t GetSectorForPitch(float pitch)
35 {
36 static uint8_t lastsector = 0;
37 float newPitch;
38 uint8_t sector = 0;
39
40 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */
41 if (newPitch < 0.0) /* clip value */
42 {
43 newPitch = 0.0;
44 }
45 if (newPitch > sectorDetection.window) /* clip value */
46 {
47 newPitch = sectorDetection.window;
48 }
49
50 /* switch to other sector? */
51 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
52 {
53 sector = (uint16_t) newPitch / sectorDetection.size;
54 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
55 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
56 lastsector = sector;
57 }
58
59 return lastsector;
60 }
61
62 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
63 {
64 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
65 {
66 if(settingsGetPointer()->design == 3) /* Big font view ? */
67 {
68 sectorDetection.count = t3_GetEnabled_customviews();
69 }
70 else
71 {
72 sectorDetection.count = t7_GetEnabled_customviews();
73 }
74 if(sectorDetection.count > 7)
75 {
76 sectorDetection.count = 7; /* more views are hard to manually control */
77 }
78 }
79 else
80 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
81 {
82 sectorDetection.count = numOfSectors;
83 }
84
85 if(sectorDetection.count == SECTOR_MAX)
86 {
87 sectorDetection.window = SECTOR_WINDOW_MAX;
88 }
89 else
90 {
91 sectorDetection.window = SECTOR_WINDOW;
92 }
93
94 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
95 sectorDetection.size = sectorDetection.window / sectorDetection.count;
96 sectorDetection.center = 0;
97
98 /* reset border values */
99 sectorDetection.lowerborder = SECTOR_BORDER;
100 sectorDetection.upperborder = SECTOR_BORDER * -1.0;
101 /* get the current sector */
102 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
103 sectorDetection.target = sectorDetection.current;
104 /* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
105 sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
106
107 }
108
109 void InitMotionDetection(void)
110 {
111 sectorDetection.target = 0;
112 sectorDetection.current = 0;
113 sectorDetection.size = 0;
114 sectorDetection.count = 0;
115
116 switch(settingsGetPointer()->MotionDetection)
117 {
118 case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS);
119 break;
120 case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX);
121 break;
122 case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL);
123 break;
124 default:
125 break;
126 }
127
128 }
129
130 /* Map the current pitch value to a sector and create button event in case the sector is left */
131 detectionState_t detectSectorButtonEvent(float curPitch)
132 {
133 static uint8_t lastTargetSector = 0;
134 uint8_t newTargetSector;
135 uint8_t PitchEvent = DETECT_NOTHING;
136
137 /* only change sector if reading is stable */
138 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
139 if(lastTargetSector == newTargetSector)
140 {
141 sectorDetection.target = newTargetSector;
142 }
143 lastTargetSector = newTargetSector;
144 if(sectorDetection.target != sectorDetection.current)
145 {
146 if(sectorDetection.target > sectorDetection.current)
147 {
148 sectorDetection.current++;
149 PitchEvent = DETECT_POS_PITCH;
150 }
151 else
152 {
153 sectorDetection.current--;
154 PitchEvent = DETECT_NEG_PITCH;
155 }
156 }
157 return PitchEvent;
158 }
159
160 /* Check if pitch is not in center position and trigger a button action if needed */
161 detectionState_t detectScrollButtonEvent(float curPitch)
162 {
163 static uint8_t delayscroll = 0; /* slow down the number of scroll events */
164
165 uint8_t PitchEvent = DETECT_NOTHING;
166 uint8_t newSector;
167
168 if(delayscroll == 0)
169 {
170 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
171 /* for scroll detection the motion windoe is split into 6 sectors => set event accoring to the sector number*/
172 switch(newSector)
173 {
174 case 0:
175 case 1: PitchEvent = DETECT_POS_PITCH;
176 break;
177 case 5:
178 case 6: PitchEvent = DETECT_NEG_PITCH;
179 break;
180 default:
181 break;
182 }
183 if(PitchEvent != DETECT_NOTHING)
184 {
185 delayscroll = 5;
186 }
187 }
188 else
189 {
190 delayscroll--;
191 }
192 return PitchEvent;
193 }
194
195
196 /* Detect if user is generating an pitch including return to starting position */
197 /* This is done by feeding the past movements value per value into a state machine */
198 detectionState_t detectPitch(float currentPitch)
199 {
200 static uint8_t lastSector = 0;
201 static uint8_t startSector = 0;
202 static uint8_t stableCnt = 0;
203
204 uint8_t curSector;
205
206 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */
207 {
208 detectionState = DETECT_NOTHING;
209 }
210
211 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
212
213 /* feed value into state machine */
214 switch (detectionState)
215 {
216 case DETECT_NOTHING: if(curSector != lastSector) /* detect a stable condition before evaluating for the next move */
217 {
218 stableCnt=0;
219 }
220
221 if(stableCnt > STABLE_STATE_COUNT)
222 {
223 detectionState = DETECT_START;
224 stableCnt = 0;
225 startSector = lastSector;
226 }
227 break;
228 case DETECT_START: if(curSector != lastSector)
229 {
230 if(abs(curSector - startSector) > 1)
231 {
232 if(curSector > lastSector)
233 {
234 detectionState = DETECT_POS_MOVE;
235 }
236 else
237 {
238 detectionState = DETECT_NEG_MOVE;
239 }
240 stableCnt = 0;
241 startSector = lastSector;
242 }
243 }
244 break;
245 case DETECT_NEG_MOVE:
246 case DETECT_POS_MOVE: if(curSector == lastSector) /* Moved to a max? */
247 {
248 if(abs(startSector - curSector) > 2)
249 {
250 detectionState++;
251 stableCnt = 0;
252 }
253 if(stableCnt > 2)
254 {
255 detectionState = DETECT_NOTHING;
256 stableCnt = 0;
257 }
258 }
259 break;
260 case DETECT_MAXIMA:
261 case DETECT_MINIMA: if(curSector != lastSector) /* reset timeout detection */
262 {
263 detectionState++;
264 stableCnt = 0;
265 }
266 break;
267 case DETECT_RISEBACK:
268 case DETECT_FALLBACK:
269 if(curSector == lastSector) /* check if we are back at start position at end of movement */
270 {
271 if(abs(startSector - curSector) <= 1)
272 {
273 if(stableCnt > 2)
274 {
275 detectionState++;
276 stableCnt = 0;
277 }
278 }
279 }
280 break;
281 default:
282 detectionState = DETECT_NOTHING;
283 break;
284 }
285 if(detectionState != DETECT_START)
286 {
287 stableCnt++;
288 }
289 lastSector = curSector;
290 if(stableCnt > STABLE_STATE_TIMEOUT)
291 {
292 detectionState = DETECT_NOTHING;
293 stableCnt = 0;
294 }
295
296 return detectionState;
297 }