Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 333 → Rev 338

/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666AccMM3filtered/compass.c
64,8 → 64,8
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °
MM3.NickGrad = asin_i((float)Aktuell_ax/215*200);
MM3.RollGrad = asin_i((float)Aktuell_ay/205*200);
MM3.NickGrad = asin_i((float)Aktuell_ax/208*200);
MM3.RollGrad = asin_i((float)Aktuell_ay/208*200);
MM3.AXIS = MM3_X;
MM3.STATE = MM3_RESET;
103,10 → 103,26
return;
case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da
// versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern
// verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen
if (MM3.AXIS == MM3_X)
{
MM3.x_axis |= SPDR;
MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen
if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin)
{
MM3.x_axis = MM3.x_axis_last_valid;
}
else
{
//m = (MM3.x_axis + MM3.x_axis_last_valid) /2;
MM3.x_axis_last_valid = MM3.x_axis;
//MM3.x_axis = m;
}
 
MM3.AXIS = MM3_Y;
MM3.STATE = MM3_RESET;
}
114,49 → 130,13
{
MM3.y_axis |= SPDR;
MM3.y_axis -= OFF_Y;
MM3.AXIS = MM3_Z;
MM3.STATE = MM3_RESET;
}
else // if (MM3.AXIS == MM3_Z)
{
MM3.z_axis |= SPDR;
MM3.z_axis -= OFF_Z;
MM3.STATE = MM3_TILT;
}
// versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern
// verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen
if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin)
{
MM3.x_axis = MM3.x_axis_last_valid;
}
else
if (MM3.x_axis_last_valid *1,10 < MM3.x_axis || MM3.x_axis_last_valid *0,90 > MM3.x_axis)
{
MM3.x_axis = MM3.x_axis_last_valid;
}
else
{
//m = (MM3.x_axis + MM3.x_axis_last_valid) /2;
MM3.x_axis_last_valid = MM3.x_axis;
//MM3.x_axis = m;
}
 
 
 
 
if (MM3.y_axis > MM3_Ymax || MM3.y_axis < MM3_Ymin)
{
MM3.y_axis = MM3.y_axis_last_valid;
}
else
if (MM3.y_axis_last_valid *1,10 < MM3.y_axis || MM3.y_axis_last_valid *0,90 > MM3.y_axis)
{
MM3.y_axis = MM3.y_axis_last_valid;
}
else
{
//m = (MM3.y_axis + MM3.y_axis_last_valid) /2;
MM3.y_axis_last_valid = MM3.y_axis;
//MM3.y_axis = m;
163,29 → 143,27
}
 
MM3.AXIS = MM3_Z;
MM3.STATE = MM3_RESET;
}
else // if (MM3.AXIS == MM3_Z)
{
MM3.z_axis |= SPDR;
MM3.z_axis -= OFF_Z;
 
 
if (MM3.z_axis > MM3_Zmax || MM3.z_axis < MM3_Zmin)
{
MM3.z_axis = MM3.z_axis_last_valid;
}
else
if (MM3.z_axis_last_valid *1,10 < MM3.z_axis || MM3.z_axis_last_valid *0,90 > MM3.z_axis)
{
MM3.z_axis = MM3.z_axis_last_valid;
}
else
{
//m = (MM3.z_axis + MM3.z_axis_last_valid) /2;
MM3.z_axis_last_valid = MM3.z_axis;
//MM3.z_axis = m;
}
 
 
 
MM3.STATE = MM3_TILT;
}
return;
}
}