/branches/v0.60_MicroMag3_Nick666/v0.66c/compass.c |
---|
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); |
/branches/v0.60_MicroMag3_Nick666/v0.66c/compass.h |
---|
8,8 → 8,6 |
signed int x_axis; |
signed int y_axis; |
signed int z_axis; |
signed int NickGrad; |
signed int RollGrad; |
}; |
/branches/v0.60_MicroMag3_Nick666/v0.66c/fc.c |
---|
72,9 → 72,10 |
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
volatile long Mess_Integral_Hoch = 0; |
volatile int KompassValue = 0; |
volatile int KompassStartwert = 0; |
volatile int KompassRichtung = 0; |
int KompassValue = 0; |
int KompassStartwert = 0; |
int KompassRichtung = 0; |
uint8_t updKompass = 0; |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
396,7 → 397,7 |
static unsigned int modell_fliegt = 0; |
static int hoehenregler = 0; |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
static char NeueKompassRichtungMerken = 1; |
static long ausgleichNick, ausgleichRoll; |
Mittelwert(); |
835,7 → 836,15 |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
int w,v; |
static int SignalSchlecht = 0; |
static char SignalSchlecht = 0; |
if (!updKompass--) // Aufruf mit ~10 Hz |
{ |
KompassValue = heading_MM3(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
updKompass = 50; |
} |
w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /1024); |
if(v > w) w = v; // grösste Neigung ermitteln |
851,7 → 860,7 |
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
if(SignalSchlecht) SignalSchlecht--; |
} |
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/branches/v0.60_MicroMag3_Nick666/v0.66c/fc.h |
---|
14,9 → 14,9 |
extern volatile long IntegralAccNick,IntegralAccRoll; |
extern volatile long Mess_Integral_Hoch; |
extern volatile long Integral_Gier,Mess_Integral_Gier,Mess_Integral_Gier2; |
extern volatile int KompassValue; |
extern volatile int KompassStartwert; |
extern volatile int KompassRichtung; |
extern int KompassValue; |
extern int KompassStartwert; |
extern int KompassRichtung; |
extern int HoehenWert; |
extern int SollHoehe; |
extern volatile int MesswertNick,MesswertRoll,MesswertGier; |
/branches/v0.60_MicroMag3_Nick666/v0.66c/timer0.c |
---|
3,7 → 3,6 |
volatile unsigned int CountMilliseconds = 0; |
volatile static unsigned int tim_main; |
volatile unsigned char UpdateMotor = 0; |
volatile unsigned int cntKompass = 0; |
volatile unsigned int beeptime = 0; |
unsigned int BeepMuster = 0xffff; |
int ServoValue = 0; |
20,7 → 19,7 |
}; |
SIGNAL (SIG_OVERFLOW0) // 8kHz |
SIGNAL (SIG_OVERFLOW0) // 9,8kHz |
{ |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char pieper_ein = 0; |
28,7 → 27,7 |
if(!cnt--) |
{ |
cnt = 9; |
cnt = 10; |
cnt_1ms++; |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; |
62,17 → 61,7 |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
timer0_MM3(); // Kompass auslesen |
if (!cntKompass--) // Aufruf mit 10 Hz |
{ |
KompassValue = heading_MM3(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
cntKompass = 980; |
} |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) timer0_MM3(); // Kompass auslesen |
} |
/branches/v0.60_MicroMag3_Nick666/v0.66c/timer0.h |
---|
11,6 → 11,5 |
extern volatile unsigned int CountMilliseconds; |
extern volatile unsigned char UpdateMotor; |
extern volatile unsigned int beeptime; |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
extern unsigned int BeepMuster; |