178,9 → 178,9 |
MM3_calib.Z_range = (z_max - z_min); |
|
// Offset der Achsen |
MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max; |
MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max; |
MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max; |
MM3_calib.X_off = (x_max + x_min) / 2; |
MM3_calib.Y_off = (y_max + y_min) / 2; |
MM3_calib.Z_off = (z_max + z_min) / 2; |
|
// und im EEProm abspeichern |
eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
201,17 → 201,19 |
|
// Berechung von sinus und cosinus |
MM3.NickGrad = (IntegralNick/div_faktor); |
MM3.NickGrad = asin_i(MM3.NickGrad); |
sin_nick = sin_f(MM3.NickGrad); |
cos_nick = cos_f(MM3.NickGrad); |
|
MM3.RollGrad = (IntegralRoll/div_faktor); |
MM3.RollGrad = asin_i(MM3.RollGrad); |
sin_roll = sin_f(MM3.RollGrad); |
cos_roll = cos_f(MM3.RollGrad); |
|
// Offset |
x_axis = (MM3.x_axis + MM3_calib.X_off); |
y_axis = (MM3.y_axis + MM3_calib.Y_off); |
z_axis = (MM3.z_axis + MM3_calib.Z_off); |
x_axis = (MM3.x_axis - MM3_calib.X_off); |
y_axis = (MM3.y_axis - MM3_calib.Y_off); |
z_axis = (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)) |
243,7 → 245,10 |
y_corr += z_axis * sin_roll; |
|
// Winkelberechnung |
|
heading = atan2_i(x_corr, y_corr); |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
|
/* |
if (!x_corr && y_corr <0) return (90); |
254,6 → 259,5 |
if (x_corr > 0 && y_corr < 0) heading = -heading; |
if (x_corr > 0 && y_corr > 0) heading = 360 - heading; |
*/ |
|
return (heading); |
} |