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