Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 465 → Rev 466

/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;