| 110,18 → 110,17 |
|---|
| signed int x_corr, y_corr; |
| signed int heading; |
| |
| //Berechung von sinus und cosinus |
| // Berechung von sinus und cosinus |
| sin_nick = sin_f(MM3.NickGrad); |
| cos_nick = cos_f(MM3.NickGrad); |
| sin_roll = sin_f(MM3.RollGrad); |
| cos_roll = cos_f(MM3.RollGrad); |
| |
| //Neigungskompensation |
| //x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
| x_corr = (cos_nick * MM3.x_axis) + (sin_roll * MM3.y_axis* sin_nick) - (cos_roll * MM3.z_axis * sin_nick); |
| // Neigungskompensation |
| x_corr = (cos_nick * MM3.x_axis) + (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick); |
| y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
| |
| //Winkelberechnung |
| // Winkelberechnung |
| heading = arctan_i(x_corr, y_corr); |
| |
| return (heading); |