Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1165 → Rev 1166

/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