Rev 343 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 343 | Rev 349 | ||
|---|---|---|---|
| Line 66... | Line 66... | ||
| 66 | return; |
66 | return; |
| Line 67... | Line 67... | ||
| 67 | 67 | ||
| 68 | case MM3_WAIT_DRDY: |
68 | case MM3_WAIT_DRDY: |
| 69 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
69 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
| 70 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
70 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
| 71 | 71 | /* |
|
| 72 | case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
72 | case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
| 73 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
73 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
| Line 74... | Line 74... | ||
| 74 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
74 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
| 75 | 75 | |
|
| 76 | MM3.AXIS = MM3_X; |
76 | MM3.AXIS = MM3_X; |
| - | 77 | MM3.STATE = MM3_RESET; |
|
| 77 | MM3.STATE = MM3_RESET; |
78 | return; |
| 78 | return; |
79 | */ |
| Line 79... | Line 80... | ||
| 79 | } |
80 | } |
| Line 129... | Line 130... | ||
| 129 | else // if (MM3.AXIS == MM3_Z) |
130 | else // if (MM3.AXIS == MM3_Z) |
| 130 | { |
131 | { |
| 131 | MM3.z_axis |= SPDR; |
132 | MM3.z_axis |= SPDR; |
| 132 | if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis; |
133 | if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis; |
| 133 | else MM3.z_axis = MM3.z_axis_old; |
134 | else MM3.z_axis = MM3.z_axis_old; |
| - | 135 | MM3.AXIS = MM3_X; |
|
| 134 | MM3.STATE = MM3_TILT; |
136 | MM3.STATE = MM3_RESET; |
| 135 | } |
137 | } |
| Line 136... | Line 138... | ||
| 136 | 138 | ||
| 137 | return; |
139 | return; |
| 138 | } |
140 | } |
| Line 194... | Line 196... | ||
| 194 | //############################################################################ |
196 | //############################################################################ |
| 195 | { |
197 | { |
| 196 | float sin_nick, cos_nick, sin_roll, cos_roll; |
198 | float sin_nick, cos_nick, sin_roll, cos_roll; |
| 197 | signed int x_corr, y_corr, heading; |
199 | signed int x_corr, y_corr, heading; |
| 198 | signed int x_axis,y_axis,z_axis; |
200 | signed int x_axis,y_axis,z_axis; |
| 199 | 201 | ||
| - | 202 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
|
| - | 203 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
|
| - | 204 | ||
| 200 | // Berechung von sinus und cosinus |
205 | // Berechung von sinus und cosinus |
| 201 | sin_nick = sin_f(MM3.NickGrad); |
206 | sin_nick = sin_f(MM3.NickGrad); |
| 202 | cos_nick = cos_f(MM3.NickGrad); |
207 | cos_nick = cos_f(MM3.NickGrad); |
| 203 | sin_roll = sin_f(MM3.RollGrad); |
208 | sin_roll = sin_f(MM3.RollGrad); |
| 204 | cos_roll = cos_f(MM3.RollGrad); |
209 | cos_roll = cos_f(MM3.RollGrad); |