Mercurial > public > ostc4
annotate Discovery/Src/motion.c @ 563:d97f0e395058
Refine o2 sensor diagnostic functions:
Changed implementation to ignore connection errors if analog interface is selected. Because the HUD is using cBars, the code has been modified to be compatible with the Bar units provided by the analog interface. Behavior (outofbounds) should be identical to the previous implementation.
| author | Ideenmodellierer |
|---|---|
| date | Sun, 15 Nov 2020 20:01:56 +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 } |