Mercurial > public > ostc4
annotate Discovery/Src/motion.c @ 558:66aef216828b
Changed operation mode of RecoverSampleIndex:
In previous implementation the check was only done for the first 16 sector to be able to display status using a string. This may not be done by an OSTC companion function externally => changed function to handle all 192 sector used for log samples. The lastSampleIdx will now be stored immediatly (dive was necessary in previous version)
author | Ideenmodellierer |
---|---|
date | Thu, 12 Nov 2020 20:06:04 +0100 |
parents | e3237f580ae9 |
children | 01ee21dd311f |
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 | 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 | 21 #define SECTOR_WINDOW 40.0 /* Pitch window which is used for custom view projection */ |
381 | 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 | 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 | |
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 | 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 } | |
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 | 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 | 142 newPitch = 0.0; |
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 | 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 | 148 |
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 | 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 | 155 lastsector = sector; |
156 } | |
157 | |
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 | 187 } |
188 else | |
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 | 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 | 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 | 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 | 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 | 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 | 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 | 300 uint8_t exit = 0; |
301 uint8_t step = 0; | |
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 | 304 detectionState = DETECT_NOTHING; |
305 while((step != MOTION_DELTA_HISTORY_SIZE) && (!exit)) /* start backward evalution of pitch changes*/ | |
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 */ | |
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 | 362 else /* dont detect the same event twice */ |
363 { | |
364 resetMotionDeltaHistory(); | |
365 } | |
366 return detectionState; | |
367 } | |
363 | 368 |
551 | 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; | |
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
389 |
551 | 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 } | |
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
417 |
551 | 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; | |
381 | 470 } |
551 | 471 |
472 if (compYaw > 360.0) | |
381 | 473 { |
551 | 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 | 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; | |
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
586 } |
551 | 587 uint8_t viewInFocus(void) |
588 { | |
589 return inFocus; | |
590 } | |
591 void resetFocusState(void) | |
592 { | |
593 inFocus = 0; | |
594 } |