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