Rev 718 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 718 | Rev 725 | ||
---|---|---|---|
Line 188... | Line 188... | ||
188 | |
188 | |
189 | tilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
189 | tilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
190 | sin_roll = sin_i(tilt); |
190 | sin_roll = sin_i(tilt); |
191 | cos_roll = cos_i(tilt); |
191 | cos_roll = cos_i(tilt); |
- | 192 | */ |
|
192 | */ |
193 | |
193 | // calibration factor for transforming Gyro Integrals to angular degrees |
194 | // calibration factor for transforming Gyro Integrals to angular degrees |
Line 194... | Line 195... | ||
194 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
195 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
195 | 196 | ||
196 | // calculate sinus cosinus of pitch and tilt angle |
197 | // calculate sinus cosinus of pitch and tilt angle |
197 | tilt = (IntegralNick/div_faktor); |
198 | tilt = (IntegralNick /div_faktor); |
Line 198... | Line 199... | ||
198 | sin_nick = sin_i(tilt); |
199 | sin_nick = sin_i(tilt); |
199 | cos_nick = cos_i(tilt); |
200 | cos_nick = cos_i(tilt); |
200 | 201 | ||
Line 201... | Line -... | ||
201 | tilt = (IntegralRoll/div_faktor); |
- | |
202 | sin_roll = sin_i(tilt); |
- | |
203 | cos_roll = cos_i(tilt); |
- | |
204 | - | ||
205 | // Offset |
- | |
206 | Hx = (MM3.x_axis - MM3_calib.X_off); |
202 | tilt = (IntegralRoll /div_faktor); |
207 | Hy = (MM3.y_axis - MM3_calib.Y_off); |
- | |
208 | Hz = (MM3.z_axis - MM3_calib.Z_off); |
- | |
209 | - | ||
210 | // Normierung Wertebereich |
- | |
211 | if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
- | |
212 | { |
- | |
213 | Hy = (Hy * MM3_calib.X_range) / MM3_calib.Y_range; |
- | |
214 | Hz = (Hz * MM3_calib.X_range) / MM3_calib.Z_range; |
203 | sin_roll = sin_i(tilt); |
215 | } |
204 | cos_roll = cos_i(tilt); |
216 | else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range)) |
- | |
217 | { |
- | |
218 | Hx = (Hx * MM3_calib.Y_range) / MM3_calib.X_range; |
- | |
219 | Hz = (Hz * MM3_calib.Y_range) / MM3_calib.Z_range; |
205 | |
220 | } |
- | |
221 | else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range)) |
- | |
Line 222... | Line 206... | ||
222 | { |
206 | // Offset und Normalisierung |
223 | Hx = (Hx * MM3_calib.Z_range) / MM3_calib.X_range; |
207 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) *1024) /MM3_calib.X_range; |
224 | Hy = (Hy * MM3_calib.Z_range) / MM3_calib.Y_range; |
208 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) *1024) /MM3_calib.Y_range; |
225 | } |
209 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) *1024) /MM3_calib.Z_range; |