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; |