Rev 349 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 349 | Rev 384 | ||
---|---|---|---|
Line 194... | Line 194... | ||
194 | // Neigungskompensierung und Berechnung der Ausrichtung |
194 | // Neigungskompensierung und Berechnung der Ausrichtung |
195 | signed int MM3_heading(void) |
195 | signed int MM3_heading(void) |
196 | //############################################################################ |
196 | //############################################################################ |
197 | { |
197 | { |
198 | float sin_nick, cos_nick, sin_roll, cos_roll; |
198 | float sin_nick, cos_nick, sin_roll, cos_roll; |
199 | signed int x_corr, y_corr, heading; |
199 | float x_corr, y_corr; |
200 | signed int x_axis,y_axis,z_axis; |
200 | signed int x_axis,y_axis,z_axis, heading; |
201 | - | ||
202 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
- | |
203 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
- | |
Line 204... | Line 201... | ||
204 | 201 | ||
- | 202 | // Berechung von sinus und cosinus |
|
205 | // Berechung von sinus und cosinus |
203 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
206 | sin_nick = sin_f(MM3.NickGrad); |
204 | sin_nick = sin_f(MM3.NickGrad); |
- | 205 | cos_nick = cos_f(MM3.NickGrad); |
|
- | 206 | ||
207 | cos_nick = cos_f(MM3.NickGrad); |
207 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
208 | sin_roll = sin_f(MM3.RollGrad); |
208 | sin_roll = sin_f(MM3.RollGrad); |
Line 209... | Line 209... | ||
209 | cos_roll = cos_f(MM3.RollGrad); |
209 | cos_roll = cos_f(MM3.RollGrad); |
210 | 210 | ||
211 | // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen |
211 | // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen |
212 | x_axis = MM3.x_axis - X_off; |
212 | x_axis = MM3.x_axis - X_off; |
Line 213... | Line 213... | ||
213 | y_axis = MM3.y_axis - Y_off; |
213 | y_axis = MM3.y_axis - Y_off; |
- | 214 | z_axis = MM3.z_axis - Z_off; |
|
214 | z_axis = MM3.z_axis - Z_off; |
215 | |
215 | 216 | // Neigungskompensation |
|
- | 217 | x_corr = x_axis * cos_nick; |
|
- | 218 | x_corr += y_axis * sin_roll * sin_nick; |
|
Line 216... | Line 219... | ||
216 | // Neigungskompensation |
219 | x_corr -= z_axis * cos_roll * sin_nick; |
217 | x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick); |
220 | y_corr = y_axis * cos_roll; |
- | 221 | y_corr += z_axis * sin_roll; |
|
- | 222 | ||
- | 223 | // Winkelberechnung |
|
Line 218... | Line 224... | ||
218 | y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis)); |
224 | heading = atan2_i(x_corr, y_corr); |
219 | 225 |