Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 250 → Rev 251

/branches/v0.60_MicroMag3_Nick666/compass.c
117,8 → 117,9
cos_roll = cos_f(MM3.RollGrad);
//Neigungskompensation
//x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
x_corr = (cos_nick * MM3.x_axis) + (sin_roll * MM3.y_axis* sin_nick) - (cos_roll * MM3.z_axis * sin_nick);
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis);
//Winkelberechnung
heading = arctan_i(x_corr, y_corr);
/branches/v0.60_MicroMag3_Nick666/compass.h
37,16 → 37,16
// Die Werte der Kompasskalibrierung
 
// RANGE: Maximaler Wert - minimaler Wert
#define RANGE_X 1161
#define RANGE_Y 1209
#define RANGE_Z 1276
#define RANGE_X 0
#define RANGE_Y 0
#define RANGE_Z 0
 
// OFFSET: (Maximaler Wert + minimaler Wert) / 2
#define OFF_X -46
#define OFF_Y 52
#define OFF_Z 25
#define OFF_X 0
#define OFF_Y 0
#define OFF_Z 0
 
// z. Z. nicht verwendet
#define GAIN_X 16
#define GAIN_Y 15
#define GAIN_Z 15
#define GAIN_X 1
#define GAIN_Y 1
#define GAIN_Z 1
/branches/v0.60_MicroMag3_Nick666/fc.c
628,29 → 628,16
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//KompassValue = 12;
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
if(NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
ANALOG_ON; // ADC einschalten
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_Integral_Gier -= (KompassRichtung * Parameter_KompassWirkung) / 32; // nach Kompass ausrichten
ANALOG_ON; // ADC einschalten
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
673,9 → 660,9
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[7] = GasMischanteil;
// DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[8] = MM3_heading();
//DebugOut.Analog[8] = MM3_heading();
DebugOut.Analog[9] = MM3.x_axis;
DebugOut.Analog[10] = MM3.y_axis;
DebugOut.Analog[11] = MM3.z_axis;
/branches/v0.60_MicroMag3_Nick666/timer0.c
4,6 → 4,7
volatile static unsigned int tim_main;
volatile unsigned char UpdateMotor = 0;
volatile unsigned int beeptime = 0;
volatile unsigned int cntKompass = 0;
int ServoValue = 0;
 
enum {
41,10 → 42,18
else
PORTD &= ~(1<<PD2);
 
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) MM3_timer0();
//KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
MM3_timer0(); // Kompass auslesen
 
if (!cntKompass--) // Aufruf mit 25 Hz
{
KompassValue = MM3_heading();
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 320;
}
}
 
}