1 /* |
|
2 * Copyright (c) 2007 Nokia Corporation and/or its subsidiary(-ies). |
|
3 * All rights reserved. |
|
4 * This component and the accompanying materials are made available |
|
5 * under the terms of "Eclipse Public License v1.0" |
|
6 * which accompanies this distribution, and is available |
|
7 * at the URL "http://www.eclipse.org/legal/epl-v10.html". |
|
8 * |
|
9 * Initial Contributors: |
|
10 * Nokia Corporation - initial contribution. |
|
11 * |
|
12 * Contributors: |
|
13 * |
|
14 * Description: Compensate function definition. |
|
15 * |
|
16 */ |
|
17 |
|
18 |
|
19 #include <e32math.h> |
|
20 #include <tiltcompensation.h> |
|
21 #include <e32debug.h> |
|
22 |
|
23 //----------------------------------------------------------------------------- |
|
24 // Direction6D |
|
25 //----------------------------------------------------------------------------- |
|
26 // |
|
27 EXPORT_C TInt Compensate( |
|
28 const TTiltCompensationInput& aInput, |
|
29 TTiltCompensationOutput& aOutput, |
|
30 const TInt16 aPreviousTheta |
|
31 ) |
|
32 { |
|
33 TReal target; |
|
34 TReal source( 0 ); |
|
35 TReal inputY( aInput.iMagneticVector.iY ); |
|
36 TReal inputZ( aInput.iMagneticVector.iZ ); |
|
37 |
|
38 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 ) |
|
39 // 0 - 90 degrees |
|
40 { |
|
41 source = inputY / inputZ; |
|
42 } |
|
43 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 ) |
|
44 // 90 - 180 degrees |
|
45 { |
|
46 source = inputZ / inputY * -1; |
|
47 } |
|
48 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 ) |
|
49 // 180 - 270 degrees |
|
50 { |
|
51 source = inputY / inputZ; |
|
52 } |
|
53 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 ) |
|
54 // 270 - 360 degrees |
|
55 { |
|
56 source = inputZ / inputY * -1; |
|
57 } |
|
58 |
|
59 Math::ATan( target, source ); |
|
60 TInt16 declination( ( target * 180 ) / 3.14 ); |
|
61 |
|
62 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 ) |
|
63 // 0 - 90 degrees |
|
64 { |
|
65 } |
|
66 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 ) |
|
67 // 90 - 180 degrees |
|
68 { |
|
69 declination = declination + 90; |
|
70 } |
|
71 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 ) |
|
72 // 180 - 270 degrees |
|
73 { |
|
74 declination = declination + 180; |
|
75 } |
|
76 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 ) |
|
77 // 270 - 360 degrees |
|
78 { |
|
79 declination = declination + 270; |
|
80 } |
|
81 |
|
82 if( inputY == 0 && inputZ > 0 ) |
|
83 { |
|
84 declination = 0; |
|
85 } |
|
86 if( inputY > 0 && inputZ == 0 ) |
|
87 { |
|
88 declination = 90; |
|
89 } |
|
90 if( inputY == 0 && inputZ < 0 ) |
|
91 { |
|
92 declination = 180; |
|
93 } |
|
94 if( inputY < 0 && inputZ == 0 ) |
|
95 { |
|
96 declination = 270; |
|
97 } |
|
98 |
|
99 if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) ) |
|
100 { |
|
101 aOutput.iTheta = aPreviousTheta; |
|
102 } |
|
103 |
|
104 aOutput.iTheta = declination; |
|
105 return KErrNone; |
|
106 } |
|
107 |
|
108 //----------------------------------------------------------------------------- |
|
109 // Direction6D |
|
110 //----------------------------------------------------------------------------- |
|
111 // |
|
112 EXPORT_C TInt Compensate( |
|
113 const TTiltCompensationInput& aInput, |
|
114 TTiltCompensationOutput& aOutput, |
|
115 const TInt16 aPreviousTheta, |
|
116 const RParamsArray& aParamsArray ) |
|
117 { |
|
118 TReal target; |
|
119 TReal source( 0 ); |
|
120 TReal inputY( aInput.iMagneticVector.iY ); |
|
121 TReal inputX( aInput.iMagneticVector.iX ); |
|
122 |
|
123 // Get parameters |
|
124 for( TInt i = 0; i != aParamsArray.Count(); i++ ) |
|
125 { |
|
126 TInt parameter = aParamsArray[ i ]; |
|
127 } |
|
128 |
|
129 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 ) |
|
130 // 0 - 90 degrees |
|
131 { |
|
132 source = inputX / inputY; |
|
133 } |
|
134 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 ) |
|
135 // 90 - 180 degrees |
|
136 { |
|
137 source = inputX / inputY * -1; |
|
138 } |
|
139 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 ) |
|
140 // 180 - 270 degrees |
|
141 { |
|
142 source = inputY / inputX; |
|
143 } |
|
144 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 ) |
|
145 // 270 - 360 degrees |
|
146 { |
|
147 source = inputX / inputY * -1; |
|
148 } |
|
149 |
|
150 Math::ATan( target, source ); |
|
151 TInt16 declination( ( target * 180 ) / 3.14 ); |
|
152 |
|
153 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 ) |
|
154 // 0 - 90 degrees |
|
155 { |
|
156 } |
|
157 if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 ) |
|
158 // 90 - 180 degrees |
|
159 { |
|
160 declination = declination + 90; |
|
161 } |
|
162 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 ) |
|
163 // 180 - 270 degrees |
|
164 { |
|
165 declination = declination + 180; |
|
166 } |
|
167 if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 ) |
|
168 // 270 - 360 degrees |
|
169 { |
|
170 declination = declination + 270; |
|
171 } |
|
172 |
|
173 if( inputY == 0 && inputX > 0 ) |
|
174 { |
|
175 declination = 0; |
|
176 } |
|
177 if( inputY > 0 && inputX == 0 ) |
|
178 { |
|
179 declination = 90; |
|
180 } |
|
181 if( inputY == 0 && inputX < 0 ) |
|
182 { |
|
183 declination = 180; |
|
184 } |
|
185 if( inputY < 0 && inputX == 0 ) |
|
186 { |
|
187 declination = 270; |
|
188 } |
|
189 |
|
190 if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) ) |
|
191 { |
|
192 aOutput.iTheta = aPreviousTheta; |
|
193 } |
|
194 |
|
195 // Filter data... |
|
196 |
|
197 // For testing purposes change angle with fixed values from input parameters |
|
198 aOutput.iTheta = declination; |
|
199 const TInt KAngleChange = 30; |
|
200 aOutput.iTheta += ( aParamsArray[ 0 ] * KAngleChange ); |
|
201 aOutput.iTheta += ( aParamsArray[ 1 ] * KAngleChange ); |
|
202 aOutput.iTheta += ( aParamsArray[ 2 ] * KAngleChange ); |
|
203 aOutput.iTheta += ( aParamsArray[ 3 ] * KAngleChange ); |
|
204 aOutput.iTheta %= 360; // take modulo to avoid angle larger than 360 |
|
205 // ... end for testing purposes |
|
206 |
|
207 |
|
208 return KErrNone; |
|
209 } |
|
210 |
|
211 // End of File |
|