176,21 → 176,19 |
{ |
float sin_nick, cos_nick, sin_roll, cos_roll; |
float x_corr, y_corr; |
signed int x_axis,y_axis,z_axis, heading; |
signed int x_axis,y_axis,z_axis,nicktilt,rolltilt,heading; |
unsigned int div_faktor; |
|
div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
|
// 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); |
nicktilt = (IntegralNick/div_faktor); |
sin_nick = sin_f(nicktilt); |
cos_nick = cos_f(nicktilt); |
|
MM3.RollGrad = (IntegralRoll/div_faktor); |
//MM3.RollGrad = asin_i(MM3.RollGrad); |
sin_roll = sin_f(MM3.RollGrad); |
cos_roll = cos_f(MM3.RollGrad); |
rolltilt = (IntegralRoll/div_faktor); |
sin_roll = sin_f(rolltilt); |
cos_roll = cos_f(rolltilt); |
|
// Offset |
x_axis = (MM3.x_axis - MM3_calib.X_off); |