Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2480 → Rev 2481

/trunk/fc.c
60,7 → 60,7
 
unsigned char h,m,s;
unsigned int BaroExpandActive = 0;
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
int MesswertNick,MesswertRoll,MesswertGier,RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll;
69,14 → 69,13
int NeutralAccZ = 0;
signed char NeutralAccZfine = 0;
unsigned char ControlHeading = 0;// in 2°
long IntegralNick = 0,IntegralNick2 = 0;
long IntegralRoll = 0,IntegralRoll2 = 0;
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
long IntegralNick = 0;
long IntegralRoll = 0;
long Integral_Gier = 0;
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
long Mess_IntegralNick = 0;
long Mess_IntegralRoll = 0;
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
long MittelIntegralNick,MittelIntegralRoll;
long SummeNick=0,SummeRoll=0;
volatile long Mess_Integral_Hoch = 0;
int KompassValue = -1;
174,6 → 173,7
int MaxStickNick = 0,MaxStickRoll = 0;
unsigned int modell_fliegt = 0;
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
unsigned char FC_StatusFlags3 = 0;
long GIER_GRAD_FAKTOR = 1291;
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
signed int tmp_motorwert[MAX_MOTORS];
185,6 → 185,7
unsigned char ACC_AltitudeControl = 0;
unsigned char LowVoltageLandingActive = 0;
unsigned char LowVoltageHomeActive = 0;
signed int DriftNick = 0, DriftRoll = 0;
 
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
#define OPA_OFFSET_STEP 5
279,6 → 280,7
// Parameter: 0 -> after switch on (ignore ACC-Z fault)
// Parameter: 1 -> before Start
// Parameter: 2 -> calibrate and store ACC
// Parameter: 3 -> use stored Gyro calibration Data from EEPROM
unsigned char SetNeutral(unsigned char AdjustmentMode) // retuns: "sucess"
//############################################################################
{
299,6 → 301,8
Parameter_AchsKopplung2 = 0;
 
ExpandBaro = 0;
if(AdjustmentMode == 3) FC_StatusFlags3 |= FC_STATUS3_BOAT;
// else FC_StatusFlags3 &= ~FC_STATUS3_BOAT; -> do not clear that
 
CalibrierMittelwert();
Delay_ms_Mess(100);
340,6 → 344,9
SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);
SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ);
SetParamWord(PID_GYRO_NICK,(uint16_t)AdNeutralNick);
SetParamWord(PID_GYRO_ROLL,(uint16_t)AdNeutralRoll);
SetParamWord(PID_GYRO_YAW,(uint16_t)AdNeutralGier);
}
else
{
354,6 → 361,28
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
sucess = 0;
}
if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom
{
unsigned int nick, roll, gier;
// restore from eeprom
nick = (int16_t)GetParamWord(PID_GYRO_NICK);
roll = (int16_t)GetParamWord(PID_GYRO_ROLL);
gier = (int16_t)GetParamWord(PID_GYRO_YAW);
// strange settings?
if(((unsigned int) nick > (600 * 16)) || ((unsigned int) nick < (400 * 16))
|| ((unsigned int) roll > (600 * 16)) || ((unsigned int) roll < (400 * 16))
|| ((unsigned int) gier > (600 * 2)) || ((unsigned int) gier < (400 * 2)))
{
printf("\n\rGyro calibration data not valid\r\n");
sucess = 0;
}
else
{
AdNeutralNick = nick;
AdNeutralRoll = roll;
AdNeutralGier = gier;
}
}
}
EEAR = EE_DUMMY; // Set the EEPROM Address pointer to an unused space
MesswertNick = 0;
421,6 → 450,7
SummenHoehe = 0; Mess_Integral_Hoch = 0;
DebugOut.Analog[28] = 0; // I2C-Counter
CalcExpandBaroStep();
if(FC_StatusFlags3 & FC_STATUS3_BOAT && !EE_Parameter.Driftkomp) EE_Parameter.Driftkomp = 4;
return(sucess);
}
 
442,12 → 472,9
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L;
Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 4L;
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
NaviAccNick += AdWertAccNick;
NaviAccRoll += AdWertAccRoll;
NaviCntAcc++;
IntegralAccZ += Aktuell_az - NeutralAccZ;
 
