| 196,15 → 196,15 |
|---|
| //############################################################################ |
| { |
| float sin_nick, cos_nick, sin_roll, cos_roll; |
| signed int x_corr, y_corr, heading; |
| signed int x_axis,y_axis,z_axis; |
| float x_corr, y_corr; |
| signed int x_axis,y_axis,z_axis, heading; |
| |
| // Berechung von sinus und cosinus |
| MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
| MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
| |
| // Berechung von sinus und cosinus |
| sin_nick = sin_f(MM3.NickGrad); |
| cos_nick = cos_f(MM3.NickGrad); |
| |
| MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
| sin_roll = sin_f(MM3.RollGrad); |
| cos_roll = cos_f(MM3.RollGrad); |
| |
| 213,12 → 213,18 |
|---|
| y_axis = MM3.y_axis - Y_off; |
| z_axis = MM3.z_axis - Z_off; |
| |
| // Neigungskompensation |
| x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick); |
| y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis)); |
| // Neigungskompensation |
| x_corr = x_axis * cos_nick; |
| x_corr += y_axis * sin_roll * sin_nick; |
| x_corr -= z_axis * cos_roll * sin_nick; |
| y_corr = y_axis * cos_roll; |
| y_corr += z_axis * sin_roll; |
| |
| // Winkelberechnung |
| heading = atan2_i(x_corr, y_corr); |
| |
| // Wertebereich 0° bis 360° |
| if (heading < 0) heading = 360 + heading; |
| |
| return (heading); |
| } |