Rev 409 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 409 | Rev 412 | ||
---|---|---|---|
Line 31... | Line 31... | ||
31 | 31 | ||
Line 32... | Line 32... | ||
32 | DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
32 | DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
Line 33... | Line -... | ||
33 | - | ||
34 | PORTD &= ~(1<<PD3); // J5 auf Low |
33 | |
35 | 34 | PORTD &= ~(1<<PD3); // J5 auf Low |
|
Line 36... | Line 35... | ||
36 | // Init Statemachine |
35 | |
37 | MM3.AXIS = MM3_X; |
36 | MM3.AXIS = MM3_X; |
Line 59... | Line 58... | ||
59 | 58 | ||
60 | if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
59 | if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
61 | else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30) |
60 | else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30) |
Line 62... | Line 61... | ||
62 | else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
61 | else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
63 | 62 | ||
64 | MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
63 | MM3.DRDY = SetDelay(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
Line 65... | Line 64... | ||
65 | MM3.STATE = MM3_WAIT_DRDY; |
64 | MM3.STATE = MM3_WAIT_DRDY; |
66 | return; |
65 | return; |
Line 75... | Line 74... | ||
75 | //############################################################################ |
74 | //############################################################################ |
76 | // SPI byte ready |
75 | // SPI byte ready |
77 | SIGNAL (SIG_SPI) |
76 | SIGNAL (SIG_SPI) |
78 | //############################################################################ |
77 | //############################################################################ |
79 | { |
78 | { |
80 | switch (MM3.STATE) |
79 | static char tmp; |
81 | { |
- | |
82 | case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken |
- | |
83 | if (MM3.AXIS == MM3_X) |
- | |
84 | { |
- | |
85 | MM3.x_axis = SPDR; |
- | |
86 | MM3.x_axis <<= 8; |
- | |
87 | } |
- | |
88 | else if (MM3.AXIS == MM3_Y) |
- | |
89 | { |
- | |
90 | MM3.y_axis = SPDR; |
- | |
91 | MM3.y_axis <<= 8; |
- | |
92 | } |
- | |
93 | else // if (MM3.AXIS == MM3_Z) |
- | |
94 | { |
- | |
95 | MM3.z_axis = SPDR; |
80 | int wert; |
96 | MM3.z_axis <<= 8; |
- | |
97 | } |
- | |
Line -... | Line 81... | ||
- | 81 | ||
- | 82 | if (MM3.STATE == MM3_DRDY) // 1. Byte ist da, zwischenspeichern |
|
- | 83 | { |
|
98 | 84 | tmp = SPDR; |
|
99 | SPDR=0x00; // Übertragung von 2. Byte auslösen |
85 | SPDR = 0x00; // Übertragung von 2. Byte auslösen |
100 | MM3.STATE=MM3_BYTE2; |
86 | MM3.STATE = MM3_BYTE2; |
- | 87 | return; |
|
Line 101... | Line 88... | ||
101 | return; |
88 | } |
102 | - | ||
103 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
89 | |
104 | if (MM3.AXIS == MM3_X) |
- | |
105 | { |
- | |
106 | MM3.x_axis |= SPDR; |
90 | else //if (MM3.STATE == MM3_BYTE2) // 2. Byte der entsprechenden Achse ist da |
107 | // Spikes filtern |
- | |
108 | if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis; |
- | |
109 | else MM3.x_axis = MM3.x_axis_old; |
- | |
110 | MM3.AXIS = MM3_Y; |
91 | { |
- | 92 | wert = SPDR | tmp << 8; // 1. Byte an MSB-Stelle rücken und 2. Byte dranpappen |
|
111 | MM3.STATE = MM3_RESET; |
93 | |
112 | } |
94 | if(abs(wert) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten |
- | 95 | switch(MM3.AXIS) |
|
113 | else if (MM3.AXIS == MM3_Y) |
96 | { |
114 | { |
97 | case MM3_X: |
- | 98 | MM3.x_axis = wert; |
|
- | 99 | MM3.AXIS = MM3_Y; |
|
115 | MM3.y_axis |= SPDR; |
100 | break; |
116 | if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis; |
101 | case MM3_Y: |
117 | else MM3.y_axis = MM3.y_axis_old; |
- | |
118 | MM3.AXIS = MM3_Z; |
102 | MM3.y_axis = wert; |
119 | MM3.STATE = MM3_RESET; |
- | |
120 | } |
103 | MM3.AXIS = MM3_Z; |
121 | else // if (MM3.AXIS == MM3_Z) |
104 | break; |
122 | { |
- | |
123 | MM3.z_axis |= SPDR; |
- | |
124 | if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis; |
105 | default: |
125 | else MM3.z_axis = MM3.z_axis_old; |
- | |
126 | MM3.AXIS = MM3_X; |
106 | MM3.z_axis = wert; |
127 | MM3.STATE = MM3_RESET; |
- | |
128 | } |
107 | MM3.AXIS = MM3_X; |
129 | 108 | } |
|
130 | return; |
109 | MM3.STATE = MM3_RESET; |
Line 131... | Line 110... | ||
131 | } |
110 | } |
132 | } |
111 | } |
Line 194... | Line 173... | ||
194 | //############################################################################ |
173 | //############################################################################ |
195 | { |
174 | { |
196 | float sin_nick, cos_nick, sin_roll, cos_roll; |
175 | float sin_nick, cos_nick, sin_roll, cos_roll; |
197 | float x_corr, y_corr; |
176 | float x_corr, y_corr; |
198 | signed int x_axis,y_axis,z_axis, heading; |
177 | signed int x_axis,y_axis,z_axis, heading; |
199 | uint16_t div_faktor; |
178 | unsigned int div_faktor; |
Line 200... | Line 179... | ||
200 | 179 | ||
Line 201... | Line 180... | ||
201 | div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
180 | div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
202 | 181 |