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