48,7 → 48,6 |
if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
else {MM3.STATE == MM3_IDLE;break;} |
|
MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
MM3.STATE = MM3_WAIT_DRDY; |
80,7 → 79,6 |
case MM3_X_BYTE2: // 2. Byte der entsprechenden Achse ist da. |
MM3.x_axis |= SPDR; |
MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
//MM3.x_axis /= GAIN_X; |
MM3.AXIS = MM3_Y; |
MM3.STATE = MM3_RESET; |
break; |
88,7 → 86,6 |
case MM3_Y_BYTE2: |
MM3.y_axis |= SPDR; |
MM3.y_axis -= OFF_Y; |
//MM3.y_axis /= GAIN_Y; |
MM3.AXIS = MM3_Z; |
MM3.STATE = MM3_RESET; |
break; |
96,7 → 93,6 |
case MM3_Z_BYTE2: |
MM3.z_axis |= SPDR; |
MM3.z_axis -= OFF_Z; |
//MM3.z_axis /= GAIN_Z; |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
// Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in ° |
121,6 → 117,7 |
//Neigungskompensation |
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
|
//Winkelberechnung |
heading = arctan_i(x_corr, y_corr); |
|