Mercurial > public > ostc4
annotate Discovery/Src/motion.c @ 680:5575f21382d5 Betatest
Bugfix visualization better setpoint:
Due to a formating mistake a complete float was show in case a better setpoint had been suggested. The string formating has been updated to show a 4 characters string
| author | Ideenmodellierer |
|---|---|
| date | Mon, 25 Apr 2022 21:07:28 +0200 |
| parents | ff2b393e290f |
| children |
| 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 |
|
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
22 #define SECTOR_MAX 24 /* maximum number of sectors */ |
|
378
834e087505ec
Incremented "idle window" for scroll detection
ideenmodellierer
parents:
373
diff
changeset
|
23 #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
|
24 #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
|
25 |
| 627 | 26 |
| 27 typedef enum | |
| 28 { | |
| 29 MOTION_DELTA_STABLE = 0, | |
| 30 MOTION_DELTA_JITTER, | |
| 31 MOTION_DELTA_RAISE, | |
| 32 MOTION_DELTA_RAISE_FAST, | |
| 33 MOTION_DELTA_FALL, | |
| 34 MOTION_DELTA_FALL_FAST | |
| 35 } MotionDeltaState_t; | |
| 551 | 36 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
37 #define MOTION_DELTA_JITTER_LEVEL 2.0 /* lower values are considered as stable */ |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
38 #define MOTION_DELTA_RAISE_LEVEL 4.0 /* Movement causing a significant change detected */ |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
39 #define MOTION_DELTA_FALL_LEVEL -4.0 /* Movement causing a significant change detected */ |
| 627 | 40 #define MOTION_DELTA_FAST_LEVEL 6.0 /* Movement causing a fast change detected */ |
| 551 | 41 |
| 42 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */ | |
| 43 | |
| 627 | 44 #define MOTION_FOCUS_LIMIT 0.5 /* +/- value which defines the border of the focus area */ |
| 45 #define MOTION_FOCUS_USE_SECTOR 0.4 /* +/- value for the focus area used to map secors to views */ | |
| 46 #define MOTION_FOCUS_SCROLL_IDLE 0.3 /* +/- value for starting generation of scroll events */ | |
| 47 | |
|
385
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
48 detectionState_t detectionState = DETECT_NOTHING; |
|
0cd862e501f6
Finetune parameter for detection of small sectors as used for the pitch detection
ideenmodellierer
parents:
384
diff
changeset
|
49 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
|
50 |
| 551 | 51 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */ |
| 52 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */ | |
| 53 | |
| 54 static uint8_t focusCnt = 0; | |
| 55 static uint8_t inFocus = 0; | |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
56 static uint8_t sectorMap[SECTOR_MAX_CNT]; |
| 551 | 57 |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
58 static uint8_t suspendMotionDetectionSec = 0; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
59 |
| 551 | 60 void resetMotionDeltaHistory() |
| 61 { | |
| 62 motionDeltaHistoryIdx = 0; | |
| 63 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory)); | |
| 64 } | |
| 65 | |
| 66 void evaluateMotionDelta(float roll, float pitch, float yaw) | |
| 67 { | |
| 68 static float lastValue[3] = {0.0,0.0,0.0}; | |
| 69 uint8_t nextIndex = motionDeltaHistoryIdx + 1; | |
| 70 uint8_t axis; | |
| 71 float curValue; | |
| 72 | |
| 73 if(nextIndex == MOTION_DELTA_HISTORY_SIZE) | |
| 74 { | |
| 75 nextIndex = 0; | |
| 76 } | |
| 77 for(axis=0; axis < 3; axis++) | |
| 78 { | |
| 79 switch(axis) | |
| 80 { | |
| 81 case MOTION_HISTORY_ROLL: curValue = roll; | |
| 82 break; | |
| 83 case MOTION_HISTORY_PITCH: curValue = pitch; | |
| 84 break; | |
| 85 default: | |
| 86 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */ | |
| 87 { | |
| 88 lastValue[MOTION_HISTORY_YAW] -= 360; | |
| 89 } | |
| 90 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */ | |
| 91 { | |
| 92 lastValue[MOTION_HISTORY_YAW] += 360; | |
| 93 } | |
| 94 curValue = yaw; | |
| 95 break; | |
| 96 } | |
| 97 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL) | |
| 98 { | |
| 99 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE; | |
| 100 } | |
| 101 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL) | |
| 102 { | |
| 103 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER; | |
| 104 } | |
| 105 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL) | |
| 106 { | |
| 107 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE; | |
| 108 } | |
| 109 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL) | |
| 110 { | |
| 111 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL; | |
| 112 } | |
| 627 | 113 |
| 114 if(fabsf(curValue - lastValue[axis]) > MOTION_DELTA_FAST_LEVEL) | |
| 115 { | |
| 116 motionDeltaHistory[axis][nextIndex]++; | |
| 117 } | |
| 118 | |
| 551 | 119 lastValue[axis] = curValue; |
| 120 } | |
| 121 motionDeltaHistoryIdx = nextIndex; | |
| 122 } | |
| 123 | |
| 124 SDeltaHistory GetDeltaHistory(uint8_t stepback) | |
| 125 { | |
| 578 | 126 uint8_t loop; |
| 551 | 127 uint8_t index = motionDeltaHistoryIdx; |
| 128 | |
| 129 SDeltaHistory result = {0,0,0}; | |
| 130 | |
| 627 | 131 loop = stepback + 1; /* motionDeltaHistoryIdx is pointing to future entry => step back one more to get the latest */ |
| 551 | 132 if(stepback < MOTION_DELTA_HISTORY_SIZE) |
| 133 { | |
| 134 while(loop != 0) /* find requested entry */ | |
| 135 { | |
| 136 loop--; | |
| 137 index--; | |
| 138 if(index == 0) | |
| 139 { | |
| 140 index = MOTION_DELTA_HISTORY_SIZE - 1; | |
| 141 } | |
| 142 } | |
| 143 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index]; | |
| 144 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index]; | |
| 145 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index]; | |
| 146 } | |
| 147 return result; | |
| 148 } | |
|
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
|
149 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
150 uint8_t GetSectorForFocus(float focusOffset) |
|
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
|
151 { |
|
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 uint8_t sector = 0; |
| 627 | 153 float compare = -1.0 * MOTION_FOCUS_USE_SECTOR + sectorDetection.size ; /* start with first sector upper limit */ |
|
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
|
154 |
| 627 | 155 while(compare <= MOTION_FOCUS_USE_SECTOR) |
| 381 | 156 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
157 if(focusOffset > compare) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
158 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
159 sector++; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
160 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
161 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
162 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
163 break; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
164 } |
| 627 | 165 compare += sectorDetection.size; |
|
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
|
166 } |
| 627 | 167 if(sector >= sectorDetection.count) |
| 381 | 168 { |
| 627 | 169 sector = sectorDetection.count - 1; |
| 381 | 170 } |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
171 return sector; |
|
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
|
172 } |
|
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
|
173 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
174 void DefineSectorCount(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
|
175 { |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
176 if(numOfSectors == CUSTOMER_DEFINED_VIEWS) |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
177 { |
|
384
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
178 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
179 { |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
180 sectorDetection.count = t3_GetEnabled_customviews(); |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
181 } |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
182 else |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
183 { |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
184 sectorDetection.count = t7_GetEnabled_customviews(); |
|
427ae9f8e28e
Consider number of available t3 views in sector handling:
ideenmodellierer
parents:
383
diff
changeset
|
185 } |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
186 if(sectorDetection.count > SECTOR_MAX_CNT) |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
187 { |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
188 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
|
189 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
190 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
191 else |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
192 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS) |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
193 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
194 sectorDetection.count = numOfSectors; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
195 } |
| 627 | 196 sectorDetection.size = MOTION_FOCUS_USE_SECTOR * 2.0 / sectorDetection.count; |
|
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
|
197 } |
|
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
|
198 |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
199 |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
200 uint8_t GetCVForSector(uint8_t selSector) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
201 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
202 if(selSector < sectorDetection.count) |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
203 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
204 return sectorMap[selSector]; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
205 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
206 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
207 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
208 return 0; |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
209 } |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
210 } |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
211 |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
212 void MapCVToSector() |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
213 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
214 uint8_t ViewIndex = 0; |
| 627 | 215 memset(sectorMap, 0, sizeof(sectorMap)); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
216 |
| 627 | 217 while(ViewIndex < (sectorDetection.count / 2)) /* define center sector */ |
| 218 { | |
| 219 ViewIndex++; | |
| 220 } | |
|
592
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 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
223 { |
| 595 | 224 t3_set_customview_to_primary(); |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
225 sectorMap[ViewIndex] = t3_change_customview(ACTION_END); |
|
592
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 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
228 { |
| 595 | 229 t7_set_customview_to_primary(); |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
230 sectorMap[ViewIndex] = t7_change_customview(ACTION_END); |
| 595 | 231 |
|
592
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 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
234 ViewIndex++; |
| 627 | 235 while(sectorMap[ViewIndex] == 0) |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
236 { |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
237 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
238 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
239 sectorMap[ViewIndex] = t3_change_customview(ACTION_BUTTON_ENTER); |
|
592
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 else |
|
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
242 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
243 sectorMap[ViewIndex] = t7_change_customview(ACTION_BUTTON_ENTER); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
244 } |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
245 ViewIndex++; |
| 627 | 246 if(ViewIndex == sectorDetection.count) |
| 247 { | |
| 248 ViewIndex = 0; | |
| 249 } | |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
250 } |
|
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 |
|
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 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
|
255 { |
|
383
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
256 sectorDetection.target = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
257 sectorDetection.current = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
258 sectorDetection.size = 0; |
|
49a02dea8ae3
Combine variables in a structure to improve readability of code
ideenmodellierer
parents:
381
diff
changeset
|
259 sectorDetection.count = 0; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
260 |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
261 switch(settingsGetPointer()->MotionDetection) |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
262 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
263 case MOTION_DETECT_SECTOR: DefineSectorCount(CUSTOMER_DEFINED_VIEWS); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
264 MapCVToSector(); |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
265 break; |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
266 case MOTION_DETECT_MOVE: DefineSectorCount(SECTOR_MAX); |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
267 break; |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
268 case MOTION_DETECT_SCROLL: DefineSectorCount(SECTOR_SCROLL); |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
269 break; |
|
371
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
270 default: |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
271 break; |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
272 } |
|
fca370f847f8
Added parameter for number of sectors to be defined
ideenmodellierer
parents:
370
diff
changeset
|
273 |
| 551 | 274 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
|
275 } |
|
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
|
276 |
|
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
|
277 /* Map the current pitch value to a sector and create button event in case the sector is left */ |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
278 detectionState_t detectSectorButtonEvent(float focusOffset) |
|
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
|
279 { |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
280 static uint8_t lastTargetSector = 0xFF; |
|
648
ff2b393e290f
Motion detection: Enable focus after focus calibration:
Ideenmodellierer
parents:
642
diff
changeset
|
281 static float lastfocusOffset = 1000.0; |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
282 |
|
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
|
283 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
|
284 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
285 newTargetSector = GetSectorForFocus(focusOffset); |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
286 |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
287 /* take a small hysteresis into account to avoid fast display changes (flicker) */ |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
288 if((newTargetSector != lastTargetSector) && (fabsf(focusOffset - lastfocusOffset) > (sectorDetection.size / 3))) |
|
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
|
289 { |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
290 lastfocusOffset = focusOffset; |
|
648
ff2b393e290f
Motion detection: Enable focus after focus calibration:
Ideenmodellierer
parents:
642
diff
changeset
|
291 lastTargetSector = newTargetSector; |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
292 if(settingsGetPointer()->design == 3) /* Big font view ? */ |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
293 { |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
294 t3_select_customview(GetCVForSector(newTargetSector)); |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
295 } |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
296 else |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
297 { |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
298 t7_select_customview(GetCVForSector(newTargetSector)); |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
299 } |
|
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
|
300 } |
|
592
f52bc70e380f
MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents:
585
diff
changeset
|
301 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
|
302 } |
|
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 |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
304 /* Check if pitch is not in center position and trigger a button action if needed */ |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
305 detectionState_t detectScrollButtonEvent(float focusOffset) |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
306 { |
|
378
834e087505ec
Incremented "idle window" for scroll detection
ideenmodellierer
parents:
373
diff
changeset
|
307 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
|
308 |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
309 uint8_t PitchEvent = DETECT_NOTHING; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
310 |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
311 if(delayscroll == 0) |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
312 { |
| 627 | 313 if(focusOffset > MOTION_FOCUS_SCROLL_IDLE) |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
314 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
315 PitchEvent = DETECT_POS_PITCH; |
| 574 | 316 delayscroll = 7; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
317 } |
| 627 | 318 if(focusOffset < (-1.0 * MOTION_FOCUS_SCROLL_IDLE)) |
| 319 { | |
| 320 PitchEvent = DETECT_NEG_PITCH; | |
| 321 delayscroll = 7; | |
| 322 } | |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
323 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
324 else |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
325 { |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
326 delayscroll--; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
327 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
328 return PitchEvent; |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
329 } |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
330 |
|
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
331 |
|
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
|
332 /* 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
|
333 /* 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
|
334 detectionState_t detectPitch(float currentPitch) |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
335 { |
| 578 | 336 static int8_t lastStart = 0; |
| 551 | 337 uint8_t exit = 0; |
| 578 | 338 int8_t step = 0; |
| 574 | 339 uint8_t duration = 0; |
| 551 | 340 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
|
341 |
| 578 | 342 if(lastStart < 0) |
| 343 { | |
| 344 detectionState = DETECT_NOTHING; | |
| 345 lastStart = 0; | |
| 346 } | |
| 347 else | |
| 348 { | |
| 349 detectionState = DETECT_START; | |
| 350 } | |
| 351 step = lastStart; | |
| 352 do | |
| 551 | 353 { |
| 354 test = GetDeltaHistory(step); | |
| 574 | 355 duration++; |
| 551 | 356 switch (detectionState) |
| 357 { | |
| 358 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE) | |
| 359 { | |
| 360 exit = 1; | |
| 578 | 361 lastStart = -2; |
| 551 | 362 } |
| 363 else | |
| 364 { | |
| 365 detectionState = DETECT_START; | |
| 578 | 366 lastStart = -1; |
| 551 | 367 } |
| 368 break; | |
| 369 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE) | |
| 370 { | |
| 371 detectionState = DETECT_POS_MOVE; | |
| 578 | 372 lastStart = step; |
| 551 | 373 } |
| 578 | 374 else |
| 551 | 375 if(test.pitch == MOTION_DELTA_FALL) |
| 376 { | |
| 377 detectionState = DETECT_NEG_MOVE; | |
| 578 | 378 lastStart = step; |
| 379 } | |
| 380 else | |
| 381 { | |
| 382 lastStart = -1; | |
| 551 | 383 } |
| 574 | 384 duration = 0; |
| 551 | 385 break; |
| 627 | 386 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE) || (test.pitch == MOTION_DELTA_RAISE_FAST)) |
| 578 | 387 { |
| 388 detectionState++; | |
| 389 } | |
| 390 break; | |
| 627 | 391 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL) || (test.pitch == MOTION_DELTA_FALL_FAST)) |
| 551 | 392 { |
| 393 detectionState++; | |
| 394 } | |
| 395 break; | |
| 396 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL) | |
| 397 { | |
| 398 detectionState = DETECT_FALLBACK; | |
| 399 } | |
| 400 break; | |
| 401 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE) | |
| 402 { | |
| 403 detectionState = DETECT_RISEBACK; | |
| 404 } | |
| 405 break; | |
| 406 case DETECT_RISEBACK: | |
| 407 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE) | |
| 408 { | |
| 578 | 409 if(duration > 4) /* avoid detection triggered by short moves */ |
| 574 | 410 { |
| 411 detectionState++; | |
| 412 } | |
| 578 | 413 exit = 1; |
| 414 lastStart = -2; | |
| 551 | 415 } |
| 416 break; | |
| 417 default: | |
| 418 detectionState = DETECT_NOTHING; | |
| 419 exit = 1; | |
| 420 break; | |
| 421 } | |
| 578 | 422 step--; |
| 423 } while((step >= 0) && (!exit)); | |
| 424 | |
| 425 if((lastStart < MOTION_DELTA_HISTORY_SIZE)) | |
| 426 { | |
| 427 lastStart++; /* prepare value for next iteration (history index will be increased) */ | |
| 428 } | |
| 429 else | |
| 430 { | |
| 431 lastStart = -1; | |
| 551 | 432 } |
| 433 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
|
434 { |
|
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
435 detectionState = DETECT_NOTHING; |
|
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
436 } |
| 551 | 437 else /* dont detect the same event twice */ |
| 438 { | |
| 439 resetMotionDeltaHistory(); | |
| 440 } | |
| 441 return detectionState; | |
| 442 } | |
| 363 | 443 |
| 551 | 444 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord) |
| 445 { | |
| 446 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); | |
| 447 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); | |
| 448 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z); | |
| 449 } | |
| 450 | |
| 451 SCoord CoordAdd(SCoord cA, SCoord cB) | |
| 452 { | |
| 453 SCoord result; | |
| 454 | |
| 455 result.x = cA.x + cB.x; | |
| 456 result.y = cA.y + cB.y; | |
| 457 result.z = cA.z + cB.z; | |
| 458 return result; | |
| 459 } | |
| 460 | |
| 461 SCoord CoordSub(SCoord cA, SCoord cB) | |
| 462 { | |
| 463 SCoord result; | |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
464 |
| 551 | 465 result.x = cA.x - cB.x; |
| 466 result.y = cA.y - cB.y; | |
| 467 result.z = cA.z - cB.z; | |
| 468 return result; | |
| 469 } | |
| 470 | |
| 471 SCoord CoordCross(SCoord cA, SCoord cB) | |
| 472 { | |
| 473 SCoord result; | |
| 474 | |
| 475 result.x = (cA.y * cB.z) - (cA.z * cB.y); | |
| 476 result.y = (cA.z * cB.x) - (cA.x * cB.z); | |
| 477 result.z = (cA.x * cB.y) - (cA.y * cB.x); | |
| 478 | |
| 479 return result; | |
| 480 | |
| 481 } | |
| 482 | |
| 483 SCoord CoordMulF(SCoord op, float factor) | |
| 484 { | |
| 485 SCoord result; | |
| 486 result.x = (op.x * factor); | |
| 487 result.y = (op.y * factor); | |
| 488 result.z = (op.z * factor); | |
| 489 | |
| 490 return result; | |
| 491 } | |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
492 |
| 551 | 493 SCoord CoordDivF(SCoord op, float factor) |
| 494 { | |
| 495 SCoord result; | |
| 496 result.x = (op.x / factor); | |
| 497 result.y = (op.y / factor); | |
| 498 result.z = (op.z / factor); | |
| 499 | |
| 500 return result; | |
| 501 } | |
| 502 | |
| 503 float CoordDot(SCoord cA, SCoord cB) | |
| 504 { | |
| 505 float result; | |
| 506 | |
| 507 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z; | |
| 508 return result; | |
| 509 } | |
| 510 | |
| 511 void calibrateViewport(float roll, float pitch, float yaw) | |
| 512 { | |
| 513 SSettings* pSettings = settingsGetPointer(); | |
| 514 | |
| 621 | 515 pSettings->viewPitch = pitch + 180; |
| 516 pSettings->viewRoll = roll+ 180; | |
| 551 | 517 pSettings->viewYaw = yaw; |
| 518 } | |
| 519 | |
| 520 | |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
521 float checkViewport(float roll, float pitch, float yaw, uint8_t enableAxis) |
| 551 | 522 { |
| 523 uint8_t retval = 0; | |
| 524 float angleYaw; | |
| 525 float anglePitch; | |
| 526 float angleRoll; | |
| 527 float distance = 0; | |
| 528 float _a, _b; | |
| 529 SCoord u,v,n; | |
| 627 | 530 float r = 0.0; |
| 531 float focusLimit = 0; | |
| 551 | 532 |
| 533 SCoord refVec; | |
| 534 SCoord axis_1; | |
| 535 SCoord axis_2; | |
| 536 SCoord curVec; | |
| 537 SCoord resultVec; | |
| 538 | |
| 627 | 539 SDeltaHistory movementDelta; |
| 540 | |
| 551 | 541 SSettings* pSettings = settingsGetPointer(); |
| 542 | |
| 621 | 543 roll += 180; |
| 544 pitch += 180; | |
| 545 | |
| 551 | 546 /* calculate base vector taking calibration delta into account yaw (heading) */ |
| 621 | 547 float compYaw; |
| 574 | 548 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
549 if(enableAxis & MOTION_ENABLE_YAW) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
550 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
551 compYaw = 360.0 - yaw; /* turn to 0° */ |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
552 compYaw += pSettings->viewYaw; /* consider calib yaw value */ |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
553 compYaw += yaw; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
554 |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
555 if (compYaw < 0.0) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
556 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
557 compYaw = 360.0 + compYaw; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
558 } |
| 574 | 559 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
560 if (compYaw > 360.0) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
561 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
562 compYaw = compYaw - 360.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
563 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
564 if (compYaw > 360.0) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
565 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
566 compYaw = compYaw - 360.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
567 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
568 angleYaw = pSettings->viewYaw * M_PI / 180.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
569 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
570 else |
| 551 | 571 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
572 compYaw = 0.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
573 angleYaw = 0.0; |
| 381 | 574 } |
| 551 | 575 |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
576 if(enableAxis & MOTION_ENABLE_PITCH) |
| 381 | 577 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
578 anglePitch = pSettings->viewPitch * M_PI / 180.0; |
|
373
7b981f8bdd41
Add scroll event by pitch angle detection:
ideenmodellierer
parents:
371
diff
changeset
|
579 } |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
580 else |
| 574 | 581 { |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
582 anglePitch = 0; |
| 574 | 583 } |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
584 if(enableAxis & MOTION_ENABLE_ROLL) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
585 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
586 angleRoll = pSettings->viewRoll * M_PI / 180.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
587 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
588 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
589 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
590 angleRoll = 0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
591 } |
| 551 | 592 |
| 593 refVec.x = 0; | |
| 594 refVec.y = 0; | |
| 595 refVec.z = 1.0; | |
| 596 | |
| 597 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec); | |
| 598 | |
| 599 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */ | |
| 600 axis_1.x = 0; | |
| 601 if(refVec.y >=0) | |
| 602 { | |
| 603 axis_2.y = 1; /* => Spawn y == refVec y */ | |
| 604 } | |
| 605 else axis_1.y = -1; | |
| 606 axis_1.z = -1.0 * refVec.y / refVec.z; | |
| 607 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */ | |
| 608 | |
| 609 /* check if detection plane is correct */ | |
| 610 u = CoordSub(axis_1,refVec); | |
| 611 v = CoordSub(axis_2,refVec); | |
| 612 n = CoordCross(u,v); | |
| 613 | |
| 614 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001)) | |
| 615 { | |
| 616 retval = 2; | |
| 617 } | |
| 618 else | |
| 619 { | |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
620 if(enableAxis & MOTION_ENABLE_PITCH) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
621 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
622 anglePitch = pitch * M_PI / 180.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
623 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
624 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
625 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
626 anglePitch = 0.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
627 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
628 if(enableAxis & MOTION_ENABLE_ROLL) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
629 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
630 angleRoll = roll * M_PI / 180.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
631 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
632 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
633 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
634 angleRoll = 0.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
635 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
636 if(enableAxis & MOTION_ENABLE_YAW) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
637 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
638 angleYaw = compYaw * M_PI / 180.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
639 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
640 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
641 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
642 angleYaw = 0.0; |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
643 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
644 |
| 551 | 645 curVec.x = 0; |
| 646 curVec.y = 0; | |
| 647 curVec.z = 1.0; | |
| 648 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec); | |
| 649 | |
| 650 _a = CoordDot(curVec,n); | |
| 651 _b = CoordDot(refVec,n); | |
| 652 | |
| 653 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */ | |
| 654 { | |
| 655 retval = 3; | |
| 656 } | |
| 657 else | |
| 658 { | |
| 659 r=_a/_b; | |
| 660 if(r<0.00||r>1.40) /* are we looking into wrong direction? */ | |
| 661 { | |
| 662 retval = 4; | |
| 663 } | |
| 664 } | |
| 665 distance = retval * 1.0; /* just for debugging */ | |
| 666 if(retval == 0) | |
| 667 { | |
| 668 /* start calculating the matchpoint */ | |
| 669 curVec = CoordMulF(curVec,r); | |
| 670 resultVec = CoordSub(refVec,curVec); | |
| 671 | |
| 672 /* calculate the distance between reference and actual vector */ | |
| 673 resultVec.x = resultVec.x * resultVec.x; | |
| 674 resultVec.y = resultVec.y * resultVec.y; | |
| 675 resultVec.z = resultVec.z * resultVec.z; | |
| 676 | |
| 677 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0)) | |
| 678 { | |
| 679 distance = 0.0; | |
| 680 } | |
| 681 else | |
| 682 { | |
| 683 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z)); | |
| 684 } | |
| 685 } | |
| 686 } | |
| 687 | |
| 627 | 688 movementDelta = GetDeltaHistory(0); |
| 689 | |
| 690 if(inFocus == 0) /* consider option to use smaller spot to detect focus state */ | |
| 691 { | |
| 692 focusLimit = MOTION_FOCUS_LIMIT - (((pSettings->viewPortMode >> 5) & 0x03) / 10.0); | |
| 693 } | |
| 694 else | |
| 695 { | |
| 696 focusLimit = MOTION_FOCUS_LIMIT; /* use standard spot to detect diver interactions */ | |
| 697 } | |
| 698 | |
| 699 if((distance <= focusLimit) && (movementDelta.yaw != MOTION_DELTA_RAISE_FAST) && (movementDelta.yaw != MOTION_DELTA_FALL_FAST)) /* handle focus counter to avoid fast in/out focus changes */ | |
| 551 | 700 { |
| 701 if(focusCnt < 10) | |
| 702 { | |
| 703 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */ | |
| 704 { | |
| 705 resetMotionDeltaHistory(); | |
| 706 } | |
| 707 focusCnt++; | |
| 708 } | |
| 574 | 709 if((focusCnt == 10) && (inFocus == 0)) |
| 551 | 710 { |
| 711 inFocus = 1; | |
| 712 } | |
| 713 } | |
| 714 else | |
| 715 { | |
| 627 | 716 if((movementDelta.yaw > MOTION_DELTA_JITTER ) && (focusCnt >= 5)) |
| 717 { | |
| 718 focusCnt--; | |
| 719 } | |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
720 if(focusCnt >= 5) /* Reset focus faster then setting focus */ |
| 551 | 721 { |
| 722 focusCnt--; | |
| 723 } | |
| 724 else | |
| 725 { | |
| 574 | 726 focusCnt = 0; |
| 551 | 727 inFocus = 0; |
| 728 } | |
| 729 } | |
| 627 | 730 if ((r<1) && (retval == 0)) /* add direction information to distance */ |
| 731 { | |
| 732 distance *= -1.0; | |
| 733 } | |
| 551 | 734 return distance; |
|
359
4258ea9b67fa
Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff
changeset
|
735 } |
| 551 | 736 uint8_t viewInFocus(void) |
| 737 { | |
| 738 return inFocus; | |
| 739 } | |
| 740 void resetFocusState(void) | |
| 741 { | |
| 742 inFocus = 0; | |
| 743 } | |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
744 |
|
642
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
745 uint8_t viewDetectionSuspended(void) |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
746 { |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
747 uint8_t retVal = 0; |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
748 |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
749 if(suspendMotionDetectionSec) |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
750 { |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
751 retVal = 1; |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
752 } |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
753 return retVal; |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
754 } |
|
c737cf5d9067
Do not show focus indicator in case motion detection is suspended:
Ideenmodellierer
parents:
627
diff
changeset
|
755 |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
756 void suspendMotionDetection(uint8_t seconds) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
757 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
758 suspendMotionDetectionSec = seconds * 10; /* detection function is called every 100ms */ |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
759 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
760 |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
761 void HandleMotionDetection(void) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
762 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
763 detectionState_t pitchstate = DETECT_NOTHING; |
|
618
96af74455420
Development bugfix: focus state not remembered:
Ideenmodellierer
parents:
611
diff
changeset
|
764 static uint8_t wasInFocus = 0; |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
765 float focusOffset = 0.0; |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
766 |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
767 evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading); |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
768 if(viewInFocus()) |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
769 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
770 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, (MOTION_ENABLE_PITCH | MOTION_ENABLE_YAW)); |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
771 } |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
772 else |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
773 { |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
774 focusOffset = checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading, MOTION_ENABLE_ALL); |
|
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
775 } |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
776 if(viewInFocus()) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
777 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
778 wasInFocus = 1; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
779 set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
780 |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
781 if(suspendMotionDetectionSec == 0) /* suspend detection while diver is manually operating the OSTC */ |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
782 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
783 switch(settingsGetPointer()->MotionDetection) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
784 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
785 case MOTION_DETECT_MOVE: pitchstate = detectPitch(stateRealGetPointer()->lifeData.compass_pitch); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
786 break; |
|
625
028d8f3a9410
Switch Sector / Scroll detection to vector implementation:
Ideenmodellierer
parents:
621
diff
changeset
|
787 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(focusOffset); |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
788 break; |
| 627 | 789 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(fabs(focusOffset)); |
|
611
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
790 break; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
791 default: |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
792 pitchstate = DETECT_NOTHING; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
793 break; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
794 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
795 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
796 |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
797 if(DETECT_NEG_PITCH == pitchstate) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
798 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
799 StoreButtonAction((uint8_t)ACTION_PITCH_NEG); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
800 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
801 if(DETECT_POS_PITCH == pitchstate) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
802 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
803 StoreButtonAction((uint8_t)ACTION_PITCH_POS); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
804 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
805 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
806 else |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
807 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
808 if(wasInFocus) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
809 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
810 wasInFocus = 0; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
811 if(suspendMotionDetectionSec == 0) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
812 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
813 if(settingsGetPointer()->design == 7) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
814 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
815 t7_set_customview_to_primary(); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
816 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
817 else |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
818 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
819 t3_set_customview_to_primary(); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
820 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
821 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
822 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
823 set_Backlight_Boost(0); |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
824 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
825 if(suspendMotionDetectionSec != 0) |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
826 { |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
827 suspendMotionDetectionSec--; |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
828 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
829 } |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
830 |
|
916998f90e39
Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents:
595
diff
changeset
|
831 |