Mercurial > public > ostc4

comparison Discovery/Src/motion.c @ 551:e3237f580ae9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added viewport functionality: The viewport shall estimate if the user is looking at the display (in focus) or not. This is done by vector calculation using roll, pitch and yaw angles. The first usecase is to enable the motion detection feature: It is only active if the user is looking at the display. Still in development state => deactivated by compile switch
author Ideenmodellierer
date Sun, 08 Nov 2020 18:43:19 +0100
parents 0cd862e501f6
children 01ee21dd311f
comparison
equal deleted inserted replaced
550:af1c3e3abd5f 551:e3237f580ae9
16 #include "settings.h" 16 #include "settings.h"
17 17
18 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ 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 */ 19 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
20 20
21 #define SECTOR_WINDOW 80.0 /* Pitch window which is used for custom view projection */ 21 #define SECTOR_WINDOW 40.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 */ 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 */ 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 */ 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 */ 25 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
26 26
27 #define SECTOR_MAX 24 /* maximum number of sectors */ 27 #define SECTOR_MAX 24 /* maximum number of sectors */
28 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ 28 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
29 29
30 #define MOTION_DELTA_STABLE 0
31 #define MOTION_DELTA_JITTER 1
32 #define MOTION_DELTA_RAISE 2
33 #define MOTION_DELTA_FALL 3
34
35 #define MOTION_DELTA_JITTER_LEVEL 3.0 /* lower values are considered as stable */
36 #define MOTION_DELTA_RAISE_LEVEL 6.0 /* Movement causing a significant change detected */
37 #define MOTION_DELTA_FALL_LEVEL -6.0 /* Movement causing a significant change detected */
38
39 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
40
30 detectionState_t detectionState = DETECT_NOTHING; 41 detectionState_t detectionState = DETECT_NOTHING;
31 SSector sectorDetection; 42 SSector sectorDetection;
32 43
44 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */
45 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */
46
47 static uint8_t focusCnt = 0;
48 static uint8_t inFocus = 0;
49
50 void resetMotionDeltaHistory()
51 {
52 motionDeltaHistoryIdx = 0;
53 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory));
54 }
55
56 void evaluateMotionDelta(float roll, float pitch, float yaw)
57 {
58 static float lastValue[3] = {0.0,0.0,0.0};
59 uint8_t nextIndex = motionDeltaHistoryIdx + 1;
60 uint8_t axis;
61 float curValue;
62
63 if(nextIndex == MOTION_DELTA_HISTORY_SIZE)
64 {
65 nextIndex = 0;
66 }
67 for(axis=0; axis < 3; axis++)
68 {
69 switch(axis)
70 {
71 case MOTION_HISTORY_ROLL: curValue = roll;
72 break;
73 case MOTION_HISTORY_PITCH: curValue = pitch;
74 break;
75 default:
76 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */
77 {
78 lastValue[MOTION_HISTORY_YAW] -= 360;
79 }
80 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */
81 {
82 lastValue[MOTION_HISTORY_YAW] += 360;
83 }
84 curValue = yaw;
85 break;
86 }
87 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL)
88 {
89 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE;
90 }
91 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL)
92 {
93 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER;
94 }
95 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL)
96 {
97 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE;
98 }
99 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
100 {
101 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
102 }
103 lastValue[axis] = curValue;
104 }
105 motionDeltaHistoryIdx = nextIndex;
106 }
107
108 SDeltaHistory GetDeltaHistory(uint8_t stepback)
109 {
110 uint8_t loop = stepback;
111 uint8_t index = motionDeltaHistoryIdx;
112
113 SDeltaHistory result = {0,0,0};
114
115 if(stepback < MOTION_DELTA_HISTORY_SIZE)
116 {
117 while(loop != 0) /* find requested entry */
118 {
119 loop--;
120 index--;
121 if(index == 0)
122 {
123 index = MOTION_DELTA_HISTORY_SIZE - 1;
124 }
125 }
126 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index];
127 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index];
128 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
129 }
130 return result;
131 }
33 132
34 uint8_t GetSectorForPitch(float pitch) 133 uint8_t GetSectorForPitch(float pitch)
35 { 134 {
36 static uint8_t lastsector = 0; 135 static uint8_t lastsector = 0;
37 float newPitch; 136 float newPitch;
113 sectorDetection.size = 0; 212 sectorDetection.size = 0;
114 sectorDetection.count = 0; 213 sectorDetection.count = 0;
115 214
116 switch(settingsGetPointer()->MotionDetection) 215 switch(settingsGetPointer()->MotionDetection)
117 { 216 {
118 case MOTION_DETECT_SECTOR: DefinePitchSectors(0,CUSTOMER_DEFINED_VIEWS); 217 case MOTION_DETECT_SECTOR: DefinePitchSectors(settingsGetPointer()->viewPitch,CUSTOMER_DEFINED_VIEWS);
119 break; 218 break;
120 case MOTION_DETECT_MOVE: DefinePitchSectors(0,SECTOR_MAX); 219 case MOTION_DETECT_MOVE: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_MAX);
121 break; 220 break;
122 case MOTION_DETECT_SCROLL: DefinePitchSectors(0,SECTOR_SCROLL); 221 case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL);
123 break; 222 break;
124 default: 223 default:
125 break; 224 break;
126 } 225 }
127 226
227 resetMotionDeltaHistory();
128 } 228 }
129 229
130 /* Map the current pitch value to a sector and create button event in case the sector is left */ 230 /* Map the current pitch value to a sector and create button event in case the sector is left */
131 detectionState_t detectSectorButtonEvent(float curPitch) 231 detectionState_t detectSectorButtonEvent(float curPitch)
132 { 232 {
166 uint8_t newSector; 266 uint8_t newSector;
167 267
168 if(delayscroll == 0) 268 if(delayscroll == 0)
169 { 269 {
170 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 270 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*/ 271 /* for scroll detection the motion window is split into 6 sectors => set event accoring to the sector number*/
172 switch(newSector) 272 switch(newSector)
173 { 273 {
174 case 0: 274 case 0:
175 case 1: PitchEvent = DETECT_POS_PITCH; 275 case 1: PitchEvent = DETECT_POS_PITCH;
176 break; 276 break;
195 295
196 /* Detect if user is generating an pitch including return to starting position */ 296 /* 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 */ 297 /* This is done by feeding the past movements value per value into a state machine */
198 detectionState_t detectPitch(float currentPitch) 298 detectionState_t detectPitch(float currentPitch)
199 { 299 {
200 static uint8_t lastSector = 0; 300 uint8_t exit = 0;
201 static uint8_t startSector = 0; 301 uint8_t step = 0;
202 static uint8_t stableCnt = 0; 302 SDeltaHistory test;
203 303
204 uint8_t curSector; 304 detectionState = DETECT_NOTHING;
205 305 while((step != MOTION_DELTA_HISTORY_SIZE) && (!exit)) /* start backward evalution of pitch changes*/
206 if((detectionState == DETECT_NEG_PITCH) || (detectionState == DETECT_POS_PITCH)) /* discard last detection */ 306 {
307 test = GetDeltaHistory(step);
308 step++;
309 switch (detectionState)
310 {
311 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE)
312 {
313 exit = 1;
314 }
315 else
316 {
317 detectionState = DETECT_START;
318 }
319 break;
320 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE)
321 {
322 detectionState = DETECT_POS_MOVE;
323 }
324 if(test.pitch == MOTION_DELTA_FALL)
325 {
326 detectionState = DETECT_NEG_MOVE;
327 }
328 break;
329 case DETECT_NEG_MOVE:
330 case DETECT_POS_MOVE: if(test.pitch <= MOTION_DELTA_JITTER)
331 {
332 detectionState++;
333 }
334 break;
335 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL)
336 {
337 detectionState = DETECT_FALLBACK;
338 }
339 break;
340 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE)
341 {
342 detectionState = DETECT_RISEBACK;
343 }
344 break;
345 case DETECT_RISEBACK:
346 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE)
347 {
348 detectionState++;
349 exit = 1;
350 }
351 break;
352 default:
353 detectionState = DETECT_NOTHING;
354 exit = 1;
355 break;
356 }
357 }
358 if((detectionState != DETECT_POS_PITCH) && (detectionState != DETECT_NEG_PITCH)) /* nothing found */
207 { 359 {
208 detectionState = DETECT_NOTHING; 360 detectionState = DETECT_NOTHING;
209 } 361 }
210 362 else /* dont detect the same event twice */
211 curSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); 363 {
212 364 resetMotionDeltaHistory();
213 /* feed value into state machine */ 365 }
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; 366 return detectionState;
297 } 367 }
368
369 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord)
370 {
371 pCoord->x = ((cosf(yaw) * cosf(pitch)) * pCoord->x + (cosf(yaw)*sinf(pitch)*sinf(roll) - (sinf(yaw)* cosf(roll))) * pCoord->y + (cosf(yaw)*sinf(pitch)*cosf(roll) + sinf(yaw)*sinf(roll)) * pCoord->z);
372 pCoord->y = ((sinf(yaw) * cosf(pitch)) * pCoord->x + (sinf(yaw)*sinf(pitch)*sinf(roll) + cosf(yaw) * cosf(roll)) * pCoord->y + ( sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll))* pCoord->z);
373 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z);
374 }
375
376 SCoord CoordAdd(SCoord cA, SCoord cB)
377 {
378 SCoord result;
379
380 result.x = cA.x + cB.x;
381 result.y = cA.y + cB.y;
382 result.z = cA.z + cB.z;
383 return result;
384 }
385
386 SCoord CoordSub(SCoord cA, SCoord cB)
387 {
388 SCoord result;
389
390 result.x = cA.x - cB.x;
391 result.y = cA.y - cB.y;
392 result.z = cA.z - cB.z;
393 return result;
394 }
395
396 SCoord CoordCross(SCoord cA, SCoord cB)
397 {
398 SCoord result;
399
400 result.x = (cA.y * cB.z) - (cA.z * cB.y);
401 result.y = (cA.z * cB.x) - (cA.x * cB.z);
402 result.z = (cA.x * cB.y) - (cA.y * cB.x);
403
404 return result;
405
406 }
407
408 SCoord CoordMulF(SCoord op, float factor)
409 {
410 SCoord result;
411 result.x = (op.x * factor);
412 result.y = (op.y * factor);
413 result.z = (op.z * factor);
414
415 return result;
416 }
417
418 SCoord CoordDivF(SCoord op, float factor)
419 {
420 SCoord result;
421 result.x = (op.x / factor);
422 result.y = (op.y / factor);
423 result.z = (op.z / factor);
424
425 return result;
426 }
427
428 float CoordDot(SCoord cA, SCoord cB)
429 {
430 float result;
431
432 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z;
433 return result;
434 }
435
436 void calibrateViewport(float roll, float pitch, float yaw)
437 {
438 SSettings* pSettings = settingsGetPointer();
439
440 pSettings->viewPitch = pitch;
441 pSettings->viewRoll = roll;
442 pSettings->viewYaw = yaw;
443 }
444
445
446 float checkViewport(float roll, float pitch, float yaw)
447 {
448 uint8_t retval = 0;
449 float angleYaw;
450 float anglePitch;
451 float angleRoll;
452 float distance = 0;
453 float _a, _b;
454 SCoord u,v,n;
455 float r;
456
457 SCoord refVec;
458 SCoord axis_1;
459 SCoord axis_2;
460 SCoord curVec;
461 SCoord resultVec;
462
463 SSettings* pSettings = settingsGetPointer();
464
465 /* calculate base vector taking calibration delta into account yaw (heading) */
466 float compYaw = yaw + pSettings->viewYaw;
467 if (compYaw < 0.0)
468 {
469 compYaw = 360.0 + compYaw;
470 }
471
472 if (compYaw > 360.0)
473 {
474 compYaw = compYaw - 360.0;;
475 }
476
477 angleYaw = compYaw * M_PI / 180.0;
478 anglePitch = pSettings->viewPitch * M_PI / 180.0;
479 angleRoll = pSettings->viewRoll * M_PI / 180.0;
480
481 refVec.x = 0;
482 refVec.y = 0;
483 refVec.z = 1.0;
484
485 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
486
487 anglePitch = pitch * M_PI / 180.0;
488 angleRoll = roll * M_PI / 180.0;
489 angleYaw = yaw * M_PI / 180.0;
490
491 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
492 axis_1.x = 0;
493 if(refVec.y >=0)
494 {
495 axis_2.y = 1; /* => Spawn y == refVec y */
496 }
497 else axis_1.y = -1;
498 axis_1.z = -1.0 * refVec.y / refVec.z;
499 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */
500
501 /* check if detection plane is correct */
502 u = CoordSub(axis_1,refVec);
503 v = CoordSub(axis_2,refVec);
504 n = CoordCross(u,v);
505
506 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001))
507 {
508 retval = 2;
509 }
510 else
511 {
512 angleYaw = yaw * M_PI / 180.0;
513 anglePitch = pitch * M_PI / 180.0;
514 angleRoll = roll * M_PI / 180.0;
515 curVec.x = 0;
516 curVec.y = 0;
517 curVec.z = 1.0;
518 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
519
520 _a = CoordDot(curVec,n);
521 _b = CoordDot(refVec,n);
522
523 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */
524 {
525 retval = 3;
526 }
527 else
528 {
529 r=_a/_b;
530 if(r<0.00||r>1.40) /* are we looking into wrong direction? */
531 {
532 retval = 4;
533 }
534 }
535 distance = retval * 1.0; /* just for debugging */
536 if(retval == 0)
537 {
538
539 /* start calculating the matchpoint */
540 curVec = CoordMulF(curVec,r);
541 resultVec = CoordSub(refVec,curVec);
542
543 /* calculate the distance between reference and actual vector */
544 resultVec.x = resultVec.x * resultVec.x;
545 resultVec.y = resultVec.y * resultVec.y;
546 resultVec.z = resultVec.z * resultVec.z;
547
548 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0))
549 {
550 distance = 0.0;
551 }
552 else
553 {
554 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
555 }
556 }
557 }
558
559 if(distance < 0.5) /* handle focus counter to avoid fast in/out focus changes */
560 {
561 if(focusCnt < 10)
562 {
563 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
564 {
565 resetMotionDeltaHistory();
566 }
567 focusCnt++;
568 }
569 if(focusCnt == 10)
570 {
571 inFocus = 1;
572 }
573 }
574 else
575 {
576 if(focusCnt)
577 {
578 focusCnt--;
579 }
580 else
581 {
582 inFocus = 0;
583 }
584 }
585 return distance;
586 }
587 uint8_t viewInFocus(void)
588 {
589 return inFocus;
590 }
591 void resetFocusState(void)
592 {
593 inFocus = 0;
594 }