| 175,29 → 175,38 |
|---|
| int heading_MM3(void) |
| //############################################################################ |
| { |
| uint16_t div_faktor; |
| int16_t sin_nick, cos_nick, sin_roll, cos_roll; |
| int32_t Hx, Hy, Hz; |
| int32_t x_corr, y_corr; |
| int32_t Hx, Hy, Hz, x_corr, y_corr; |
| int16_t heading; |
| int8_t nicktilt,rolltilt; |
| |
| int8_t tilt; |
| /* |
| // Berechnung von sinus und cosinus |
| nicktilt = atan2_i(Aktuell_az-556,AdWertAccNick*64); |
| sin_nick = sin_i(nicktilt); |
| cos_nick = cos_i(nicktilt); |
| DebugOut.Analog[0] = nicktilt; |
| tilt = atan2_i(Aktuell_az-556,AdWertAccNick*64); |
| sin_nick = sin_i(tilt); |
| cos_nick = cos_i(tilt); |
| |
| rolltilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
| sin_roll = sin_i(rolltilt); |
| cos_roll = cos_i(rolltilt); |
| DebugOut.Analog[1] = rolltilt; |
| tilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
| sin_roll = sin_i(tilt); |
| cos_roll = cos_i(tilt); |
| */ |
| // calibration factor for transforming Gyro Integrals to angular degrees |
| div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
| |
| // calculate sinus cosinus of pitch and tilt angle |
| tilt = (IntegralNick/div_faktor); |
| sin_nick = sin_i(tilt); |
| cos_nick = cos_i(tilt); |
| |
| tilt = (IntegralRoll/div_faktor); |
| sin_roll = sin_i(tilt); |
| cos_roll = cos_i(tilt); |
| |
| // Offset |
| Hx = (MM3.x_axis - MM3_calib.X_off); |
| Hy = (MM3.y_axis - MM3_calib.Y_off); |
| Hz = (MM3.z_axis - MM3_calib.Z_off); |
| |
| /* |
| |
| // Normierung Wertebereich |
| if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
| { |
| 214,7 → 223,6 |
|---|
| Hx = (Hx * MM3_calib.Z_range) / MM3_calib.X_range; |
| Hy = (Hy * MM3_calib.Z_range) / MM3_calib.Y_range; |
| } |
| */ |
| |
| // Neigungskompensierung |
| x_corr = Hx * cos_nick; |