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