| 26,8 → 26,8 |
|---|
| void init_MM3(void) |
| //############################################################################ |
| { |
| SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1); //Interrupt an, Master, 625 kHz Oszillator |
| SPSR = (1<<SPI2X); |
| SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
| //SPSR = (1<<SPI2X); |
| |
| DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
| |
| 58,9 → 58,9 |
|---|
| |
| if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
| else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30) |
| else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
| else SPDR = 0x33; //if (MM3.AXIS == MM3_Z) // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
| |
| MM3.DRDY = SetDelay(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
| MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
| MM3.STATE = MM3_WAIT_DRDY; |
| return; |
| |
| 176,21 → 176,19 |
|---|
| { |
| float sin_nick, cos_nick, sin_roll, cos_roll; |
| float x_corr, y_corr; |
| signed int x_axis,y_axis,z_axis, heading; |
| signed int x_axis,y_axis,z_axis, heading, nicktilt, rolltilt; |
| unsigned int div_faktor; |
| |
| div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
| |
| // Berechung von sinus und cosinus |
| MM3.NickGrad = (IntegralNick/div_faktor); |
| //MM3.NickGrad = asin_i(MM3.NickGrad); |
| sin_nick = sin_f(MM3.NickGrad); |
| cos_nick = cos_f(MM3.NickGrad); |
| nicktilt = (IntegralNick/div_faktor); |
| sin_nick = sin_f(nicktilt); |
| cos_nick = cos_f(nicktilt); |
| |
| MM3.RollGrad = (IntegralRoll/div_faktor); |
| //MM3.RollGrad = asin_i(MM3.RollGrad); |
| sin_roll = sin_f(MM3.RollGrad); |
| cos_roll = cos_f(MM3.RollGrad); |
| rolltilt = (IntegralRoll/div_faktor); |
| sin_roll = sin_f(rolltilt); |
| cos_roll = cos_f(rolltilt); |
| |
| // Offset |
| x_axis = (MM3.x_axis - MM3_calib.X_off); |