Mercurial > public > ostc4
annotate Discovery/Src/motion.c @ 809:9602a7338f28
DevBugfix CO2 sensor in streaming mode:
In standalone mode (no multiplexer) the CO2 sensor is working in streaming mode => it could happen that the OSTC received data before the scaling factor was requested. As result the CO2 values were calculated with a zero value. This problem has been fixed.
In addition some code cleanup has been done. Unifying usage of changeBaudrate function instead of old baud selection based on protocol request of Discovery firmware.
| author | Ideenmodellierer |
|---|---|
| date | Sun, 27 Aug 2023 20:51:13 +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 |