/trunk/_Settings.h |
---|
1,11 → 1,10 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Abstimmung |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define ACC_AMPLIFY 6 |
#define ACC_AMPLIFY 3 |
#define FAKTOR_P 1 |
#define FAKTOR_I 0.0001 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debug-Interface |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/trunk/analog.c |
---|
7,10 → 7,11 |
#include "main.h" |
volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100; |
volatile int AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0; |
volatile int HiResNick = 2500, HiResRoll = 2500; |
volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0; |
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0; |
volatile char messanzahl_AccHoch = 0; |
volatile long Luftdruck = 32000; |
volatile int StartLuftdruck; |
volatile unsigned int MessLuftdruck = 1023; |
77,6 → 78,47 |
} |
Delay_ms_Mess(70); |
} |
/* |
0 n |
1 r |
2 g |
3 y |
4 n |
5 r |
6 z |
7 l |
8 n |
9 r |
10 x |
11 g |
12 n |
13 r |
14 u |
15 y |
16 n |
17 r |
18 x |
19 n |
20 r |
21 g |
22 y |
4 n |
5 r |
6 z |
7 l |
8 n |
9 r |
10 x |
11 g |
12 n |
13 r |
14 u |
15 y |
16 n |
17 r |
18 x |
*/ |
#define FILTER 3 |
//####################################################################################### |
// |
84,6 → 126,396 |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static signed int gier1, roll1, nick1; |
static signed long nick_filter, roll_filter; |
static signed int accy, accx; |
// ANALOG_OFF; |
switch(state++) |
{ |
case 0: |
J4High; |
nick1 = ADC; |
kanal = AD_ROLL; |
break; |
case 1: |
roll1 = ADC; |
kanal = AD_GIER; |
break; |
case 2: |
gier1 = ADC; |
kanal = AD_ACC_Y; |
break; |
case 3: |
J5High; |
Aktuell_ay = NeutralAccY - ADC; |
accy = Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 4: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 5: |
roll1 += ADC; |
kanal = AD_ACC_Z; |
break; |
case 6: |
AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
if(AdWertAccHoch > 1) |
{ |
if(NeutralAccZ < 750) |
{ |
NeutralAccZ += 0.02; |
if(modell_fliegt < 500) NeutralAccZ += 0.1; |
} |
} |
else if(AdWertAccHoch < -1) |
{ |
if(NeutralAccZ > 550) |
{ |
NeutralAccZ-= 0.02; |
if(modell_fliegt < 500) NeutralAccZ -= 0.1; |
} |
} |
messanzahl_AccHoch = 1; |
Aktuell_az = ADC; |
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
kanal = AD_NICK; |
break; |
case 7: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 8: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 9: |
Aktuell_ax = ADC - NeutralAccX; |
accx = Aktuell_ax; |
kanal = AD_GIER; |
break; |
case 10: |
gier1 += ADC; |
kanal = AD_NICK; |
break; |
case 11: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 12: |
roll1 += ADC; |
kanal = AD_UBAT; |
break; |
case 13: |
UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2; |
kanal = AD_ACC_Y; |
break; |
case 14: |
J5High; |
Aktuell_ay = NeutralAccY - ADC; |
accy += Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 15: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 16: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 17: |
Aktuell_ax = ADC - NeutralAccX; |
accx += Aktuell_ax; |
kanal = AD_NICK; |
break; |
case 18: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 19: |
roll1 += ADC; |
kanal = AD_GIER; |
break; |
case 20: |
gier1 += ADC; |
kanal = AD_ACC_Y; |
break; |
case 21: |
J5High; |
Aktuell_ay = NeutralAccY - ADC; |
accy += Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 22: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 23: |
roll1 += ADC; |
kanal = AD_DRUCK; |
break; |
case 24: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) |
{ |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/8; // D-Anteil = neuerWert - AlterWert |
Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; |
tmpLuftdruck = 0; |
} |
kanal = AD_NICK; |
break; |
case 25: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 26: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 27: |
Aktuell_ax = ADC - NeutralAccX; |
accx += Aktuell_ax; |
kanal = AD_GIER; |
break; |
case 28: |
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4; |
else |
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2; |
else AdWertGier = (ADC + gier1 + 1) / 2; |
kanal = AD_NICK; |
break; |
case 29: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 30: |
roll1 += ADC; |
kanal = AD_ACC_Y; |
break; |
case 31: |
J5High; |
Aktuell_ay = NeutralAccY - ADC; |
AdWertAccRoll = (Aktuell_ay + accy); |
kanal = AD_NICK; |
break; |
case 32: |
J4High; |
AdWertNick = (ADC + nick1 + 3) / 5; |
nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2; |
if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;} |
HiResNick = nick_filter - 20 * AdNeutralNick; |
AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4; |
DebugOut.Analog[21] = AdWertNickFilter / 4; |
kanal = AD_ROLL; |
break; |
case 33: |
AdWertRoll = (ADC + roll1 + 3) / 5; |
roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2; |
if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;} |
HiResRoll = roll_filter - 20 * AdNeutralRoll; |
AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4; |
DebugOut.Analog[22] = AdWertRollFilter / 4; |
kanal = AD_ACC_X; |
break; |
case 34: |
Aktuell_ax = ADC - NeutralAccX; |
AdWertAccNick = (Aktuell_ax + accx); |
kanal = AD_NICK; |
state = 0; |
AdReady = 1; |
ZaehlMessungen++; |
break; |
default: |
kanal = 0; |
state = 0; |
break; |
} |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
J4Low; |
J5Low; |
} |
/* |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static signed int gier1, roll1, nick1, nick_filter, roll_filter; |
static signed int accy, accx; |
// ANALOG_OFF; |
switch(state++) |
{ |
case 0: |
J4High; |
nick1 = ADC; |
kanal = AD_ROLL; |
break; |
case 1: |
roll1 = ADC; |
kanal = AD_GIER; |
break; |
case 2: |
gier1 = ADC; |
kanal = AD_ACC_Y; |
break; |
case 3: |
Aktuell_ay = NeutralAccY - ADC; |
accy = Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 4: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 5: |
roll1 += ADC; |
kanal = AD_ACC_Z; |
break; |
case 6: |
AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
if(AdWertAccHoch > 1) |
{ |
if(NeutralAccZ < 750) |
{ |
NeutralAccZ += 0.02; |
if(modell_fliegt < 500) NeutralAccZ += 0.1; |
} |
} |
else if(AdWertAccHoch < -1) |
{ |
if(NeutralAccZ > 550) |
{ |
NeutralAccZ-= 0.02; |
if(modell_fliegt < 500) NeutralAccZ -= 0.1; |
} |
} |
messanzahl_AccHoch = 1; |
Aktuell_az = ADC; |
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
kanal = AD_DRUCK; |
break; |
case 7: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) |
{ |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/8; // D-Anteil = neuerWert - AlterWert |
Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; |
tmpLuftdruck = 0; |
} |
kanal = AD_NICK; |
break; |
case 8: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 9: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 10: |
Aktuell_ax = ADC - NeutralAccX; |
accx = Aktuell_ax; |
kanal = AD_GIER; |
break; |
case 11: |
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1) / 2; |
else |
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1); |
else AdWertGier = (ADC + gier1); |
kanal = AD_NICK; |
break; |
case 12: |
J4High; |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 13: |
roll1 += ADC; |
kanal = AD_UBAT; |
break; |
case 14: |
UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2; |
kanal = AD_ACC_Y; |
break; |
case 15: |
Aktuell_ay = NeutralAccY - ADC; |
AdWertAccRoll = (Aktuell_ay + accy); |
kanal = AD_NICK; |
break; |
case 16: |
J4High; |
// if(PlatinenVersion == 10) AdWertNick = (ADC + nick1) / 5; |
// else AdWertNick = (5 * AdWertNick + 2*(ADC + nick1)) / 10; |
AdWertNick = 2*(ADC + nick1) / 5; |
nick_filter = (2 * nick_filter + 2 * (ADC + nick1)) / 3; |
HiResNick = nick_filter - 5 * AdNeutralNick; |
AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + 4 * HiResNick) / 4; |
DebugOut.Analog[21] = AdWertNickFilter / 4; |
kanal = AD_ROLL; |
break; |
case 17: |
// if(PlatinenVersion == 10) AdWertRoll = (ADC + roll1) / 5; |
// else AdWertRoll = (5 * AdWertRoll + 2*(ADC + roll1)) / 10; |
AdWertRoll = 2*(ADC + roll1) / 5; |
roll_filter = (2 * roll_filter + 2 * (ADC + roll1)) / 3; |
HiResRoll = roll_filter - 5 * AdNeutralRoll; |
AdWertRollFilter = (long)(3 * (long)AdWertRollFilter + 4 * HiResRoll) / 4; |
DebugOut.Analog[22] = AdWertRollFilter / 4; |
kanal = AD_ACC_X; |
break; |
case 18: |
Aktuell_ax = ADC - NeutralAccX; |
AdWertAccNick = (Aktuell_ax + accx); |
kanal = AD_NICK; |
state = 0; |
AdReady = 1; |
ZaehlMessungen++; |
J5High; |
break; |
default: |
kanal = 0; |
state = 0; |
break; |
} |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
J4Low; |
J5Low; |
} |
*/ |
/* |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static unsigned int gier1, roll1, nick1, accy, accx; |
ANALOG_OFF; |
switch(state++) |
213,3 → 645,15 |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
} |
*/ |
/trunk/analog.h |
---|
7,6 → 7,8 |
extern volatile int UBat; |
extern volatile int AdWertNick, AdWertRoll, AdWertGier; |
extern volatile int AdWertAccRoll,AdWertAccNick,AdWertAccHoch; |
extern volatile int HiResNick, HiResRoll; |
extern volatile int AdWertNickFilter, AdWertRollFilter, AdWertGierFilter; |
extern volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az; |
extern volatile long Luftdruck; |
extern volatile char messanzahl_Druck; |
37,8 → 39,7 |
#define ANALOG_OFF ADCSRA=0 |
//#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
//Free Running Mode, Division Factor 128, Interrupt on |
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(0<<ADPS0)|(1<<ADIE) |
//Free Running Mode, Division Factor 64, Interrupt on |
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(0<<ADPS0)|(1<<ADIE) |
//Signle trigger Mode, Interrupt on |
#endif //_ANALOG_H |
/trunk/eeprom.c |
---|
35,7 → 35,7 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.Gyro_D = 0; // Wert : 0-250 |
EE_Parameter.Gyro_D = 6; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
107,7 → 107,7 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
EE_Parameter.Gyro_D = 0; // Wert : 0-250 |
EE_Parameter.Gyro_D = 6; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
180,7 → 180,7 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 100; // Wert : 0-250 |
EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
EE_Parameter.Gyro_D = 0; // Wert : 0-250 |
EE_Parameter.Gyro_D = 6; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
/trunk/fc.c |
---|
58,6 → 58,7 |
unsigned char h,m,s; |
volatile unsigned int I2CTimeout = 100; |
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
int TrimNick, TrimRoll; |
int AdNeutralGierBias; |
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
89,7 → 90,7 |
char IntegralFaktor; |
int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links; |
volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8; |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
150,6 → 151,16 |
signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
int MotorSmoothing(int neu, int alt) |
{ |
int motor; |
if(neu > alt) motor = (1*(int)alt + neu) / 2; |
else motor = neu - (alt - neu)*1; |
//if(Poti2 < 20) return(neu); |
return(motor); |
} |
void Piep(unsigned char Anzahl) |
{ |
while(Anzahl--) |
184,23 → 195,24 |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
for(i=0; i<32; i++) |
#define NEUTRAL_FILTER 32 |
for(i=0; i<NEUTRAL_FILTER; i++) |
{ |
Delay_ms_Mess(10); |
gier_neutral += AdWertGier; |
nick_neutral += MesswertNick; |
roll_neutral += MesswertRoll; |
nick_neutral += AdWertNick; |
roll_neutral += AdWertRoll; |
} |
AdNeutralNick= nick_neutral / 32; |
AdNeutralRoll= roll_neutral / 32; |
AdNeutralGier= gier_neutral / 32; |
AdNeutralNick= nick_neutral / NEUTRAL_FILTER; |
AdNeutralRoll= roll_neutral / NEUTRAL_FILTER; |
AdNeutralGier= gier_neutral / NEUTRAL_FILTER; |
AdNeutralGierBias = AdNeutralGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
{ |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccY = abs(Mittelwert_AccRoll) / (4*ACC_AMPLIFY); |
NeutralAccX = abs(Mittelwert_AccNick) / (4*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
} |
else |
249,8 → 261,8 |
signed long winkel_nick, winkel_roll; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
// MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
MesswertNick = (signed int) AdWertNickFilter / 20; |
MesswertRoll = (signed int) AdWertRollFilter / 20; |
RohMesswertNick = MesswertNick; |
RohMesswertRoll = MesswertRoll; |
//DebugOut.Analog[21] = MesswertNick; |
257,10 → 269,13 |
//DebugOut.Analog[22] = MesswertRoll; |
//DebugOut.Analog[22] = Mess_Integral_Gier; |
//DebugOut.Analog[21] = MesswertNick; |
//DebugOut.Analog[22] = MesswertRoll; |
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L; |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
NaviAccNick += AdWertAccNick; |
268,8 → 283,6 |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
if(abs(Mittelwert_AccRoll > 50)) { DebugOut.Analog[16]++; DebugOut.Analog[17] = Mittelwert_AccRoll;}; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
AdReady = 0; |
301,6 → 314,7 |
KopplungsteilRollNick = tmpl4; |
tmpl4 -= tmpl3; |
ErsatzKompass += tmpl4; |
if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
tmpl *= Parameter_AchsKopplung1; // 90 |
310,20 → 324,21 |
tmpl2 /= 4096L; |
if(labs(tmpl) > 128 || labs(tmpl2) > 128 /* || abs(KopplungsteilNickRoll) > 128 || abs(KopplungsteilRollNick) > 128)*/) TrichterFlug = 1; |
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
DebugOut.Analog[21] = KopplungsteilNickRoll; |
DebugOut.Analog[22] = KopplungsteilRollNick; |
} |
else tmpl = tmpl2 = 0; |
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
MesswertRoll += tmpl; |
MesswertRoll += tmpl2 / 100L; // War: *5/512 |
// MesswertRoll += tmpl; |
// MesswertRoll += tmpl2 / 100L; // War: *5/512 |
// MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
Mess_IntegralRoll2 += MesswertRoll; |
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; |
Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
if(Mess_IntegralRoll > Umschlag180Roll) |
{ |
348,11 → 363,11 |
if(AdWertRoll > 2015) MesswertRoll = +2000; |
} |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
MesswertNick -= tmpl2; |
// MesswertNick -= tmpl2; |
// MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
MesswertNick -= tmpl / 100L; //109 |
Mess_IntegralNick2 += MesswertNick; |
Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
// MesswertNick -= tmpl / 100L; //109 |
Mess_IntegralNick2 += MesswertNick + TrimNick; |
Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
if(Mess_IntegralNick > Umschlag180Nick) |
{ |
382,29 → 397,34 |
IntegralRoll = Mess_IntegralRoll; |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
MesswertNick = (RohMesswertNick + 2 * MesswertNick) / 3; |
MesswertRoll = (RohMesswertRoll + 2 * MesswertRoll) / 3; |
// MesswertNick = (RohMesswertNick + 2 * MesswertNick) / 3; |
// MesswertRoll = (RohMesswertRoll + 2 * MesswertRoll) / 3; |
#define D_LIMIT 8 |
#define D_LIMIT 128 |
MesswertNick = HiResNick / 20; |
MesswertRoll = HiResRoll / 20; |
if(Parameter_Gyro_D) |
{ |
d2Nick = (((MesswertNick - oldNick))); |
oldNick = MesswertNick; |
d2Nick = HiResNick - oldNick; |
oldNick = (oldNick + HiResNick)/2; |
if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D); |
d2Roll = (((MesswertRoll - oldRoll))); |
oldRoll = MesswertRoll; |
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
d2Roll = HiResRoll - oldRoll; |
oldRoll = (oldRoll + HiResRoll)/2; |
if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
} |
if(RohMesswertRoll > 0) MesswertRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
else MesswertRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
if(RohMesswertNick > 0) MesswertNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
else MesswertNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
{ |
460,7 → 480,7 |
{ |
if(!MotorenEin) |
{ |
#ifdef OCTO |
#ifndef QUADRO |
Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0; |
if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];} |
if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];} |
560,6 → 580,7 |
static unsigned char delay_neutral = 0; |
static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
static int hoehenregler = 0; |
static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8; |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
static long ausgleichNick, ausgleichRoll; |
931,6 → 952,7 |
tmp_long2 /= 3; |
} |
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
937,6 → 959,8 |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
} |
//if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;} |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
} |
1208,7 → 1232,7 |
DebugOut.Analog[25] = MesswertRoll/2; |
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
1247,10 → 1271,16 |
if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN); |
#define TRIM_MAX (Poti1 * 2) |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
#ifdef OCTO |
{ |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
} |
#ifndef QUADRO |
MesswertGier = (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN)); |
#else |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN)); |
1395,28 → 1425,32 |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
#ifndef OCTO |
#ifdef QUADRO |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Quadro-Mischer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; // Mischer |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
motorwert1 = MotorSmoothing(motorwert,motorwert1); |
motorwert = motorwert1 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Vorne = motorwert; |
motorwert = GasMischanteil - pd_ergebnis_nick + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
motorwert2 = MotorSmoothing(motorwert,motorwert2); |
motorwert = motorwert2 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Hinten = motorwert; |
motorwert = GasMischanteil + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
motorwert3 = MotorSmoothing(motorwert,motorwert3); |
motorwert = motorwert3 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Links = motorwert; |
motorwert = GasMischanteil - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
motorwert4 = MotorSmoothing(motorwert,motorwert4); |
motorwert = motorwert4 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
#else |
#endif |
#ifdef OCTO |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Octo-Mischer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1453,10 → 1487,47 |
Motor8 = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
#endif |
/* |
#ifdef OCTO2 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Octo-Mischer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor1 = motorwert; |
motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor2 = motorwert; |
motorwert = GasMischanteil + - pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor3 = motorwert; |
motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor4 = motorwert; |
motorwert = GasMischanteil - pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor5 = motorwert; |
motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor6 = motorwert; |
motorwert = GasMischanteil + pd_ergebnis_roll + GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor7 = motorwert; |
motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
Motor8 = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
#endif |
if(Poti1 > 20) Motor1 = 0; |
if(Poti1 > 90) Motor6 = 0; |
if(Poti1 > 140) Motor2 = 0; |
if(Poti1 > 200) Motor7 = 0; |
*/ |
} |
/trunk/fc.h |
---|
33,6 → 33,7 |
extern int KompassValue; |
extern int KompassStartwert; |
extern int KompassRichtung; |
extern int TrimNick, TrimRoll; |
extern long ErsatzKompass; |
extern int ErsatzKompassInGrad; // Kompasswert in Grad |
extern int HoehenWert; |
/trunk/main.c |
---|
240,13 → 240,13 |
{ |
if(UpdateMotor && AdReady) // ReglerIntervall |
{ |
//J3High; |
J3High; |
UpdateMotor=0; |
if(WinkelOut.CalcState) CalMk3Mag(); |
else MotorRegler(); |
SendMotorData(); |
ROT_OFF; |
//J3Low; |
J3Low; |
if(PcZugriff) PcZugriff--; |
else |
{ |
/trunk/main.h |
---|
1,8 → 1,11 |
#ifndef _MAIN_H |
#define _MAIN_H |
#define QUADRO |
//#define OCTO |
//#define OCTO2 |
//Hier die Quarz Frequenz einstellen |
#if defined (__AVR_ATmega32__) |
#define SYSCLK 20000000L //Quarz Frequenz in Hz |
/trunk/makefile |
---|
5,7 → 5,7 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 72 |
VERSION_PATCH = 9 |
VERSION_PATCH = 11 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol |
VERSION_SERIAL_MINOR = 0 # Serial Protocol |
78,6 → 78,15 |
ifeq ($(VERSION_PATCH), 10) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k |
endif |
ifeq ($(VERSION_PATCH), 11) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)L |
endif |
ifeq ($(VERSION_PATCH), 12) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m |
endif |
ifeq ($(VERSION_PATCH), 13) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n |
endif |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
/trunk/rc.c |
---|
72,10 → 72,11 |
PPM_in[index] = tmp; |
} |
index++; |
/* |
if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen |
if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen |
if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen |
*/ |
} |
} |
} |
/trunk/timer0.c |
---|
123,7 → 123,7 |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while (!CheckDelay(akt)) ANALOG_ON; |
while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;} |
} |
/*****************************************************/ |
/trunk/twimaster.c |
---|
27,7 → 27,6 |
} |
//############################################################################ |
//Start I2C |
void i2c_stop(void) |
//############################################################################ |
{ |
53,7 → 52,6 |
} |
//############################################################################ |
//Start I2C |
char i2c_write_byte(char byte) |
//############################################################################ |
{ |
65,9 → 63,8 |
} |
#ifdef OCTO |
#ifndef QUADRO |
//############################################################################ |
//Start I2C |
SIGNAL (TWI_vect) |
//############################################################################ |
{ |
241,7 → 238,6 |
} |
#else |
//############################################################################ |
//Start I2C |
SIGNAL (TWI_vect) |
//############################################################################ |
{ |
/trunk/version.txt |
---|
215,3 → 215,12 |
0.72j: H.Buss 09.02.2009 |
- neue Implementierung der Servoausgänge |
0.72k: H.Buss 10.02.2009 |
- Abtastrate auf 5kHz erhöht |
0.72L: H.Buss 13.02.2009 |
- Signalfilterung überarbeitet |
- OCTO2 implementiert |