85,7 → 85,6 |
unsigned char HoehenReglerAktiv = 0; |
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
|
|
//Salvo 12.10.2007 |
uint8_t magkompass_ok=0; |
uint8_t gps_cmd = GPS_CMD_STOP; |
398,7 → 397,6 |
} |
|
|
|
//############################################################################ |
// Trägt ggf. das Poti als Parameter ein |
void ParameterZuordnung(void) |
466,7 → 464,6 |
//******PROVISORISCH*************** |
GRN_ON; |
|
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
476,7 → 473,7 |
// und dieser dann langsam zwangsweise reduziert |
if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
{ |
if (ubat_cnt > 700) |
if (ubat_cnt > 1000) |
{ |
ubat_cnt = 0; |
if (gas_actual > ((gas_mittel*12)/15)) gas_actual--; |
592,7 → 589,7 |
if (gps_home_position.status > 0 ) |
{ |
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
beeptime = 2000; |
beeptime = 1000; |
Delay_ms(500); |
} |
} |
633,6 → 630,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
int n; |
// Salvo 9.12.2007 |
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
// Salvo End |
648,6 → 646,7 |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
} |
} |
else delay_einschalten = 0; |
934,7 → 933,6 |
v = (abs(Mittelwert_AccRoll)); |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen |
{ |
|
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
1005,33 → 1003,6 |
ZaehlMessungen = 0; |
|
|
// Salvo 6.10.2007 |
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF) |
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0)) |
{ |
if (Parameter_MaxHoehe > 200) |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOME; |
n = GPS_CRTL(gps_cmd); |
} |
else |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOLD; |
n= GPS_CRTL(gps_cmd); |
} |
} |
else |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
} // Ende Abgleich |
|
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
1055,16 → 1026,16 |
if (v <-180) v +=360; // Uberlaufkorrektur |
if (v > 180) v -=360; // Uberlaufkorrektur |
|
w = w -v; //Differenz Gyro zu Kompass ist der Driftfehler |
v = w -v; //Differenz Gyro zu Kompass ist der Driftfehler |
|
#define GIER_COMP_MAX 4 |
if (w > GIER_COMP_MAX) w= GIER_COMP_MAX; |
if (w < -GIER_COMP_MAX) w= - GIER_COMP_MAX; |
if (!(w == 0)) |
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
if (abs(w) > 1) |
{ |
GyroGier_Comp = 0; |
GyroGier_Comp = 0; |
gyrogier_kompass = KompassValue; // Kompasswert merken |
AdNeutralGier -= w; |
AdNeutralGier -= v; |
} |
} |
} |
1108,7 → 1079,36 |
} |
// 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 |
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF) |
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0) && (GasMischanteil > 30)) |
{ |
if (Parameter_MaxHoehe > 200) |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOME; |
n = GPS_CRTL(gps_cmd); |
} |
else |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOLD; |
n= GPS_CRTL(gps_cmd); |
} |
} |
else |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 10) LED_J16_OFF; //led im GPS Mode schnell blinken lassen |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1147,8 → 1147,7 |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
// Salvo End |
} |
|
} |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1158,6 → 1157,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
|
// Salvo 13.12.2007 Beleuchtung steuern |
if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
1165,7 → 1165,6 |
else LED_J16_OFF; |
// Salvo End |
|
TimerWerteausgabe = 24; |
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[2] = Mittelwert_AccNick; |
1192,12 → 1191,12 |
DebugOut.Analog[11] = Parameter_UserParam3; |
DebugOut.Analog[24] = GPS_Nick; |
DebugOut.Analog[25] = GPS_Roll; |
DebugOut.Analog[26] = gps_rel_act_position.utm_east/10; //in m ausgeben |
DebugOut.Analog[27] = gps_rel_act_position.utm_north/10; |
DebugOut.Analog[28] = gps_rel_act_position.utm_alt/10; |
DebugOut.Analog[29] = gps_sub_state+(20*gps_cmd); |
DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
DebugOut.Analog[30] = GPS_dist_2trgt; |
DebugOut.Analog[31] = (int) GyroGier_Comp; |
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |