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

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
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>
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
10 #include <stdlib.h>
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
11 #include <math.h>
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
12 #include "motion.h"
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
13 #include "data_central.h"
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
14 #include "t7.h"
384
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
15 #include "t3.h"
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
16 #include "settings.h"
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
17
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
18 #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
19 #define STABLE_STATE_TIMEOUT 5 /* Detection shall be aborted if a movement state is stable for more than 500ms */
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
20
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
21 #define SECTOR_WINDOW 40.0 /* Pitch window which is used for custom view projection */
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
22 #define SECTOR_WINDOW_MAX 120.0 /* Pitch window which will be greater than the divers field of view */
385
0cd862e501f6 Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents: 384
diff changeset
23 #define SECTOR_HYSTERY 2 /* Additional offset to avoid fast changing displays */
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
24 #define SECTOR_BORDER 400.0 /* Define a value which is out of limit to avoid not wanted key events */
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
25 #define SECTOR_FILTER 10 /* Define speed for calculated angle to follow real value */
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
26
385
0cd862e501f6 Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents: 384
diff changeset
27 #define SECTOR_MAX 24 /* maximum number of sectors */
378
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
28 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
29
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
30 #define MOTION_DELTA_STABLE 0
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
31 #define MOTION_DELTA_JITTER 1
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
32 #define MOTION_DELTA_RAISE 2
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
33 #define MOTION_DELTA_FALL 3
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
34
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
35 #define MOTION_DELTA_JITTER_LEVEL 3.0 /* lower values are considered as stable */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
36 #define MOTION_DELTA_RAISE_LEVEL 6.0 /* Movement causing a significant change detected */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
37 #define MOTION_DELTA_FALL_LEVEL -6.0 /* Movement causing a significant change detected */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
38
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
39 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
40
385
0cd862e501f6 Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents: 384
diff changeset
41 detectionState_t detectionState = DETECT_NOTHING;
0cd862e501f6 Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents: 384
diff changeset
42 SSector sectorDetection;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
43
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
44 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
45 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
46
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
47 static uint8_t focusCnt = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
48 static uint8_t inFocus = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
49
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
50 void resetMotionDeltaHistory()
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
51 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
52 motionDeltaHistoryIdx = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
53 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory));
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
54 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
55
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
56 void evaluateMotionDelta(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
57 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
58 static float lastValue[3] = {0.0,0.0,0.0};
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
59 uint8_t nextIndex = motionDeltaHistoryIdx + 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
60 uint8_t axis;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
61 float curValue;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
62
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
63 if(nextIndex == MOTION_DELTA_HISTORY_SIZE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
64 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
65 nextIndex = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
66 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
67 for(axis=0; axis < 3; axis++)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
68 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
69 switch(axis)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
70 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
71 case MOTION_HISTORY_ROLL: curValue = roll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
72 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
73 case MOTION_HISTORY_PITCH: curValue = pitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
74 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
75 default:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
76 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
77 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
78 lastValue[MOTION_HISTORY_YAW] -= 360;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
79 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
80 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
81 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
82 lastValue[MOTION_HISTORY_YAW] += 360;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
83 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
84 curValue = yaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
85 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
86 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
87 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
88 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
89 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
90 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
91 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
92 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
93 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
94 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
95 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
96 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
97 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
98 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
99 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
100 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
101 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
102 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
103 lastValue[axis] = curValue;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
104 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
105 motionDeltaHistoryIdx = nextIndex;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
106 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
107
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
108 SDeltaHistory GetDeltaHistory(uint8_t stepback)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
109 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
110 uint8_t loop = stepback;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
111 uint8_t index = motionDeltaHistoryIdx;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
112
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
113 SDeltaHistory result = {0,0,0};
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
114
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
115 if(stepback < MOTION_DELTA_HISTORY_SIZE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
116 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
117 while(loop != 0) /* find requested entry */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
118 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
119 loop--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
120 index--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
121 if(index == 0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
122 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
123 index = MOTION_DELTA_HISTORY_SIZE - 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
124 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
125 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
126 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
127 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
128 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
129 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
130 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
131 }
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
132
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
133 uint8_t GetSectorForPitch(float pitch)
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
134 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
135 static uint8_t lastsector = 0;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
136 float newPitch;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
137 uint8_t sector = 0;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
138
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
139 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
140 if (newPitch < 0.0) /* clip value */
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
141 {
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
142 newPitch = 0.0;
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
143 }
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
144 if (newPitch > sectorDetection.window) /* clip value */
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
145 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
146 newPitch = sectorDetection.window;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
147 }
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
148
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
149 /* switch to other sector? */
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
150 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
151 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
152 sector = (uint16_t) newPitch / sectorDetection.size;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
153 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
154 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
155 lastsector = sector;
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
156 }
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
157
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
158 return lastsector;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
159 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
160
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
161 void DefinePitchSectors(float centerPitch,uint8_t numOfSectors)
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
162 {
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
163 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
164 {
384
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
165 if(settingsGetPointer()->design == 3) /* Big font view ? */
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
166 {
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
167 sectorDetection.count = t3_GetEnabled_customviews();
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
168 }
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
169 else
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
170 {
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
171 sectorDetection.count = t7_GetEnabled_customviews();
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
172 }
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
173 if(sectorDetection.count > 7)
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
174 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
175 sectorDetection.count = 7; /* more views are hard to manually control */
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
176 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
177 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
178 else
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
179 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
180 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
181 sectorDetection.count = numOfSectors;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
182 }
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
183
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
184 if(sectorDetection.count == SECTOR_MAX)
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
185 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
186 sectorDetection.window = SECTOR_WINDOW_MAX;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
187 }
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
188 else
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
189 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
190 sectorDetection.window = SECTOR_WINDOW;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
191 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
192
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
193 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
194 sectorDetection.size = sectorDetection.window / sectorDetection.count;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
195 sectorDetection.center = 0;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
196
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
197 /* reset border values */
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
198 sectorDetection.lowerborder = SECTOR_BORDER;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
199 sectorDetection.upperborder = SECTOR_BORDER * -1.0;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
200 /* get the current sector */
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
201 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
202 sectorDetection.target = sectorDetection.current;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
203 /* do a small adjustment to center pitch to make sure the actual pitch is in the center of the current sector */
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
204 sectorDetection.center = (sectorDetection.upperborder) - ((sectorDetection.size + 2 *SECTOR_HYSTERY) / 2.0) - (centerPitch + sectorDetection.offset);
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
205
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
206 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
207
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
208 void InitMotionDetection(void)
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
209 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
210 sectorDetection.target = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
211 sectorDetection.current = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
212 sectorDetection.size = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
213 sectorDetection.count = 0;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
214
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
215 switch(settingsGetPointer()->MotionDetection)
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
216 {
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
217 case MOTION_DETECT_SECTOR: DefinePitchSectors(settingsGetPointer()->viewPitch,CUSTOMER_DEFINED_VIEWS);
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
218 break;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
219 case MOTION_DETECT_MOVE: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_MAX);
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
220 break;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
221 case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL);
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
222 break;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
223 default:
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
224 break;
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
225 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
226
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
227 resetMotionDeltaHistory();
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
228 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
229
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
230 /* Map the current pitch value to a sector and create button event in case the sector is left */
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
231 detectionState_t detectSectorButtonEvent(float curPitch)
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
232 {
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
233 static uint8_t lastTargetSector = 0;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
234 uint8_t newTargetSector;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
235 uint8_t PitchEvent = DETECT_NOTHING;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
236
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
237 /* only change sector if reading is stable */
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
238 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
239 if(lastTargetSector == newTargetSector)
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
240 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
241 sectorDetection.target = newTargetSector;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
242 }
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
243 lastTargetSector = newTargetSector;
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
244 if(sectorDetection.target != sectorDetection.current)
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
245 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
246 if(sectorDetection.target > sectorDetection.current)
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
247 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
248 sectorDetection.current++;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
249 PitchEvent = DETECT_POS_PITCH;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
250 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
251 else
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
252 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
253 sectorDetection.current--;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
254 PitchEvent = DETECT_NEG_PITCH;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
255 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
256 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
257 return PitchEvent;
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
258 }
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
259
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
260 /* Check if pitch is not in center position and trigger a button action if needed */
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
261 detectionState_t detectScrollButtonEvent(float curPitch)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
262 {
378
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
263 static uint8_t delayscroll = 0; /* slow down the number of scroll events */
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
264
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
265 uint8_t PitchEvent = DETECT_NOTHING;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
266 uint8_t newSector;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
267
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
268 if(delayscroll == 0)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
269 {
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
270 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
271 /* for scroll detection the motion window is split into 6 sectors => set event accoring to the sector number*/
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
272 switch(newSector)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
273 {
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
274 case 0:
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
275 case 1: PitchEvent = DETECT_POS_PITCH;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
276 break;
378
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
277 case 5:
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
278 case 6: PitchEvent = DETECT_NEG_PITCH;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
279 break;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
280 default:
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
281 break;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
282 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
283 if(PitchEvent != DETECT_NOTHING)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
284 {
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
285 delayscroll = 5;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
286 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
287 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
288 else
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
289 {
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
290 delayscroll--;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
291 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
292 return PitchEvent;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
293 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
294
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
295
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
296 /* Detect if user is generating an pitch including return to starting position */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
297 /* This is done by feeding the past movements value per value into a state machine */
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
298 detectionState_t detectPitch(float currentPitch)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
299 {
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
300 uint8_t exit = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
301 uint8_t step = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
302 SDeltaHistory test;
370
77cdfbdaca8c 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
ideenmodellierer
parents: 363
diff changeset
303
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
304 detectionState = DETECT_NOTHING;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
305 while((step != MOTION_DELTA_HISTORY_SIZE) && (!exit)) /* start backward evalution of pitch changes*/
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
306 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
307 test = GetDeltaHistory(step);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
308 step++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
309 switch (detectionState)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
310 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
311 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
312 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
313 exit = 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
314 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
315 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
316 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
317 detectionState = DETECT_START;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
318 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
319 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
320 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
321 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
322 detectionState = DETECT_POS_MOVE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
323 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
324 if(test.pitch == MOTION_DELTA_FALL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
325 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
326 detectionState = DETECT_NEG_MOVE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
327 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
328 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
329 case DETECT_NEG_MOVE:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
330 case DETECT_POS_MOVE: if(test.pitch <= MOTION_DELTA_JITTER)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
331 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
332 detectionState++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
333 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
334 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
335 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
336 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
337 detectionState = DETECT_FALLBACK;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
338 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
339 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
340 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
341 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
342 detectionState = DETECT_RISEBACK;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
343 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
344 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
345 case DETECT_RISEBACK:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
346 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
347 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
348 detectionState++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
349 exit = 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
350 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
351 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
352 default:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
353 detectionState = DETECT_NOTHING;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
354 exit = 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
355 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
356 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
357 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
358 if((detectionState != DETECT_POS_PITCH) && (detectionState != DETECT_NEG_PITCH)) /* nothing found */
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
359 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
360 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
361 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
362 else /* dont detect the same event twice */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
363 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
364 resetMotionDeltaHistory();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
365 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
366 return detectionState;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
367 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
368
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
369 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
370 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
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);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
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);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
373 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
374 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
375
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
376 SCoord CoordAdd(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
377 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
378 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
379
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
380 result.x = cA.x + cB.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
381 result.y = cA.y + cB.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
382 result.z = cA.z + cB.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
383 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
384 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
385
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
386 SCoord CoordSub(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
387 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
388 SCoord result;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
389
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
390 result.x = cA.x - cB.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
391 result.y = cA.y - cB.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
392 result.z = cA.z - cB.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
393 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
394 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
395
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
396 SCoord CoordCross(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
397 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
398 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
399
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
400 result.x = (cA.y * cB.z) - (cA.z * cB.y);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
401 result.y = (cA.z * cB.x) - (cA.x * cB.z);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
402 result.z = (cA.x * cB.y) - (cA.y * cB.x);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
403
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
404 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
405
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
406 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
407
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
408 SCoord CoordMulF(SCoord op, float factor)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
409 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
410 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
411 result.x = (op.x * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
412 result.y = (op.y * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
413 result.z = (op.z * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
414
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
415 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
416 }
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
417
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
418 SCoord CoordDivF(SCoord op, float factor)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
419 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
420 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
421 result.x = (op.x / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
422 result.y = (op.y / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
423 result.z = (op.z / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
424
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
425 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
426 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
427
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
428 float CoordDot(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
429 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
430 float result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
431
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
432 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
433 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
434 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
435
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
436 void calibrateViewport(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
437 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
438 SSettings* pSettings = settingsGetPointer();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
439
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
440 pSettings->viewPitch = pitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
441 pSettings->viewRoll = roll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
442 pSettings->viewYaw = yaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
443 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
444
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
445
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
446 float checkViewport(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
447 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
448 uint8_t retval = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
449 float angleYaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
450 float anglePitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
451 float angleRoll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
452 float distance = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
453 float _a, _b;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
454 SCoord u,v,n;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
455 float r;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
456
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
457 SCoord refVec;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
458 SCoord axis_1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
459 SCoord axis_2;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
460 SCoord curVec;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
461 SCoord resultVec;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
462
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
463 SSettings* pSettings = settingsGetPointer();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
464
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
465 /* calculate base vector taking calibration delta into account yaw (heading) */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
466 float compYaw = yaw + pSettings->viewYaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
467 if (compYaw < 0.0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
468 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
469 compYaw = 360.0 + compYaw;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
470 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
471
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
472 if (compYaw > 360.0)
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
473 {
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
474 compYaw = compYaw - 360.0;;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
475 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
476
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
477 angleYaw = compYaw * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
478 anglePitch = pSettings->viewPitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
479 angleRoll = pSettings->viewRoll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
480
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
481 refVec.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
482 refVec.y = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
483 refVec.z = 1.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
484
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
485 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
486
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
487 anglePitch = pitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
488 angleRoll = roll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
489 angleYaw = yaw * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
490
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
491 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
492 axis_1.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
493 if(refVec.y >=0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
494 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
495 axis_2.y = 1; /* => Spawn y == refVec y */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
496 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
497 else axis_1.y = -1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
498 axis_1.z = -1.0 * refVec.y / refVec.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
499 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
500
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
501 /* check if detection plane is correct */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
502 u = CoordSub(axis_1,refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
503 v = CoordSub(axis_2,refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
504 n = CoordCross(u,v);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
505
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
506 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001))
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
507 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
508 retval = 2;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
509 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
510 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
511 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
512 angleYaw = yaw * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
513 anglePitch = pitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
514 angleRoll = roll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
515 curVec.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
516 curVec.y = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
517 curVec.z = 1.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
518 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
519
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
520 _a = CoordDot(curVec,n);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
521 _b = CoordDot(refVec,n);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
522
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
523 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
524 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
525 retval = 3;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
526 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
527 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
528 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
529 r=_a/_b;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
530 if(r<0.00||r>1.40) /* are we looking into wrong direction? */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
531 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
532 retval = 4;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
533 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
534 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
535 distance = retval * 1.0; /* just for debugging */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
536 if(retval == 0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
537 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
538
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
539 /* start calculating the matchpoint */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
540 curVec = CoordMulF(curVec,r);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
541 resultVec = CoordSub(refVec,curVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
542
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
543 /* calculate the distance between reference and actual vector */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
544 resultVec.x = resultVec.x * resultVec.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
545 resultVec.y = resultVec.y * resultVec.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
546 resultVec.z = resultVec.z * resultVec.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
547
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
548 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0))
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
549 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
550 distance = 0.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
551 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
552 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
553 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
554 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
555 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
556 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
557 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
558
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
559 if(distance < 0.5) /* handle focus counter to avoid fast in/out focus changes */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
560 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
561 if(focusCnt < 10)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
562 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
563 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
564 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
565 resetMotionDeltaHistory();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
566 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
567 focusCnt++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
568 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
569 if(focusCnt == 10)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
570 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
571 inFocus = 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
572 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
573 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
574 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
575 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
576 if(focusCnt)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
577 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
578 focusCnt--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
579 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
580 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
581 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
582 inFocus = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
583 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
584 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
585 return distance;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
586 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
587 uint8_t viewInFocus(void)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
588 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
589 return inFocus;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
590 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
591 void resetFocusState(void)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
592 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
593 inFocus = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
594 }