Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 282 → Rev 341

/branches/v0.60_MicroMag3_Nick666/compass.c
16,11 → 16,13
#include "main.h"
 
MM3_struct MM3;
int8_t Kompass_Offset[2] EEMEM; // X_off[0], Y_off[1], Z_off[2]
int8_t X_off, Y_off, Z_off;
 
 
//############################################################################
//Initialisierung der SPI-Schnittstelle
void init_spi(void)
// Initialisierung
void init_MM3(void)
//############################################################################
{
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator
32,11 → 34,16
MM3.AXIS = MM3_X;
MM3.STATE = MM3_RESET;
// Kalibrierung aus dem EEprom lesen
X_off = (int8_t)eeprom_read_byte(&Kompass_Offset[0]);
Y_off = (int8_t)eeprom_read_byte(&Kompass_Offset[1]);
Z_off = (int8_t)eeprom_read_byte(&Kompass_Offset[2]);
}
 
 
//############################################################################
//Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
void MM3_timer0(void)
//############################################################################
{
50,19 → 57,19
case MM3_START_TRANSFER:
PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High)
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
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
MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024)
MM3.DRDY = SetDelay(8); // 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)
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °
case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °
MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
71,10 → 78,10
return;
}
}
 
 
//############################################################################
//SPI byte ready
// SPI byte ready
SIGNAL (SIG_SPI)
//############################################################################
{
91,7 → 98,7
MM3.y_axis = SPDR;
MM3.y_axis <<= 8;
}
else // if (MM3.AXIS == MM3_Z)
else // if (MM3.AXIS == MM3_Z)
{
MM3.z_axis = SPDR;
MM3.z_axis <<= 8;
105,21 → 112,25
if (MM3.AXIS == MM3_X)
{
MM3.x_axis |= SPDR;
MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen
// Spikes filtern
if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;
else MM3.x_axis = MM3.x_axis_old;
MM3.AXIS = MM3_Y;
MM3.STATE = MM3_RESET;
}
else if (MM3.AXIS == MM3_Y)
{
MM3.y_axis |= SPDR;
MM3.y_axis -= OFF_Y;
MM3.y_axis |= SPDR;
if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;
else MM3.y_axis = MM3.y_axis_old;
MM3.AXIS = MM3_Z;
MM3.STATE = MM3_RESET;
}
else // if (MM3.AXIS == MM3_Z)
else // if (MM3.AXIS == MM3_Z)
{
MM3.z_axis |= SPDR;
MM3.z_axis -= OFF_Z;
if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
else MM3.z_axis = MM3.z_axis_old;
MM3.STATE = MM3_TILT;
}
127,21 → 138,77
}
}
 
//############################################################################
// Kompass kalibrieren
void MM3_calib(void)
//############################################################################
{
signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
uint8_t measurement=50,beeper=0;
unsigned int timer;
while (measurement)
{
//H_earth = MM3.x_axis*MM3.x_axis + MM3.y_axis*MM3.y_axis + MM3.z_axis*MM3.z_axis;
if (MM3.x_axis > x_max) x_max = MM3.x_axis;
else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
if (MM3.y_axis > y_max) y_max = MM3.y_axis;
else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
if (MM3.z_axis > z_max) z_max = MM3.z_axis;
else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
if (!beeper)
{
beeper = 50;
beeptime = 50;
}
beeper--;
// Schleife mit 100 Hz voll ausreichend
timer = SetDelay(10);
while(!CheckDelay(timer));
// Wenn Gas zurück genommen wird, Kalibrierung mit Verzögerung beenden
if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
}
X_off = (x_max + x_min) / 2;
Y_off = (y_max + y_min) / 2;
Z_off = (z_max + z_min) / 2;
eeprom_write_byte(&Kompass_Offset[0], X_off);
eeprom_write_byte(&Kompass_Offset[1], Y_off);
eeprom_write_byte(&Kompass_Offset[2], Z_off);
}
 
 
//############################################################################
// Neigungskompensierung und Berechnung der Ausrichtung
signed int MM3_heading(void)
//############################################################################
{
float sin_nick, cos_nick, sin_roll, cos_roll;
signed int x_corr, y_corr;
signed int heading;
signed int x_corr, y_corr, heading;
signed int x_axis,y_axis,z_axis;
// Berechung von sinus und cosinus
sin_nick = sin_f(MM3.NickGrad);
cos_nick = cos_f(MM3.NickGrad);
sin_roll = sin_f(MM3.RollGrad);
cos_roll = cos_f(MM3.RollGrad);
cos_roll = cos_f(MM3.RollGrad);
 
// Kompasskalibrierung berücksichtigen
x_axis = MM3.x_axis - X_off;
y_axis = MM3.y_axis - Y_off;
z_axis = MM3.z_axis - Z_off;
// Neigungskompensation
x_corr = (cos_nick * MM3.x_axis) + (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick);
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
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));
// Winkelberechnung
heading = atan2_i(x_corr, y_corr);