/branches/v0.60_MicroMag3_Nick666/compass.c |
---|
117,8 → 117,9 |
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); |
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
//Winkelberechnung |
heading = arctan_i(x_corr, y_corr); |