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 | ||