annotate Discovery/Src/motion.c @ 618:96af74455420

Development bugfix: focus state not remembered: Somehow the static in front of the focus state has disappeared. After returning it again the focus detection is available again.
author Ideenmodellierer
date Wed, 27 Jan 2021 22:04:45 +0100
parents 916998f90e39
children 6826731ff2be
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
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
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
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
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
32 #define MOTION_DELTA_STABLE 0
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
33 #define MOTION_DELTA_JITTER 1
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
34 #define MOTION_DELTA_RAISE 2
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
35 #define MOTION_DELTA_FALL 3
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
36
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
37 #define MOTION_DELTA_JITTER_LEVEL 3.0 /* lower values are considered as stable */
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
38 #define MOTION_DELTA_RAISE_LEVEL 6.0 /* Movement causing a significant change detected */
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
39 #define MOTION_DELTA_FALL_LEVEL -6.0 /* Movement causing a significant change detected */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
40
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
41 #define MOTION_DELTA_HISTORY_SIZE 20 /* Number of history data sets */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
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
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
46 static uint8_t motionDeltaHistory[3][MOTION_DELTA_HISTORY_SIZE]; /* Change history of roll, pitch and yaw */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
47 static uint8_t motionDeltaHistoryIdx; /* Current index of history data */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
48
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
49 static uint8_t focusCnt = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
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
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
52
611
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
53 static uint8_t suspendMotionDetectionSec = 0;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
54
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
55 void resetMotionDeltaHistory()
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
56 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
57 motionDeltaHistoryIdx = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
58 memset(motionDeltaHistory, 0, sizeof(motionDeltaHistory));
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
59 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
60
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
61 void evaluateMotionDelta(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
62 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
63 static float lastValue[3] = {0.0,0.0,0.0};
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
64 uint8_t nextIndex = motionDeltaHistoryIdx + 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
65 uint8_t axis;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
66 float curValue;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
67
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
68 if(nextIndex == MOTION_DELTA_HISTORY_SIZE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
69 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
70 nextIndex = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
71 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
72 for(axis=0; axis < 3; axis++)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
73 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
74 switch(axis)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
75 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
76 case MOTION_HISTORY_ROLL: curValue = roll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
77 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
78 case MOTION_HISTORY_PITCH: curValue = pitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
79 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
80 default:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
81 case MOTION_HISTORY_YAW: if((yaw < 90) && (lastValue[MOTION_HISTORY_YAW] > 270.0)) /* transition 360 => 0 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
82 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
83 lastValue[MOTION_HISTORY_YAW] -= 360;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
84 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
85 else if((yaw > 270) && (lastValue[MOTION_HISTORY_YAW] < 90.0)) /* transition 0 => 360 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
86 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
87 lastValue[MOTION_HISTORY_YAW] += 360;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
88 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
89 curValue = yaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
90 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
91 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
92 if(curValue - lastValue[axis] > MOTION_DELTA_RAISE_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
93 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
94 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_RAISE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
95 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
96 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_RAISE_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
97 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
98 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_JITTER;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
99 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
100 if(fabsf(curValue - lastValue[axis]) < MOTION_DELTA_JITTER_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
101 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
102 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_STABLE;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
103 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
104 if(curValue - lastValue[axis] < MOTION_DELTA_FALL_LEVEL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
105 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
106 motionDeltaHistory[axis][nextIndex] = MOTION_DELTA_FALL;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
107 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
108 lastValue[axis] = curValue;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
109 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
110 motionDeltaHistoryIdx = nextIndex;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
111 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
112
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
113 SDeltaHistory GetDeltaHistory(uint8_t stepback)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
114 {
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
115 uint8_t loop;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
116 uint8_t index = motionDeltaHistoryIdx;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
117
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
118 SDeltaHistory result = {0,0,0};
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
119
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
120 stepback++; /* motionDeltaHistoryIdx is pointing to future entry => step back one to get the latest */
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
121 loop = stepback;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
122 if(stepback < MOTION_DELTA_HISTORY_SIZE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
123 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
124 while(loop != 0) /* find requested entry */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
125 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
126 loop--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
127 index--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
128 if(index == 0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
129 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
130 index = MOTION_DELTA_HISTORY_SIZE - 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
131 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
132 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
133 result.roll = motionDeltaHistory[MOTION_HISTORY_ROLL][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
134 result.pitch = motionDeltaHistory[MOTION_HISTORY_PITCH][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
135 result.yaw = motionDeltaHistory[MOTION_HISTORY_YAW][index];
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
136 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
137 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
138 }
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
139
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
140 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
141 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
142 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
143 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
144 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
145
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
146 newPitch = pitch + sectorDetection.offset + sectorDetection.center; /* do not use negative values and consider offset to center position */
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
147 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
148 {
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
149 newPitch = 0.0;
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
150 }
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
151 if (newPitch > sectorDetection.window) /* clip value */
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
152 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
153 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
154 }
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
155
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
156 /* switch to other sector? */
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
157 if((newPitch > sectorDetection.upperborder) || (newPitch <= sectorDetection.lowerborder))
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
158 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
159 sector = (uint16_t) newPitch / sectorDetection.size;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
160 sectorDetection.lowerborder = sector * sectorDetection.size - SECTOR_HYSTERY;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
161 sectorDetection.upperborder = (sector + 1) * sectorDetection.size + SECTOR_HYSTERY;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
162 lastsector = sector;
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
163 }
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
164
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
165 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
166 }
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 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
169 {
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
170 if(numOfSectors == CUSTOMER_DEFINED_VIEWS)
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
171 {
384
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
172 if(settingsGetPointer()->design == 3) /* Big font view ? */
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 sectorDetection.count = t3_GetEnabled_customviews();
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 else
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
177 {
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
178 sectorDetection.count = t7_GetEnabled_customviews();
427ae9f8e28e Consider number of available t3 views in sector handling:
ideenmodellierer
parents: 383
diff changeset
179 }
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
180 if(sectorDetection.count > SECTOR_MAX_CNT)
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
181 {
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
182 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
183 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
184 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
185 else
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
186 if(numOfSectors != CUSTOMER_KEEP_LAST_SECTORS)
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
187 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
188 sectorDetection.count = numOfSectors;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
189 }
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 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
192 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
193 sectorDetection.window = SECTOR_WINDOW_MAX;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
194 }
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
195 else
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
196 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
197 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
198 }
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
199
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
200 sectorDetection.offset = (centerPitch - (sectorDetection.window / 2)) * -1.0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
201 sectorDetection.size = sectorDetection.window / sectorDetection.count;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
202 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
203
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
204 /* reset border values */
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
205 sectorDetection.lowerborder = SECTOR_BORDER;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
206 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
207 /* get the current sector */
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
208 sectorDetection.current = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
209 sectorDetection.target = sectorDetection.current;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
210 /* 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
211 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
212
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
213 }
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
214
592
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 uint8_t GetCVForSector(uint8_t selSector)
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 if(selSector < sectorDetection.count)
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 return sectorMap[selSector];
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 else
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 return 0;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
225 }
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 void MapCVToSector()
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 uint8_t centerView = 0;
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 memset(sectorMap, 0, sizeof(sectorMap));
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 switch(sectorDetection.count)
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
234 {
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
235 case 1: centerView = 0; break;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
236 case 2: centerView = 0; break;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
237 case 3: centerView = 1; break;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
238 case 4: centerView = 1; break;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
239 case 5: centerView = 2; break;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
240 default: centerView = sectorDetection.count / 2 - 1;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
241 break;
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 if(settingsGetPointer()->design == 3) /* Big font view ? */
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
244 {
595
fd0b60dee6f3 Bugfix Selection of primary view:
Ideenmodellierer
parents: 592
diff changeset
245 t3_set_customview_to_primary();
fd0b60dee6f3 Bugfix Selection of primary view:
Ideenmodellierer
parents: 592
diff changeset
246 sectorMap[centerView] = t3_change_customview(ACTION_END);
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
247 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
248 else
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
249 {
595
fd0b60dee6f3 Bugfix Selection of primary view:
Ideenmodellierer
parents: 592
diff changeset
250 t7_set_customview_to_primary();
fd0b60dee6f3 Bugfix Selection of primary view:
Ideenmodellierer
parents: 592
diff changeset
251 sectorMap[centerView] = t7_change_customview(ACTION_END);
fd0b60dee6f3 Bugfix Selection of primary view:
Ideenmodellierer
parents: 592
diff changeset
252
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
253 }
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 centerView++;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
256 while(sectorMap[centerView] == 0)
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
257 {
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
258 if(settingsGetPointer()->design == 3) /* Big font view ? */
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
259 {
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
260 sectorMap[centerView] = t3_change_customview(ACTION_BUTTON_ENTER);
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
261 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
262 else
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 sectorMap[centerView] = t7_change_customview(ACTION_BUTTON_ENTER);
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 centerView++;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
267 if(centerView == sectorDetection.count)
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 centerView = 0;
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
270 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
271 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
272
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
273 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
274
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 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
276 {
383
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
277 sectorDetection.target = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
278 sectorDetection.current = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
279 sectorDetection.size = 0;
49a02dea8ae3 Combine variables in a structure to improve readability of code
ideenmodellierer
parents: 381
diff changeset
280 sectorDetection.count = 0;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
281
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
282 switch(settingsGetPointer()->MotionDetection)
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
283 {
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
284 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
285 MapCVToSector();
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
286 break;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
287 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
288 break;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
289 case MOTION_DETECT_SCROLL: DefinePitchSectors(settingsGetPointer()->viewPitch,SECTOR_SCROLL);
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
290 break;
371
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
291 default:
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
292 break;
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
293 }
fca370f847f8 Added parameter for number of sectors to be defined
ideenmodellierer
parents: 370
diff changeset
294
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
295 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
296 }
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 /* 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
299 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
300 {
77cdfbdaca8c Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
parents: 363
diff changeset
301 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
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 newTargetSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
304
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
305 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
306 {
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
307 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
308 }
592
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
309 else
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
310 {
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
311 t7_select_customview(GetCVForSector(newTargetSector));
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
312 }
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
313
f52bc70e380f MotionCtrl - Sectorview Map sectors directly to custom views:
Ideenmodellierer
parents: 585
diff changeset
314 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
315 }
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 /* 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
318 detectionState_t detectScrollButtonEvent(float curPitch)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
319 {
378
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
320 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
321
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
322 uint8_t PitchEvent = DETECT_NOTHING;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
323 uint8_t newSector;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
324
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
325 if(delayscroll == 0)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
326 {
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
327 newSector = GetSectorForPitch(stateRealGetPointer()->lifeData.compass_pitch);
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
328 /* 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
329 switch(newSector)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
330 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
331 case 0: PitchEvent = DETECT_POS_PITCH;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
332 break;
378
834e087505ec Incremented "idle window" for scroll detection
ideenmodellierer
parents: 373
diff changeset
333 case 6: PitchEvent = DETECT_NEG_PITCH;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
334 break;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
335 default:
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
336 break;
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 if(PitchEvent != DETECT_NOTHING)
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
339 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
340 delayscroll = 7;
373
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 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
343 else
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 delayscroll--;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
346 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
347 return PitchEvent;
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
348 }
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
349
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
350
370
77cdfbdaca8c Changed function names from shake to pitch and improved detection function: Shake might be confusing for people reading the code because pitch values are ased for calculation => changed name to pitch to be more transparent
ideenmodellierer
parents: 363
diff changeset
351 /* 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
352 /* 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
353 detectionState_t detectPitch(float currentPitch)
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
354 {
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
355 static int8_t lastStart = 0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
356 uint8_t exit = 0;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
357 int8_t step = 0;
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
358 uint8_t duration = 0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
359 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
360
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
361 if(lastStart < 0)
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
362 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
363 detectionState = DETECT_NOTHING;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
364 lastStart = 0;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
365 }
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
366 else
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
367 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
368 detectionState = DETECT_START;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
369 }
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
370 step = lastStart;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
371 do
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
372 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
373 test = GetDeltaHistory(step);
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
374 duration++;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
375 switch (detectionState)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
376 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
377 case DETECT_NOTHING: if(test.pitch > MOTION_DELTA_STABLE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
378 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
379 exit = 1;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
380 lastStart = -2;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
381 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
382 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
383 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
384 detectionState = DETECT_START;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
385 lastStart = -1;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
386 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
387 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
388 case DETECT_START: if(test.pitch == MOTION_DELTA_RAISE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
389 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
390 detectionState = DETECT_POS_MOVE;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
391 lastStart = step;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
392 }
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
393 else
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
394 if(test.pitch == MOTION_DELTA_FALL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
395 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
396 detectionState = DETECT_NEG_MOVE;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
397 lastStart = step;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
398 }
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
399 else
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
400 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
401 lastStart = -1;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
402 }
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
403 duration = 0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
404 break;
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
405 case DETECT_NEG_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_RAISE))
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
406 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
407 detectionState++;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
408 }
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
409 break;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
410 case DETECT_POS_MOVE: if((test.pitch <= MOTION_DELTA_JITTER) || (test.pitch == MOTION_DELTA_FALL))
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
411 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
412 detectionState++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
413 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
414 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
415 case DETECT_MAXIMA: if(test.pitch == MOTION_DELTA_FALL)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
416 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
417 detectionState = DETECT_FALLBACK;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
418 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
419 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
420 case DETECT_MINIMA: if(test.pitch == MOTION_DELTA_RAISE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
421 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
422 detectionState = DETECT_RISEBACK;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
423 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
424 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
425 case DETECT_RISEBACK:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
426 case DETECT_FALLBACK: if(test.pitch == MOTION_DELTA_STABLE)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
427 {
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
428 if(duration > 4) /* avoid detection triggered by short moves */
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
429 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
430 detectionState++;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
431 }
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
432 exit = 1;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
433 lastStart = -2;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
434 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
435 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
436 default:
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
437 detectionState = DETECT_NOTHING;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
438 exit = 1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
439 break;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
440 }
578
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
441 step--;
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
442 } while((step >= 0) && (!exit));
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
443
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
444 if((lastStart < MOTION_DELTA_HISTORY_SIZE))
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
445 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
446 lastStart++; /* prepare value for next iteration (history index will be increased) */
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
447 }
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
448 else
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
449 {
beb4d47542f1 Rework pitch detection:
Ideenmodellierer
parents: 574
diff changeset
450 lastStart = -1;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
451 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
452 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
453 {
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
454 detectionState = DETECT_NOTHING;
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
455 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
456 else /* dont detect the same event twice */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
457 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
458 resetMotionDeltaHistory();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
459 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
460 return detectionState;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
461 }
363
bdf978d2a5d4 Reworked detection function
ideenmodellierer
parents: 362
diff changeset
462
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
463 void anglesToCoord(float roll, float pitch, float yaw, SCoord *pCoord)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
464 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
465 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);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
466 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);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
467 pCoord->z = ((-1*sinf(pitch)) * pCoord->x + (cosf(pitch) *sinf(roll)) * pCoord->y + (cosf(pitch) * cosf(roll))* pCoord->z);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
468 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
469
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
470 SCoord CoordAdd(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
471 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
472 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
473
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
474 result.x = cA.x + cB.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
475 result.y = cA.y + cB.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
476 result.z = cA.z + cB.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
477 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
478 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
479
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
480 SCoord CoordSub(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
481 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
482 SCoord result;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
483
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
484 result.x = cA.x - cB.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
485 result.y = cA.y - cB.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
486 result.z = cA.z - cB.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
487 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
488 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
489
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
490 SCoord CoordCross(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
491 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
492 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
493
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
494 result.x = (cA.y * cB.z) - (cA.z * cB.y);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
495 result.y = (cA.z * cB.x) - (cA.x * cB.z);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
496 result.z = (cA.x * cB.y) - (cA.y * cB.x);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
497
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
498 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
499
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
500 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
501
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
502 SCoord CoordMulF(SCoord op, float factor)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
503 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
504 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
505 result.x = (op.x * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
506 result.y = (op.y * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
507 result.z = (op.z * factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
508
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
509 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
510 }
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
511
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
512 SCoord CoordDivF(SCoord op, float factor)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
513 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
514 SCoord result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
515 result.x = (op.x / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
516 result.y = (op.y / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
517 result.z = (op.z / factor);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
518
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
519 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
520 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
521
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
522 float CoordDot(SCoord cA, SCoord cB)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
523 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
524 float result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
525
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
526 result = cA.x * cB.x + cA.y * cB.y + cB.z*cA.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
527 return result;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
528 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
529
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
530 void calibrateViewport(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
531 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
532 SSettings* pSettings = settingsGetPointer();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
533
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
534 pSettings->viewPitch = pitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
535 pSettings->viewRoll = roll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
536 pSettings->viewYaw = yaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
537 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
538
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
539
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
540 float checkViewport(float roll, float pitch, float yaw)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
541 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
542 static float freezeRoll = 0;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
543 static float freezeYaw = 0;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
544
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
545 uint8_t retval = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
546 float angleYaw;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
547 float anglePitch;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
548 float angleRoll;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
549 float distance = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
550 float _a, _b;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
551 SCoord u,v,n;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
552 float r;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
553
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
554 SCoord refVec;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
555 SCoord axis_1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
556 SCoord axis_2;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
557 SCoord curVec;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
558 SCoord resultVec;
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
559 SDeltaHistory test;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
560
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
561 SSettings* pSettings = settingsGetPointer();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
562
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
563 /* calculate base vector taking calibration delta into account yaw (heading) */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
564 float compYaw = yaw + pSettings->viewYaw;
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
565
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
566 compYaw = 360.0 - yaw; /* turn to 0° */
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
567 compYaw += pSettings->viewYaw; /* consider calib yaw value */
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
568 compYaw += yaw;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
569
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
570 if (compYaw < 0.0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
571 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
572 compYaw = 360.0 + compYaw;
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
573 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
574
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
575 if (compYaw > 360.0)
381
695434a6dcf6 Simplified pitch detection state machine:
ideenmodellierer
parents: 378
diff changeset
576 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
577 compYaw = compYaw - 360.0;
373
7b981f8bdd41 Add scroll event by pitch angle detection:
ideenmodellierer
parents: 371
diff changeset
578 }
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
579 if (compYaw > 360.0)
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
580 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
581 compYaw = compYaw - 360.0;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
582 }
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
583 angleYaw = pSettings->viewYaw * M_PI / 180.0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
584 anglePitch = pSettings->viewPitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
585 angleRoll = pSettings->viewRoll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
586
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
587 refVec.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
588 refVec.y = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
589 refVec.z = 1.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
590
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
591 anglesToCoord(angleRoll,anglePitch,angleYaw, &refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
592
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
593 anglePitch = pitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
594 angleRoll = roll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
595 angleYaw = yaw * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
596
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
597 /* assume x = 0 and y = 1 => find matching vector so axis_1 is 90° to axis_2 */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
598 axis_1.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
599 if(refVec.y >=0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
600 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
601 axis_2.y = 1; /* => Spawn y == refVec y */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
602 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
603 else axis_1.y = -1;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
604 axis_1.z = -1.0 * refVec.y / refVec.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
605 axis_2 = CoordCross(refVec, axis_1); /* Cross is 90° to refVec and Spawn as well => Plane Spawn / cross */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
606
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
607 /* check if detection plane is correct */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
608 u = CoordSub(axis_1,refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
609 v = CoordSub(axis_2,refVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
610 n = CoordCross(u,v);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
611
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
612 if((fabsf(n.x) <= 0.0001) && (fabsf(n.y) <= 0.0001) && (fabsf(n.z) <= 0.0001))
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
613 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
614 retval = 2;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
615 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
616 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
617 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
618 angleYaw = compYaw * M_PI / 180.0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
619 anglePitch = pitch * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
620 angleRoll = roll * M_PI / 180.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
621 curVec.x = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
622 curVec.y = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
623 curVec.z = 1.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
624 anglesToCoord(angleRoll,anglePitch,angleYaw, &curVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
625
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
626 _a = CoordDot(curVec,n);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
627 _b = CoordDot(refVec,n);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
628
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
629 if(_b>=(-0.0001)&&_b<=0.0001) /* Check if view port is parallel (no matchpoint) */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
630 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
631 retval = 3;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
632 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
633 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
634 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
635 r=_a/_b;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
636 if(r<0.00||r>1.40) /* are we looking into wrong direction? */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
637 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
638 retval = 4;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
639 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
640 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
641 distance = retval * 1.0; /* just for debugging */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
642 if(retval == 0)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
643 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
644
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
645 /* start calculating the matchpoint */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
646 curVec = CoordMulF(curVec,r);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
647 resultVec = CoordSub(refVec,curVec);
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
648
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
649 /* calculate the distance between reference and actual vector */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
650 resultVec.x = resultVec.x * resultVec.x;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
651 resultVec.y = resultVec.y * resultVec.y;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
652 resultVec.z = resultVec.z * resultVec.z;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
653
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
654 if((resultVec.x == 0) && (resultVec.y == 0) && (resultVec.z == 0))
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
655 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
656 distance = 0.0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
657 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
658 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
659 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
660 distance = sqrtf((resultVec.x + resultVec.y + resultVec.z));
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
661 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
662 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
663 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
664
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
665 if(distance < 0.5) /* handle focus counter to avoid fast in/out focus changes */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
666 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
667 if(focusCnt < 10)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
668 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
669 if((focusCnt == 9) && (inFocus == 0)) /* we will get into focus */
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
670 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
671 resetMotionDeltaHistory();
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
672 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
673 focusCnt++;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
674 }
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
675 if((focusCnt == 10) && (inFocus == 0))
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
676 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
677 inFocus = 1;
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
678 freezeRoll = roll;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
679 freezeYaw = yaw;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
680 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
681 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
682 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
683 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
684 if(focusCnt >= 5) /* Reset focus faster then setting focus */
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
685 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
686 if(pSettings->MotionDetection != MOTION_DETECT_MOVE) /* only apply extended focus for methods using absolute pitch values */
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
687 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
688 test = GetDeltaHistory(0);
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
689 if((test.yaw == MOTION_DELTA_STABLE) && (test.roll == MOTION_DELTA_STABLE))
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
690 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
691 if((fabsf(freezeRoll - roll) < MOTION_DELTA_JITTER_LEVEL) && (fabsf(freezeYaw - yaw) < MOTION_DELTA_JITTER_LEVEL))
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
692 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
693 focusCnt++;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
694 }
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
695 }
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
696 else
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
697 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
698 if(freezeRoll != 0.0)
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
699 {
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
700 focusCnt = 1;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
701 }
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
702 }
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
703 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
704 focusCnt--;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
705 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
706 else
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
707 {
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
708 focusCnt = 0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
709 inFocus = 0;
574
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
710 freezeRoll = 0;
01ee21dd311f Improved focus detection:
Ideenmodellierer
parents: 551
diff changeset
711 freezeYaw = 0;
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
712 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
713 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
714 return distance;
359
4258ea9b67fa Added new files for motion detection (shaking) detection
ideenmodellierer
parents:
diff changeset
715 }
551
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
716 uint8_t viewInFocus(void)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
717 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
718 return inFocus;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
719 }
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
720 void resetFocusState(void)
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
721 {
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
722 inFocus = 0;
e3237f580ae9 Added viewport functionality:
Ideenmodellierer
parents: 385
diff changeset
723 }
611
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
724
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
725 void suspendMotionDetection(uint8_t seconds)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
726 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
727 suspendMotionDetectionSec = seconds * 10; /* detection function is called every 100ms */
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
728 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
729
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
730 void HandleMotionDetection(void)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
731 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
732 detectionState_t pitchstate = DETECT_NOTHING;
618
96af74455420 Development bugfix: focus state not remembered:
Ideenmodellierer
parents: 611
diff changeset
733 static uint8_t wasInFocus = 0;
611
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
734
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
735 evaluateMotionDelta(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
736 checkViewport(stateUsed->lifeData.compass_roll, stateUsed->lifeData.compass_pitch, stateUsed->lifeData.compass_heading);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
737
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
738 if(viewInFocus())
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
739 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
740 wasInFocus = 1;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
741 set_Backlight_Boost(settingsGetPointer()->viewPortMode & 0x03);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
742
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
743 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
744 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
745 switch(settingsGetPointer()->MotionDetection)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
746 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
747 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
748 break;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
749 case MOTION_DETECT_SECTOR: pitchstate = detectSectorButtonEvent(stateRealGetPointer()->lifeData.compass_pitch);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
750 break;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
751 case MOTION_DETECT_SCROLL: pitchstate = detectScrollButtonEvent(stateRealGetPointer()->lifeData.compass_pitch);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
752 break;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
753 default:
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
754 pitchstate = DETECT_NOTHING;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
755 break;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
756 }
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
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
759 if(DETECT_NEG_PITCH == pitchstate)
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 StoreButtonAction((uint8_t)ACTION_PITCH_NEG);
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 if(DETECT_POS_PITCH == pitchstate)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
764 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
765 StoreButtonAction((uint8_t)ACTION_PITCH_POS);
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 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
768 else
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
769 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
770 if(wasInFocus)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
771 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
772 wasInFocus = 0;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
773 if(suspendMotionDetectionSec == 0)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
774 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
775 if(settingsGetPointer()->design == 7)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
776 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
777 t7_set_customview_to_primary();
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
778 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
779 else
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 t3_set_customview_to_primary();
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 }
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 set_Backlight_Boost(0);
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
786 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
787 if(suspendMotionDetectionSec != 0)
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
788 {
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
789 suspendMotionDetectionSec--;
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
790 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
791 }
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
792
916998f90e39 Suspend MotionDetection events while diver is operationg the OSTC:
Ideenmodellierer
parents: 595
diff changeset
793