Mercurial > public > ostc4
annotate Discovery/Src/motion.c @ 599:5a8f9126e4cb
Bugfix T3 skip deactivated views:
I previous version the OSTC hangs if only conditional views (like decoplan which is only displayed in case of deco time) are switched. Rootcause was that the select function could never switch to another view because no visible view was available. To avoid this use case a fallback option has been added. In case no other view is available, the current view will be kept. In case no view at all (even no conditional view) is available then a switch to the "None" view will be done independend if it is enabled or not.
T3_Profile: Added functionalty to show max depth and divetime of a profile provided by the replay block.
| author | Ideenmodellierer |
|---|---|
| date | Mon, 04 Jan 2021 21:48:31 +0100 |
| parents | fd0b60dee6f3 |
| children | 916998f90e39 |
| 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" |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
17 #include "base.h" |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
18 |
|
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
19 #define STABLE_STATE_COUNT 2 /* number of count to declare a state as stable (at the moment based on 100ms) */ |
| 363 | 20 #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
|
21 |
|
585
4dfdf230d8ba
Reduced view angle and number of vusiualized custom views in setor mode:
Ideenmodellierer
parents:
578
diff
changeset
|
22 #define SECTOR_WINDOW 30.0 /* Pitch window which is used for custom view projection */ |
| 381 | 23 #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
|
24 #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
|
25 #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
|
26 #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
|
27 |
|
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
28 #define SECTOR_MAX 24 /* maximum number of sectors */ |
|
378
834e087505ec
Incremented "idle window" for scroll detection
ideenmodellierer
parents:
373
diff
changeset
|
29 #define SECTOR_SCROLL 7 /* number of sectors used for scroll detection */ |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
30 #define SECTOR_MAX_CNT 5 /* max number of views used for sector control */ |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
31 |
| 551 | 32 #define MOTION_DELTA_STABLE 0 |
| 33 #define MOTION_DELTA_JITTER 1 | |
| 34 #define MOTION_DELTA_RAISE 2 | |
| 35 #define MOTION_DELTA_FALL 3 | |
| 36 | |
| 37 #define MOTION_DELTA_JITTER_LEVEL 3.0 /* lower values are considered as stable */ | |
| 574 | 38 #define MOTION_DELTA_RAISE_LEVEL 6.0 /* Movement causing a significant change detected */ |
| 551 | 39 #define MOTION_DELTA_FALL_LEVEL -6.0 /* Movement causing a significant change detected */ |
| 40 | |
| 41 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */ | |
| 42 | |
|
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
43 detectionState_t detectionState = DETECT_NOTHING; |
|
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
44 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
|
45 |
| 551 | 46 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */ |
| 47 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */ | |
| 48 | |
| 49 static uint8_t focusCnt = 0; | |
| 50 static uint8_t inFocus = 0; | |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
51 static uint8_t sectorMap[SECTOR_MAX_CNT]; |
| 551 | 52 |
| 53 void resetMotionDeltaHistory() | |
| 54 { | |
| 55 motionDeltaHistoryIdx = 0; | |
| 56 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory)); | |
| 57 } | |
| 58 | |
| 59 void evaluateMotionDelta(float roll, float pitch, float yaw) | |
| 60 { | |
| 61 static float lastValue[3] = {0.0,0.0,0.0}; | |
| 62 uint8_t nextIndex = motionDeltaHistoryIdx + 1; | |
| 63 uint8_t axis; | |
| 64 float curValue; | |
| 65 | |
| 66 if(nextIndex == MOTION_DELTA_HISTORY_SIZE) | |
| 67 { | |
| 68 nextIndex = 0; | |
| 69 } | |
| 70 for(axis=0; axis < 3; axis++) | |
| 71 { | |
| 72 switch(axis) | |
| 73 { | |
| 74 case MOTION_HISTORY_ROLL: curValue = roll; | |
| 75 break; | |
| 76 case MOTION_HISTORY_PITCH: curValue = pitch; | |
| 77 break; | |
| 78 default: | |
| 79 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */ | |
| 80 { | |
| 81 lastValue[MOTION_HISTORY_YAW] -= 360; | |
| 82 } | |
| 83 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */ | |
| 84 { | |
| 85 lastValue[MOTION_HISTORY_YAW] += 360; | |
| 86 } | |
| 87 curValue = yaw; | |
| 88 break; | |
| 89 } | |
| 90 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL) | |
| 91 { | |
| 92 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE; | |
| 93 } | |
| 94 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL) | |
| 95 { | |
| 96 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER; | |
| 97 } | |
| 98 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL) | |
| 99 { | |
| 100 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE; | |
| 101 } | |
| 102 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL) | |
| 103 { | |
| 104 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL; | |
| 105 } | |
| 106 lastValue[axis] = curValue; | |
| 107 } | |
| 108 motionDeltaHistoryIdx = nextIndex; | |
| 109 } | |
| 110 | |
| 111 SDeltaHistory GetDeltaHistory(uint8_t stepback) | |
| 112 { | |
| 578 | 113 uint8_t loop; |
| 551 | 114 uint8_t index = motionDeltaHistoryIdx; |
| 115 | |
| 116 SDeltaHistory result = {0,0,0}; | |
| 117 | |
| 578 | 118 stepback++; /* motionDeltaHistoryIdx is pointing to future entry => step back one to get the latest */ |
| 119 loop = stepback; | |
| 551 | 120 if(stepback < MOTION_DELTA_HISTORY_SIZE) |
| 121 { | |
| 122 while(loop != 0) /* find requested entry */ | |
| 123 { | |
| 124 loop--; | |
| 125 index--; | |
| 126 if(index == 0) | |
| 127 { | |
| 128 index = MOTION_DELTA_HISTORY_SIZE - 1; | |
| 129 } | |
| 130 } | |
| 131 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index]; | |
| 132 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index]; | |
| 133 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index]; | |
| 134 } | |
| 135 return result; | |
| 136 } | |
|
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
|
137 |
|
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 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
|
139 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
140 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
|
141 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
|
142 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
|
143 |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
144 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */ |
| 381 | 145 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
|
146 { |
| 381 | 147 newPitch = 0.0; |
| 148 } | |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
149 if (newPitch > sectorDetection.window) /* clip value */ |
| 381 | 150 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
151 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
|
152 } |
| 381 | 153 |
| 154 /* switch to other sector? */ | |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
155 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder)) |
| 381 | 156 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
157 sector = (uint16_t) newPitch / sectorDetection.size; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
158 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
159 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY; |
| 381 | 160 lastsector = sector; |
| 161 } | |
| 162 | |
| 163 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
|
164 } |
|
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
|
165 |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
166 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
|
167 { |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
168 if(numOfSectors == CUSTOMER_DEFINED_VIEWS) |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
169 { |
|
384
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
170 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
171 { |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
172 sectorDetection.count = t3_GetEnabled_customviews(); |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
173 } |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
174 else |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
175 { |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
176 sectorDetection.count = t7_GetEnabled_customviews(); |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
177 } |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
178 if(sectorDetection.count > SECTOR_MAX_CNT) |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
179 { |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
180 sectorDetection.count = SECTOR_MAX_CNT; /* more views are hard to manually control */ |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
181 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
182 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
183 else |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
184 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
185 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
186 sectorDetection.count = numOfSectors; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
187 } |
|
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
|
188 |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
189 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
|
190 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
191 sectorDetection.window = SECTOR_WINDOW_MAX; |
| 381 | 192 } |
| 193 else | |
| 194 { | |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
195 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
|
196 } |
|
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
|
197 |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
198 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
199 sectorDetection.size = sectorDetection.window / sectorDetection.count; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
200 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
|
201 |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
202 /* reset border values */ |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
203 sectorDetection.lowerborder = SECTOR_BORDER; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
204 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
|
205 /* get the current sector */ |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
206 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
207 sectorDetection.target = sectorDetection.current; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
208 /* 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
|
209 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
|
210 |
|
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
|
211 } |
|
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
|
212 |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
213 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
214 uint8_t GetCVForSector(uint8_t selSector) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
215 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
216 if(selSector < sectorDetection.count) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
217 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
218 return sectorMap[selSector]; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
219 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
220 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
221 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
222 return 0; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
223 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
224 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
225 void MapCVToSector() |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
226 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
227 uint8_t centerView = 0; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
228 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
229 memset(sectorMap, 0, sizeof(sectorMap)); |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
230 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
231 switch(sectorDetection.count) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
232 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
233 case 1: centerView = 0; break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
234 case 2: centerView = 0; break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
235 case 3: centerView = 1; break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
236 case 4: centerView = 1; break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
237 case 5: centerView = 2; break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
238 default: centerView = sectorDetection.count / 2 - 1; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
239 break; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
240 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
241 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
242 { |
| 595 | 243 t3_set_customview_to_primary(); |
| 244 sectorMap[centerView] = t3_change_customview(ACTION_END); | |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
245 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
246 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
247 { |
| 595 | 248 t7_set_customview_to_primary(); |
| 249 sectorMap[centerView] = t7_change_customview(ACTION_END); | |
| 250 | |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
251 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
252 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
253 centerView++; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
254 while(sectorMap[centerView] == 0) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
255 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
256 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
257 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
258 sectorMap[centerView] = t3_change_customview(ACTION_BUTTON_ENTER); |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
259 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
260 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
261 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
262 sectorMap[centerView] = t7_change_customview(ACTION_BUTTON_ENTER); |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
263 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
264 centerView++; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
265 if(centerView == sectorDetection.count) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
266 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
267 centerView = 0; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
268 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
269 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
270 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
271 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
272 |
|
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
|
273 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
|
274 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
275 sectorDetection.target = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
276 sectorDetection.current = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
277 sectorDetection.size = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
278 sectorDetection.count = 0; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
279 |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
280 switch(settingsGetPointer()->MotionDetection) |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
281 { |
| 551 | 282 case MOTION_DETECT_SECTOR: DefinePitchSectors(settingsGetPointer()->viewPitch,CUSTOMER_DEFINED_VIEWS); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
283 MapCVToSector(); |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
284 break; |
| 551 | 285 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
|
286 break; |
| 551 | 287 case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL); |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
288 break; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
289 default: |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
290 break; |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
291 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
292 |
| 551 | 293 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
|
294 } |
|
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
|
295 |
|
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 /* 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
|
297 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
|
298 { |
|
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
|
299 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
|
300 |
|
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
|
301 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
302 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
303 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
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
|
304 { |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
305 t3_select_customview(GetCVForSector(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
|
306 } |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
307 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
308 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
309 t7_select_customview(GetCVForSector(newTargetSector)); |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
310 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
311 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
312 return DETECT_NOTHING; |
|
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
|
313 } |
|
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
|
314 |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
315 /* 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
|
316 detectionState_t detectScrollButtonEvent(float curPitch) |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
317 { |
|
378
834e087505ec
Incremented "idle window" for scroll detection
ideenmodellierer
parents:
373
diff
changeset
|
318 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
|
319 |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
320 uint8_t PitchEvent = DETECT_NOTHING; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
321 uint8_t newSector; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
322 |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
323 if(delayscroll == 0) |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
324 { |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
325 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch); |
| 551 | 326 /* 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
|
327 switch(newSector) |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
328 { |
| 574 | 329 case 0: PitchEvent = DETECT_POS_PITCH; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
330 break; |
|
378
834e087505ec
Incremented "idle window" for scroll detection
ideenmodellierer
parents:
373
diff
changeset
|
331 case 6: PitchEvent = DETECT_NEG_PITCH; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
332 break; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
333 default: |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
334 break; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
335 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
336 if(PitchEvent != DETECT_NOTHING) |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
337 { |
| 574 | 338 delayscroll = 7; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
339 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
340 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
341 else |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
342 { |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
343 delayscroll--; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
344 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
345 return PitchEvent; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
346 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
347 |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
348 |
|
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
|
349 /* 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
|
350 /* 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
|
351 detectionState_t detectPitch(float currentPitch) |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
352 { |
| 578 | 353 static int8_t lastStart = 0; |
| 551 | 354 uint8_t exit = 0; |
| 578 | 355 int8_t step = 0; |
| 574 | 356 uint8_t duration = 0; |
| 551 | 357 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
|
358 |
| 578 | 359 if(lastStart < 0) |
| 360 { | |
| 361 detectionState = DETECT_NOTHING; | |
| 362 lastStart = 0; | |
| 363 } | |
| 364 else | |
| 365 { | |
| 366 detectionState = DETECT_START; | |
| 367 } | |
| 368 step = lastStart; | |
| 369 do | |
| 551 | 370 { |
| 371 test = GetDeltaHistory(step); | |
| 574 | 372 duration++; |
| 551 | 373 switch (detectionState) |
| 374 { | |
| 375 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE) | |
| 376 { | |
| 377 exit = 1; | |
| 578 | 378 lastStart = -2; |
| 551 | 379 } |
| 380 else | |
| 381 { | |
| 382 detectionState = DETECT_START; | |
| 578 | 383 lastStart = -1; |
| 551 | 384 } |
| 385 break; | |
| 386 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE) | |
| 387 { | |
| 388 detectionState = DETECT_POS_MOVE; | |
| 578 | 389 lastStart = step; |
| 551 | 390 } |
| 578 | 391 else |
| 551 | 392 if(test.pitch == MOTION_DELTA_FALL) |
| 393 { | |
| 394 detectionState = DETECT_NEG_MOVE; | |
| 578 | 395 lastStart = step; |
| 396 } | |
| 397 else | |
| 398 { | |
| 399 lastStart = -1; | |
| 551 | 400 } |
| 574 | 401 duration = 0; |
| 551 | 402 break; |
| 578 | 403 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE)) |
| 404 { | |
| 405 detectionState++; | |
| 406 } | |
| 407 break; | |
| 408 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL)) | |
| 551 | 409 { |
| 410 detectionState++; | |
| 411 } | |
| 412 break; | |
| 413 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL) | |
| 414 { | |
| 415 detectionState = DETECT_FALLBACK; | |
| 416 } | |
| 417 break; | |
| 418 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE) | |
| 419 { | |
| 420 detectionState = DETECT_RISEBACK; | |
| 421 } | |
| 422 break; | |
| 423 case DETECT_RISEBACK: | |
| 424 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE) | |
| 425 { | |
| 578 | 426 if(duration > 4) /* avoid detection triggered by short moves */ |
| 574 | 427 { |
| 428 detectionState++; | |
| 429 } | |
| 578 | 430 exit = 1; |
| 431 lastStart = -2; | |
| 551 | 432 } |
| 433 break; | |
| 434 default: | |
| 435 detectionState = DETECT_NOTHING; | |
| 436 exit = 1; | |
| 437 break; | |
| 438 } | |
| 578 | 439 step--; |
| 440 } while((step >= 0) && (!exit)); | |
| 441 | |
| 442 if((lastStart < MOTION_DELTA_HISTORY_SIZE)) | |
| 443 { | |
| 444 lastStart++; /* prepare value for next iteration (history index will be increased) */ | |
| 445 } | |
| 446 else | |
| 447 { | |
| 448 lastStart = -1; | |
| 551 | 449 } |
| 450 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
|
451 { |
|
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
452 detectionState = DETECT_NOTHING; |
|
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
453 } |
| 551 | 454 else /* dont detect the same event twice */ |
| 455 { | |
| 456 resetMotionDeltaHistory(); | |
| 457 } | |
| 458 return detectionState; | |
| 459 } | |
| 363 | 460 |
| 551 | 461 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord) |
| 462 { | |
| 463 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); | |
| 464 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); | |
| 465 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z); | |
| 466 } | |
| 467 | |
| 468 SCoord CoordAdd(SCoord cA, SCoord cB) | |
| 469 { | |
| 470 SCoord result; | |
| 471 | |
| 472 result.x = cA.x + cB.x; | |
| 473 result.y = cA.y + cB.y; | |
| 474 result.z = cA.z + cB.z; | |
| 475 return result; | |
| 476 } | |
| 477 | |
| 478 SCoord CoordSub(SCoord cA, SCoord cB) | |
| 479 { | |
| 480 SCoord result; | |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
481 |
| 551 | 482 result.x = cA.x - cB.x; |
| 483 result.y = cA.y - cB.y; | |
| 484 result.z = cA.z - cB.z; | |
| 485 return result; | |
| 486 } | |
| 487 | |
| 488 SCoord CoordCross(SCoord cA, SCoord cB) | |
| 489 { | |
| 490 SCoord result; | |
| 491 | |
| 492 result.x = (cA.y * cB.z) - (cA.z * cB.y); | |
| 493 result.y = (cA.z * cB.x) - (cA.x * cB.z); | |
| 494 result.z = (cA.x * cB.y) - (cA.y * cB.x); | |
| 495 | |
| 496 return result; | |
| 497 | |
| 498 } | |
| 499 | |
| 500 SCoord CoordMulF(SCoord op, float factor) | |
| 501 { | |
| 502 SCoord result; | |
| 503 result.x = (op.x * factor); | |
| 504 result.y = (op.y * factor); | |
| 505 result.z = (op.z * factor); | |
| 506 | |
| 507 return result; | |
| 508 } | |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
509 |
| 551 | 510 SCoord CoordDivF(SCoord op, float factor) |
| 511 { | |
| 512 SCoord result; | |
| 513 result.x = (op.x / factor); | |
| 514 result.y = (op.y / factor); | |
| 515 result.z = (op.z / factor); | |
| 516 | |
| 517 return result; | |
| 518 } | |
| 519 | |
| 520 float CoordDot(SCoord cA, SCoord cB) | |
| 521 { | |
| 522 float result; | |
| 523 | |
| 524 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z; | |
| 525 return result; | |
| 526 } | |
| 527 | |
| 528 void calibrateViewport(float roll, float pitch, float yaw) | |
| 529 { | |
| 530 SSettings* pSettings = settingsGetPointer(); | |
| 531 | |
| 532 pSettings->viewPitch = pitch; | |
| 533 pSettings->viewRoll = roll; | |
| 534 pSettings->viewYaw = yaw; | |
| 535 } | |
| 536 | |
| 537 | |
| 538 float checkViewport(float roll, float pitch, float yaw) | |
| 539 { | |
| 574 | 540 static float freezeRoll = 0; |
| 541 static float freezeYaw = 0; | |
| 542 | |
| 551 | 543 uint8_t retval = 0; |
| 544 float angleYaw; | |
| 545 float anglePitch; | |
| 546 float angleRoll; | |
| 547 float distance = 0; | |
| 548 float _a, _b; | |
| 549 SCoord u,v,n; | |
| 550 float r; | |
| 551 | |
| 552 SCoord refVec; | |
| 553 SCoord axis_1; | |
| 554 SCoord axis_2; | |
| 555 SCoord curVec; | |
| 556 SCoord resultVec; | |
| 574 | 557 SDeltaHistory test; |
| 551 | 558 |
| 559 SSettings* pSettings = settingsGetPointer(); | |
| 560 | |
| 561 /* calculate base vector taking calibration delta into account yaw (heading) */ | |
| 562 float compYaw = yaw + pSettings->viewYaw; | |
| 574 | 563 |
| 564 compYaw = 360.0 - yaw; /* turn to 0° */ | |
| 565 compYaw += pSettings->viewYaw; /* consider calib yaw value */ | |
| 566 compYaw += yaw; | |
| 567 | |
| 551 | 568 if (compYaw < 0.0) |
| 569 { | |
| 570 compYaw = 360.0 + compYaw; | |
| 381 | 571 } |
| 551 | 572 |
| 573 if (compYaw > 360.0) | |
| 381 | 574 { |
| 574 | 575 compYaw = compYaw - 360.0; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
576 } |
| 574 | 577 if (compYaw > 360.0) |
| 578 { | |
| 579 compYaw = compYaw - 360.0; | |
| 580 } | |
| 581 angleYaw = pSettings->viewYaw * M_PI / 180.0; | |
| 551 | 582 anglePitch = pSettings->viewPitch * M_PI / 180.0; |
| 583 angleRoll = pSettings->viewRoll * M_PI / 180.0; | |
| 584 | |
| 585 refVec.x = 0; | |
| 586 refVec.y = 0; | |
| 587 refVec.z = 1.0; | |
| 588 | |
| 589 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec); | |
| 590 | |
| 591 anglePitch = pitch * M_PI / 180.0; | |
| 592 angleRoll = roll * M_PI / 180.0; | |
| 593 angleYaw = yaw * M_PI / 180.0; | |
| 594 | |
| 595 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */ | |
| 596 axis_1.x = 0; | |
| 597 if(refVec.y >=0) | |
| 598 { | |
| 599 axis_2.y = 1; /* => Spawn y == refVec y */ | |
| 600 } | |
| 601 else axis_1.y = -1; | |
| 602 axis_1.z = -1.0 * refVec.y / refVec.z; | |
| 603 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */ | |
| 604 | |
| 605 /* check if detection plane is correct */ | |
| 606 u = CoordSub(axis_1,refVec); | |
| 607 v = CoordSub(axis_2,refVec); | |
| 608 n = CoordCross(u,v); | |
| 609 | |
| 610 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001)) | |
| 611 { | |
| 612 retval = 2; | |
| 613 } | |
| 614 else | |
| 615 { | |
| 574 | 616 angleYaw = compYaw * M_PI / 180.0; |
| 551 | 617 anglePitch = pitch * M_PI / 180.0; |
| 618 angleRoll = roll * M_PI / 180.0; | |
| 619 curVec.x = 0; | |
| 620 curVec.y = 0; | |
| 621 curVec.z = 1.0; | |
| 622 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec); | |
| 623 | |
| 624 _a = CoordDot(curVec,n); | |
| 625 _b = CoordDot(refVec,n); | |
| 626 | |
| 627 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */ | |
| 628 { | |
| 629 retval = 3; | |
| 630 } | |
| 631 else | |
| 632 { | |
| 633 r=_a/_b; | |
| 634 if(r<0.00||r>1.40) /* are we looking into wrong direction? */ | |
| 635 { | |
| 636 retval = 4; | |
| 637 } | |
| 638 } | |
| 639 distance = retval * 1.0; /* just for debugging */ | |
| 640 if(retval == 0) | |
| 641 { | |
| 642 | |
| 643 /* start calculating the matchpoint */ | |
| 644 curVec = CoordMulF(curVec,r); | |
| 645 resultVec = CoordSub(refVec,curVec); | |
| 646 | |
| 647 /* calculate the distance between reference and actual vector */ | |
| 648 resultVec.x = resultVec.x * resultVec.x; | |
| 649 resultVec.y = resultVec.y * resultVec.y; | |
| 650 resultVec.z = resultVec.z * resultVec.z; | |
| 651 | |
| 652 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0)) | |
| 653 { | |
| 654 distance = 0.0; | |
| 655 } | |
| 656 else | |
| 657 { | |
| 658 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z)); | |
| 659 } | |
| 660 } | |
| 661 } | |
| 662 | |
| 663 if(distance < 0.5) /* handle focus counter to avoid fast in/out focus changes */ | |
| 664 { | |
| 665 if(focusCnt < 10) | |
| 666 { | |
| 667 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */ | |
| 668 { | |
| 669 resetMotionDeltaHistory(); | |
| 670 } | |
| 671 focusCnt++; | |
| 672 } | |
| 574 | 673 if((focusCnt == 10) && (inFocus == 0)) |
| 551 | 674 { |
| 675 inFocus = 1; | |
| 574 | 676 freezeRoll = roll; |
| 677 freezeYaw = yaw; | |
| 551 | 678 } |
| 679 } | |
| 680 else | |
| 681 { | |
| 574 | 682 if(focusCnt >= 5) /* Reset focus faster then setting focus */ |
| 551 | 683 { |
| 574 | 684 if(pSettings->MotionDetection != MOTION_DETECT_MOVE) /* only apply extended focus for methods using absolute pitch values */ |
| 685 { | |
| 686 test = GetDeltaHistory(0); | |
| 687 if((test.yaw == MOTION_DELTA_STABLE) && (test.roll == MOTION_DELTA_STABLE)) | |
| 688 { | |
| 689 if((fabsf(freezeRoll - roll) < MOTION_DELTA_JITTER_LEVEL) && (fabsf(freezeYaw - yaw) < MOTION_DELTA_JITTER_LEVEL)) | |
| 690 { | |
| 691 focusCnt++; | |
| 692 } | |
| 693 } | |
| 694 else | |
| 695 { | |
| 696 if(freezeRoll != 0.0) | |
| 697 { | |
| 698 focusCnt = 1; | |
| 699 } | |
| 700 } | |
| 701 } | |
| 551 | 702 focusCnt--; |
| 703 } | |
| 704 else | |
| 705 { | |
| 574 | 706 focusCnt = 0; |
| 551 | 707 inFocus = 0; |
| 574 | 708 freezeRoll = 0; |
| 709 freezeYaw = 0; | |
| 551 | 710 } |
| 711 } | |
| 712 return distance; | |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
713 } |
| 551 | 714 uint8_t viewInFocus(void) |
| 715 { | |
| 716 return inFocus; | |
| 717 } | |
| 718 void resetFocusState(void) | |
| 719 { | |
| 720 inFocus = 0; | |
| 721 } |