Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 420 → Rev 446

/branches/salvo_gps/Basis_V0066c/trunk/fc.c
52,6 → 52,12
// + 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 6.10.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem Neutralwert
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
*/
 
#include "main.h"
#include "eeprom.c"
80,6 → 86,19
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;
static int ubat_cnt =0;
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
int w,v;
//Salvo End
 
//Salvo 2.9.2007 Ersatzkompass
volatile long GyroKomp_Int;
volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass
// Salvo End
 
float GyroFaktor;
float IntegralFaktor;
 
103,7 → 122,7
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
unsigned char Parameter_Gyro_P = 50; // Wert : 10-250
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
unsigned char Parameter_Gier_P = 2; // Wert : 1-20
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
140,6 → 159,7
AdNeutralGier = 0;
Parameter_AchsKopplung1 = 0;
Parameter_AchsGegenKopplung1 = 0;
 
CalibrierMittelwert();
Delay_ms_Mess(100);
CalibrierMittelwert();
153,8 → 173,19
AdNeutralGier= AdWertGier;
StartNeutralRoll = AdNeutralRoll;
StartNeutralNick = AdNeutralNick;
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 
// Salvo 1.9.2007 ACC mit festen neutralwerten *****
if (ACC_NEUTRAL_FIXED > 0)
{
NeutralAccX = ACC_NICK_NEUTRAL;
NeutralAccY = ACC_ROLL_NEUTRAL;
}
else
{ // Automatisch bei Offsetabgleich ermitteln
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
}
// Salvo End
NeutralAccZ = Aktuell_az;
Mess_IntegralNick = 0;
173,6 → 204,12
beeptime = 50;
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
 
//Salvo 13.10.2007 Ersatzkompass und Gas
GyroKomp_Int = KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
gas_mittel = 30;
gas_actual = gas_mittel;
// Salvo End
}
 
//############################################################################
193,6 → 230,9
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
IntegralAccZ += Aktuell_az - 704;//NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
//Salvo 12.11.2007
GyroKomp_Int += MesswertGier;
//Salvo End
Mess_Integral_Gier += MesswertGier;
Mess_Integral_Gier2 += MesswertGier;
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
400,15 → 440,55
static long ausgleichNick, ausgleichRoll;
Mittelwert();
//****** GPS Daten holen ***************
short int n;
if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
n = Get_Rel_Position();
if (n == 0)
{
ROT_ON; //led blitzen lassen
}
//******PROVISORISCH***************
GRN_ON;
 
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
// und dieser dann langsam zwangsweise reduziert
if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren
{
if (ubat_cnt > 700)
{
ubat_cnt = 0;
if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
}
else ubat_cnt++;
if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
}
else //Falls UBAT wieder ok ist
{
if (ubat_cnt > 1000)
{
gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
gas_actual = GasMischanteil;
}
else
{
ubat_cnt++;
if ((ubat_cnt % 10) == 0)
{
if (gas_actual < GasMischanteil) gas_actual++;
else gas_actual = GasMischanteil;
}
}
GasMischanteil = gas_actual;
}
// Salvo End
if(GasMischanteil < 0) GasMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
467,6 → 547,7
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
SetNeutral();
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
480,14 → 561,25
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken
}
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
 
 
 
 
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();
 
Piep(GetActiveParamSetNumber());
}
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
if (gps_home_position.status > 0 )
{
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind
beeptime = 2000;
Delay_ms(500);
}
}
}
else delay_neutral = 0;
}
641,6 → 733,7
}
 
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
731,7 → 824,13
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 
//Salvo Ersatzkompass Ueberlauf korrigieren
if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
ROT_OFF;
// Salvo End
 
/*
DebugOut.Analog[30] = I_LageRoll * 10;
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
741,6 → 840,7
DebugOut.Analog[22] = MittelIntegralRoll / 26;
DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
*/
 
#define FEHLER_LIMIT (ABGLEICH_ANZAHL/2)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4)
776,7 → 876,7
 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
ausgleichRoll = 0;
ausgleichRoll = 0;
if(IntegralFehlerRoll > FEHLER_LIMIT2)
{
if(last_r_p)
830,29 → 930,87
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// Salvo Ersatzkompass 26.9.2007 **********************
if ((Kompass_Neuer_Wert > 0))
{
Kompass_Neuer_Wert = 0;
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alles ok
{
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
{
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
w = KompassValue - GyroKomp_Int;
if ((w > 0) && (w < 180))
{
++GyroKomp_Int;
}
else if ((w > 0) && (w >= 180))
{
--GyroKomp_Int;
}
else if ((w < 0) && (w >= -180))
{
--GyroKomp_Int;
}
else if ((w < 0) && (w < -180))
{
++GyroKomp_Int;
}
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360;
GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
}
}
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
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(StickRoll) < GPS_STICK_HOLDOFF) && (abs(StickNick) < 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 (n= GPS_CRTL(GPS_CMD_STOP)); //GPS Lageregelung beenden
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
{
int w,v;
static int SignalSchlecht = 0;
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 < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
if(v > w) w = v; // grösste Neigung ermitteln
 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
// Salvo 13.9.2007
w=0;
// Salvo End
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
if(SignalSchlecht) SignalSchlecht--;
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
870,10 → 1028,25
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mittelwert_AccHoch;
DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 
// Diverse parameter Debugging
DebugOut.Analog[16] = dataset_cnt;
DebugOut.Analog[17] = UBat;
DebugOut.Analog[18] = MesswertNick;
DebugOut.Analog[19] = MesswertRoll;
DebugOut.Analog[20] = MesswertGier;
DebugOut.Analog[21] = StickNick;
DebugOut.Analog[22] = StickRoll;
DebugOut.Analog[23] = StickGier;
 
 
// GPS Debugging
DebugOut.Analog[26] = gps_rel_act_position.utm_east;
DebugOut.Analog[27] = gps_rel_act_position.utm_north;
DebugOut.Analog[28] = gps_rel_act_position.utm_alt;
DebugOut.Analog[29] = gps_sub_state+(20*gps_cmd);
 
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];