51,7 → 51,7 |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 9.11.2008 |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 29.12.2008 |
/* |
Driftkompensation fuer Gyros verbessert |
Linearsensor optional mit fixem Neutralwert |
1101,6 → 1101,7 |
ZaehlMessungen = 0; |
} |
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
|
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
{ |
1111,7 → 1112,6 |
{ |
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
{ |
|
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
{ |
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
1166,20 → 1166,19 |
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
} |
} |
else //Kompassfehler |
else //Kompass wegen grosser Neigung ungueltig |
{ |
magkompass_ok = 0; |
GyroGier_Comp = 0; |
} |
Kompass_Value_Old = KompassValue; |
Kompass_Value_Old =KompassValue; |
} |
else magkompass_ok = 0; |
|
// Salvo End ************************* |
|
|
// Salvo 6.10.2007 |
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
//GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
//GPS Hold Aktivieren wenn Knueppel in Ruhelage sind |
if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
{ |
1219,10 → 1218,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
if(abs(StickGier) > 15) // war 35 |
{ |
{ |
KompassSignalSchlecht = 1000; |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
} |
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
1233,27 → 1232,24 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
|
// Salvo 29.12.2008 |
if (NeueKompassRichtungMerken) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
if (NeueKompassRichtungMerken) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
// Salvo 13.9.2007 |
w=0; |
w=0; |
// w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
// Salvo End |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w >= 0) |
{ |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
// Salvo End |
} |
} |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |