Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 564 → Rev 565

/branches/salvo_gps/Basis_v0067g/trunk/fc.c
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];