| 15,7 → 15,7 |
|---|
| |
| #include "main.h" |
| |
| struct MM3_calib_struct ee_calib EEMEM; |
| struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM |
| |
| struct MM3_working_struct MM3; |
| struct MM3_calib_struct MM3_calib; |
| 26,8 → 26,8 |
|---|
| void init_MM3(void) |
| //############################################################################ |
| { |
| SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
| //SPSR = (1<<SPI2X); |
| SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1); //Interrupt an, Master, 625 kHz Oszillator |
| SPSR = (1<<SPI2X); |
| |
| DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
| |
| 60,22 → 60,13 |
|---|
| 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 |
| |
| MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
| MM3.DRDY = SetDelay(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
| MM3.STATE = MM3_WAIT_DRDY; |
| return; |
| |
| case MM3_WAIT_DRDY: |
| if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
| return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
| /* |
| case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
| MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
| MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
| |
| MM3.AXIS = MM3_X; |
| MM3.STATE = MM3_RESET; |
| return; |
| */ |
| return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
| } |
| } |
| |
| 168,7 → 159,7 |
|---|
| { |
| ROT_FLASH; |
| GRN_FLASH; |
| //beeptime = 50; |
| beeptime = 50; |
| beeper = 50; |
| } |
| beeper--; |
| 181,14 → 172,18 |
|---|
| if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--; |
| } |
| |
| // Offset der Achsen berechnen |
| MM3_calib.X_off = (x_max + x_min) / 2; |
| MM3_calib.Y_off = (y_max + y_min) / 2; |
| MM3_calib.Z_off = (z_max + z_min) / 2; |
| // Wertebereich der Achsen |
| MM3_calib.X_range = (x_max - x_min); |
| MM3_calib.Y_range = (y_max - y_min); |
| MM3_calib.Z_range = (z_max - z_min); |
| |
| // Offset der Achsen |
| MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max; |
| MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max; |
| MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max; |
| |
| // und im EEProm abspeichern |
| eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
| |
| } |
| |
| |
| 198,27 → 193,55 |
|---|
| //############################################################################ |
| { |
| float sin_nick, cos_nick, sin_roll, cos_roll; |
| signed int x_corr, y_corr, heading; |
| signed int x_axis,y_axis,z_axis; |
| float x_corr, y_corr; |
| signed int x_axis,y_axis,z_axis, heading; |
| unsigned int div_faktor; |
| |
| div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
| |
| MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8)); |
| MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8)); |
| |
| // Berechung von sinus und cosinus |
| MM3.NickGrad = -(IntegralNick/div_faktor); |
| sin_nick = sin_f(MM3.NickGrad); |
| cos_nick = cos_f(MM3.NickGrad); |
| |
| MM3.RollGrad = -(IntegralRoll/div_faktor); |
| sin_roll = sin_f(MM3.RollGrad); |
| cos_roll = cos_f(MM3.RollGrad); |
| |
| // Offset |
| x_axis = (MM3.x_axis + MM3_calib.X_off); |
| y_axis = (MM3.y_axis + MM3_calib.Y_off); |
| z_axis = (MM3.z_axis + MM3_calib.Z_off); |
| |
| // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen |
| x_axis = -(MM3.x_axis - MM3_calib.X_off); |
| y_axis = -(MM3.y_axis - MM3_calib.Y_off); |
| z_axis = -(MM3.z_axis - MM3_calib.Z_off); |
| // Normierung Wertebereich |
| if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
| { |
| y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range; |
| z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range; |
| } |
| else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range)) |
| { |
| x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range; |
| z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range; |
| } |
| else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range)) |
| { |
| x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range; |
| y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range; |
| } |
| |
| DebugOut.Analog[9] = x_axis; |
| DebugOut.Analog[10] = y_axis; |
| DebugOut.Analog[11] = z_axis; |
| |
| // Neigungskompensation |
| x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick); |
| y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis)); |
| x_corr = x_axis * cos_nick; |
| x_corr += y_axis * sin_roll * sin_nick; |
| x_corr -= z_axis * cos_roll * sin_nick; |
| |
| y_corr = y_axis * cos_roll; |
| y_corr += z_axis * sin_roll; |
| |
| // Winkelberechnung |
| heading = atan2_i(x_corr, y_corr); |
| |