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] |