Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 724 → Rev 725

/branches/MicroMag3_Nick666/trunc/compass.c
190,39 → 190,23
sin_roll = sin_i(tilt);
cos_roll = cos_i(tilt);
*/
 
// calibration factor for transforming Gyro Integrals to angular degrees
div_faktor = (uint16_t)EE_Parameter.UserParam3 *8;
 
// calculate sinus cosinus of pitch and tilt angle
tilt = (IntegralNick/div_faktor);
tilt = (IntegralNick /div_faktor);
sin_nick = sin_i(tilt);
cos_nick = cos_i(tilt);
 
tilt = (IntegralRoll/div_faktor);
tilt = (IntegralRoll /div_faktor);
sin_roll = sin_i(tilt);
cos_roll = cos_i(tilt);
// Offset
Hx = (MM3.x_axis - MM3_calib.X_off);
Hy = (MM3.y_axis - MM3_calib.Y_off);
Hz = (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))
{
Hy = (Hy * MM3_calib.X_range) / MM3_calib.Y_range;
Hz = (Hz * 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))
{
Hx = (Hx * MM3_calib.Y_range) / MM3_calib.X_range;
Hz = (Hz * 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))
{
Hx = (Hx * MM3_calib.Z_range) / MM3_calib.X_range;
Hy = (Hy * MM3_calib.Z_range) / MM3_calib.Y_range;
}
// Offset und Normalisierung
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) *1024) /MM3_calib.X_range;
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) *1024) /MM3_calib.Y_range;
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) *1024) /MM3_calib.Z_range;
 
// Neigungskompensierung
x_corr = Hx * cos_nick;
/branches/MicroMag3_Nick666/trunc/fc.c
75,6 → 75,7
int KompassValue = 0;
int KompassStartwert = 0;
int KompassRichtung = 0;
uint8_t updKompass = 0;
 
unsigned char MAX_GAS,MIN_GAS;
unsigned char Notlandung = 0;
904,7 → 905,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
if(abs(StickGier) > 15) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
}
918,33 → 919,21
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
int w,v;
static uint8_t updKompass = 0;
 
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
if (!updKompass--) // Aufruf mit ~10 Hz
{
KompassValue = heading_MM3();
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
updKompass = 50;
}
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 < 35 && NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
Mess_Integral_Gier -= (KompassRichtung * w) / 32; // nach Kompass ausrichten
}
}
if(NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
Mess_Integral_Gier -= (KompassRichtung *Parameter_KompassWirkung); // nach Kompass ausrichten
}
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
952,6 → 941,7
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;