Rev 229 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 229 | Rev 232 | ||
|---|---|---|---|
| Line 48... | Line 48... | ||
| 48 | if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
48 | if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
| 49 | else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
49 | else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
| 50 | else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
50 | else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
| 51 | else {MM3.STATE == MM3_IDLE;break;} |
51 | else {MM3.STATE == MM3_IDLE;break;} |
| Line 52... | Line 52... | ||
| 52 | 52 | ||
| 53 | MM3.DRDY = SetDelay(10); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
53 | MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
| 54 | MM3.STATE = MM3_WAIT_DRDY; |
54 | MM3.STATE = MM3_WAIT_DRDY; |
| Line 55... | Line 55... | ||
| 55 | break; |
55 | break; |
| 56 | 56 | ||
| Line 98... | Line 98... | ||
| 98 | MM3.z_axis -= OFF_Z; |
98 | MM3.z_axis -= OFF_Z; |
| 99 | //MM3.z_axis /= GAIN_Z; |
99 | //MM3.z_axis /= GAIN_Z; |
| 100 | MM3.AXIS = MM3_X; |
100 | MM3.AXIS = MM3_X; |
| 101 | MM3.STATE = MM3_RESET; |
101 | MM3.STATE = MM3_RESET; |
| 102 | // Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in ° |
102 | // Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in ° |
| 103 | MM3.NickGrad = IntegralNick/Int2Grad_Faktor; |
103 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
| 104 | MM3.RollGrad = IntegralRoll/Int2Grad_Faktor; |
104 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
| 105 | break; |
105 | break; |
| 106 | } |
106 | } |
| 107 | } |
107 | } |
| Line 108... | Line 108... | ||
| 108 | 108 | ||
| Line 119... | Line 119... | ||
| 119 | cos_roll = (float)cos_f(MM3.RollGrad); |
119 | cos_roll = (float)cos_f(MM3.RollGrad); |
| Line 120... | Line 120... | ||
| 120 | 120 | ||
| 121 | //Neigungskompensation |
121 | //Neigungskompensation |
| 122 | y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
122 | y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
| 123 | x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
- | |
| 124 | 123 | x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
|
| 125 | //Winkelberechnung |
124 | //Winkelberechnung |
| Line 126... | Line 125... | ||
| 126 | heading = arctan_f(x_corr, y_corr); |
125 | heading = arctan_i(x_corr, y_corr); |
| 127 | 126 | ||