//++++++++++++++++++++++++++++++++++++++++++++++++
HoehenWert = HoehenWert_Mess;
504,38 → 531,31
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll;
if(Mess_IntegralRoll > Umschlag180Roll)
{
Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(Mess_IntegralRoll <-Umschlag180Roll)
{
Mess_IntegralRoll = (Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
Mess_IntegralNick2 += MesswertNick + TrimNick;
Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick;
if(Mess_IntegralNick > Umschlag180Nick)
{
Mess_IntegralNick = -(Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(Mess_IntegralNick <-Umschlag180Nick)
{
Mess_IntegralNick = (Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
 
Integral_Gier = Mess_Integral_Gier;
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
IntegralRoll2 = Mess_IntegralRoll2;
 
 
#define D_LIMIT 128
 
MesswertNick = HiResNick / 8;
620,7 → 640,7
// Make noise
if((BeepMuster == 0xffff)) { beeptime = 25000; BeepMuster = 0x0080; }
// Do not send I2C-Data
if(RedundanceBlOperation)
if(FC_StatusFlags3 & FC_STATUS3_REDUNDANCE)
{
I2CTimeout = 100;
return;
778,13 → 798,10
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
int GierMischanteil,GasMischanteil;
static long sollGier = 0,tmp_long,tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
static signed char move_safety_switch = 0;
static long ausgleichNick, ausgleichRoll;
int IntegralNickMalFaktor,IntegralRollMalFaktor;
unsigned char i;
Mittelwert();
954,26 → 971,26
#endif
}
} // end of: modell_fliegt > 256
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0 && !(NC_To_FC_Flags & NC_TO_FC_SIMULATION_ACTIVE))
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 75) // Neutralwerte
{
if(++delay_neutral > 200) // nicht sofort
{
unsigned char setting = 0;
delay_neutral = 0;
modell_fliegt = 0;
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
{
unsigned char setting=1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -70) setting = 1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
SetActiveParamSet(setting); // aktiven Datensatz merken
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -70) setting = 5;
if(setting) SetActiveParamSet(setting); // aktiven Datensatz merken
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
{
990,7 → 1007,10
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
CalibrationDone = SetNeutral(1);
if(!setting && PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) CalibrationDone = SetNeutral(3);
else
if(!setting && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75 && abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < 70) CalibrationDone = SetNeutral(2); // store ACC values into EEPROM
else CalibrationDone = SetNeutral(1);
ServoActive = 1;
DDRD |=0x80; // enable J7 -> Servo signal
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
998,12 → 1018,13
else
if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION;
else SpeakHoTT = SPEAK_CALIBRATE;
ShowSettingNameTime = 5; // for HoTT & Jeti
ShowSettingNameTime = 10; // for HoTT & Jeti
#endif
Piep(ActiveParamSet,120);
}
}
}
/*
else
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
{
1022,6 → 1043,7
Piep(ActiveParamSet,120);
}
}
*/
else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1035,8 → 1057,9
// Motoren Starten
if(!MotorenEin)
{
if((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -100) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) || EE_Parameter.MotorSafetySwitch == 0)))
|| (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] > -10 && move_safety_switch == 100)))
if(((((PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -100) && ((!(EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] < -75) || EE_Parameter.MotorSafetySwitch == 0)))
|| (((EE_Parameter.GlobalConfig3 & CFG3_MOTOR_SWITCH_MODE) && PPM_in[EE_Parameter.MotorSafetySwitch] > -10 && move_safety_switch == 100)))
&& !(NC_To_FC_Flags & NC_TO_FC_SIMULATION_ACTIVE))
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
1046,7 → 1069,7
StartLuftdruck = Luftdruck;
HoehenWert = 0;
HoehenWert_Mess = 0;
GasIsZeroCnt = 600;
GasIsZeroCnt = 600;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
HoehenWertF_Mess = 0;
#endif
1054,6 → 1077,8
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 100) && EE_Parameter.MotorSafetySwitch == 0) delay_einschalten = 0;
if(++delay_einschalten > 253)
{
if(FC_StatusFlags3 & FC_STATUS3_BOAT) { if((abs(MesswertGier) > 32*2 || abs(MesswertNick) > 20*3) || abs(MesswertRoll) > 20*3) CalibrationDone = 0; } // dann ist der Gyro defekt, schlecht kalibriert oder der MK dreht sich
else
if((abs(MesswertGier) > 32 || abs(MesswertNick) > 20) || abs(MesswertRoll) > 20) CalibrationDone = 0; // dann ist der Gyro defekt, schlecht kalibriert oder der MK dreht sich
delay_einschalten = 0;
if(!VersionInfo.HardwareError[0] && CalibrationDone && !NC_ErrorCode)
1065,8 → 1090,6
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
// ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
1315,19 → 1338,10
 
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
 
if(Looping_Nick || Looping_Roll)
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
1377,19 → 1391,48
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
}
 
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
 
//DebugOut.Analog[16] += tmp_long;
//DebugOut.Analog[17] += tmp_long2;
 
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
DriftNick += tmp_long;
DriftRoll += tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ZaehlMessungen >= ABGLEICH_ANZAHL)
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
// static int cnt = 0;
// static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(EE_Parameter.Driftkomp && abs(Mittelwert_AccNick) < 200*4 && abs(Mittelwert_AccRoll) < 200*4 && !TrichterFlug && abs(MesswertGier) < 32/* && (FC_StatusFlags & FC_STATUS_FLY)*/)
{
DebugOut.Analog[16] = EE_Parameter.Driftkomp;
DriftNick -= DriftNick / (64 * (unsigned int) EE_Parameter.Driftkomp);
DriftRoll -= DriftRoll / (64 * (unsigned int) EE_Parameter.Driftkomp);
GierGyroFehler -= GierGyroFehler / (64 * (unsigned int) EE_Parameter.Driftkomp);
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
{
DriftNick /= 2;
DriftRoll /= 2;
GierGyroFehler = 0;
}
if(DriftNick > 3000) { DriftNick = 0; AdNeutralNick++;}
if(DriftNick <-3000) { DriftNick = 0; AdNeutralNick--;}
if(DriftRoll > 3000) { DriftRoll = 0; AdNeutralRoll++;}
if(DriftRoll <-3000) { DriftRoll = 0; AdNeutralRoll--;}
if(GierGyroFehler > 3500) { GierGyroFehler = 0; AdNeutralGier++; }
if(GierGyroFehler <-3500) { GierGyroFehler = 0; AdNeutralGier--; }
}
else
{
DriftNick = 0;
DriftRoll = 0;
GierGyroFehler = 0;
}
TrichterFlug = 0;
 
/*
if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
{
MittelIntegralNick /= ABGLEICH_ANZAHL;
1438,6 → 1481,7
#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
#define BEWEGUNGS_LIMIT 20000
 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
1519,19 → 1563,14
LageKorrekturNick = 0;
TrichterFlug = 0;
}
 
*/
if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
} // ZaehlMessungen >= ABGLEICH_ANZAHL
 
2287,6 → 2326,5
#ifdef REDUNDANT_FC_MASTER
if(Parameter_UserParam6 > 230) Motor[0].SetPoint = 0;
#endif
 
}
//DebugOut.Analog[16]