Rev 409 | Go to most recent revision | Show entire file | Ignore 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 74... | Line 73... | ||
| 74 | 73 | ||
| 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) |
- | |
| 81 | { |
- | |
| 82 | case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken |
- | |
| 83 | if (MM3.AXIS == MM3_X) |
- | |
| 84 | { |
79 | static char tmp; |
| 85 | MM3.x_axis = SPDR; |
- | |
| 86 | MM3.x_axis <<= 8; |
80 | int wert; |
| 87 | } |
81 | |
| 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) |
82 | if (MM3.STATE == MM3_DRDY) // 1. Byte ist da, zwischenspeichern |
| 94 | { |
83 | { |
| 95 | MM3.z_axis = SPDR; |
- | |
| 96 | MM3.z_axis <<= 8; |
- | |
| 97 | } |
- | |
| 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; |
|
| - | 88 | } |
|
| - | 89 | ||
| - | 90 | else //if (MM3.STATE == MM3_BYTE2) // 2. Byte der entsprechenden Achse ist da |
|
| - | 91 | { |
|
| Line 101... | Line 92... | ||
| 101 | return; |
92 | wert = SPDR | tmp << 8; // 1. Byte an MSB-Stelle rücken und 2. Byte dranpappen |
| 102 | 93 | ||
| 103 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
94 | if(abs(wert) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten |
| 104 | if (MM3.AXIS == MM3_X) |
- | |
| 105 | { |
95 | switch(MM3.AXIS) |
| 106 | MM3.x_axis |= SPDR; |
- | |
| 107 | // Spikes filtern |
96 | { |
| 108 | if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis; |
97 | case MM3_X: |
| 109 | else MM3.x_axis = MM3.x_axis_old; |
- | |
| 110 | MM3.AXIS = MM3_Y; |
98 | MM3.x_axis = wert; |
| 111 | MM3.STATE = MM3_RESET; |
99 | MM3.AXIS = MM3_Y; |
| 112 | } |
- | |
| 113 | else if (MM3.AXIS == MM3_Y) |
100 | break; |
| 114 | { |
- | |
| 115 | MM3.y_axis |= SPDR; |
- | |
| 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 | ||