WebSVN - FlightCtrl - Rev 1286 - /branches/V0.74d_ACC-HH

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#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 messanzahl_AccHoch = 0;
volatile long Luftdruck = 32000;
volatile int StartLuftdruck;
volatile unsigned int MessLuftdruck = 1023;
unsigned char DruckOffsetSetting;
signed char ExpandBaro = 0;
volatile int HoeheD = 0;
volatile char messanzahl_Druck;
volatile int tmpLuftdruck;
volatile unsigned int ZaehlMessungen = 0;
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
volatile unsigned char AdReady = 1;
//#######################################################################################
//
void ADC_Init(void)
//#######################################################################################
{
ADMUX = 0;//Referenz ist extern
ANALOG_ON;
}

void SucheLuftruckOffset(void)
{
unsigned int off;
off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
if(off > 20) off -= 10;
OCR0A = off;
ExpandBaro = 0;
Delay_ms_Mess(100);
if(MessLuftdruck < 850) off = 0;
for(; off < 250;off++)
{
OCR0A = off;
Delay_ms_Mess(50);
printf(".");
if(MessLuftdruck < 850) break;
}
eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
DruckOffsetSetting = off;
Delay_ms_Mess(300);
}

void SucheGyroOffset(void)
{
unsigned char i, ready = 0;
int timeout;
GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
timeout = SetDelay(2000);
for(i=140; i != 0; i--)
{
if(ready == 3 && i > 10) i = 9;
ready = 0;
if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
twi_state = 8;
i2c_start();
if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;}
messanzahl_Druck = 0;
ANALOG_ON;
while(messanzahl_Druck == 0);
if(i<10) Delay_ms_Mess(10);
}
Delay_ms_Mess(70);
}

/* // MartinR: so war es
0 n
1 r
2 g
3 y
4 x
5 n
6 r
7 u
8 z
9 L
10 n
11 r
12 g
13 y
14 x
15 n
16 r
17 L
*/

/* MartinR : geändert
0 n
1 r
2 g
3 L
4 n
5 r
6 y
7 x
8 n
9 r
10 g
11 L
12 n
13 r
14 u
15 z
*/

//#######################################################################################
//
SIGNAL(SIG_ADC)
//#######################################################################################
{
static unsigned char kanal=0,state = 0;
static signed int gier1, roll1, nick1, nick_filter, roll_filter;
static signed int roll_alt1, roll_alt2, roll_alt3, nick_alt1, nick_alt2, nick_alt3; // MartinR: 4x Mittelung
//static signed int roll_alt1, nick_alt1; // MartinR: 2x Mittelung
static signed int accy, accx;

/* // MartinR : so war es Anfang
switch(state++)
{
case 0:
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_ACC_X;
break;
case 4:
Aktuell_ax = ADC - NeutralAccX;
accx = Aktuell_ax;
kanal = AD_NICK;
break;
case 5:
nick1 += ADC;
kanal = AD_ROLL;
break;
case 6:
roll1 += ADC;
kanal = AD_UBAT;
break;
case 7:
UBat = (3 * UBat + ADC / 3) / 4;
kanal = AD_ACC_Z;
break;
case 8:
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 8:" fehlt hier absichtlich
case 10:
nick1 += ADC;
kanal = AD_ROLL;
break;
case 11:
roll1 += ADC;
kanal = AD_GIER;
break;
case 12:
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 1) / 2;
else
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1);
else    AdWertGier = (ADC + gier1);
kanal = AD_ACC_Y;
break;
case 13:
Aktuell_ay = NeutralAccY - ADC;
AdWertAccRoll = (Aktuell_ay + accy);
kanal = AD_ACC_X;
break;
case 14:
Aktuell_ax = ADC - NeutralAccX;
AdWertAccNick = (Aktuell_ax + accx);
kanal = AD_NICK;
break;
case 15:
nick1 += ADC;
if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4;
AdWertNick = nick1 / 8;
nick_filter = (nick_filter + nick1) / 2;
HiResNick = nick_filter - AdNeutralNick;
AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
kanal = AD_ROLL;
break;
case 16:
roll1 += ADC;
if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4;
AdWertRoll = roll1 / 8;
roll_filter = (roll_filter + roll1) / 2;
HiResRoll = roll_filter - AdNeutralRoll;
AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
kanal = AD_DRUCK;
break;
case 17:
state = 0;
AdReady = 1;
ZaehlMessungen++;
// "break" fehlt hier absichtlich
case 9:
tmpLuftdruck += ADC;
if(++messanzahl_Druck >= 5)
{
tmpLuftdruck /= 2;
MessLuftdruck = ADC;
messanzahl_Druck = 0;
HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32; // D-Anteil = neuerWert - AlterWert
Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
tmpLuftdruck /= 2;
}
kanal = AD_NICK;
break;
default:
kanal = 0; state = 0; kanal = AD_NICK;
break;
}
*/ // MartinR : so war es Ende
// MartinR : geändert Anfang

switch(state++)
{
case 0:
nick1 = ADC;
kanal = AD_ROLL;
break;
case 1:
roll1 = ADC;
kanal = AD_GIER;
break;

case 2:
gier1 = ADC;
kanal = AD_DRUCK;
break;
case 3:
tmpLuftdruck += ADC;
messanzahl_Druck ++;
kanal = AD_NICK;
break;

case 4:
nick1 += ADC;
kanal = AD_ROLL;
break;
case 5:
roll1 += ADC;
kanal = AD_ACC_Y;
break;

case 6:
Aktuell_ay = NeutralAccY - ADC;
AdWertAccRoll = (Aktuell_ay + accy);
accy = Aktuell_ay;
kanal = AD_ACC_X;
break;
case 7:
Aktuell_ax = ADC - NeutralAccX;
AdWertAccNick = (Aktuell_ax + accx);
accx = Aktuell_ax;
kanal = AD_NICK;
break;

case 8:
nick1 += ADC;
kanal = AD_ROLL;
break;
case 9:
roll1 += ADC;
kanal = AD_GIER;
break;

case 10:
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 1) / 2;
else
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1);
else    AdWertGier = (ADC + gier1);
kanal = AD_DRUCK;
break;

case 11:

// MartinR: neu
tmpLuftdruck += ADC;
//MessLuftdruck = tmpLuftdruck / 2;
MessLuftdruck = ADC;

if(++messanzahl_Druck >= 5)
{
tmpLuftdruck -= 2616L * ExpandBaro; // -523.19 counts per 10 counts offset step
tmpLuftdruck /= 2;
messanzahl_Druck = 0;
//HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)((int)(StartLuftdruck - tmpLuftdruck) - HoehenWert))/32; // D-Anteil = neuerWert - AlterWert
Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
HoehenWert = StartLuftdruck - Luftdruck;
//tmpLuftdruck /= 2;
tmpLuftdruck = 0;
}

// Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
// HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; // MartinR: zu Höhenregler

kanal = AD_NICK;
break;

case 12:
nick1 += ADC;
//if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4; // MartinR: so war es
//if(PlatinenVersion == 10) ; else nick1 *= 2; // MartinR: 2x Mittelung
if(PlatinenVersion == 10) nick1 /= 2; //else nick1 *= 1; // MartinR: 4x Mittelung
//AdWertNick = nick1 / 8; // MartinR: so war es
//AdWertNick = nick1 / 4; // MartinR: 2x Mittelung
AdWertNick = nick1 / 2; // MartinR: 4x Mittelung

// nick_filter = (nick_filter + nick1) / 2; // MartinR: so war es
//nick_filter = (nick_alt1 + nick1); // MartinR: anderer Nick-Filter: 2x Mittelung
nick_filter = (nick_alt3 +nick_alt2 +nick_alt1 + nick1); // MartinR: anderer Nick-Filter: 4x Mittelung
nick_alt3 = nick_alt2; // MartinR: anderer Nick-Filter
nick_alt2 = nick_alt1; // MartinR: anderer Nick-Filter
nick_alt1 = nick1; // MartinR: anderer Nick-Filter

HiResNick = nick_filter - AdNeutralNick;
//AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2; // MartinR: so war es
AdWertNickFilter = HiResNick; // MartinR: Filter deaktiviert
kanal = AD_ROLL;

break;
case 13:
roll1 += ADC;
// if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4; // MartinR: so war es
//if(PlatinenVersion == 10) ; else roll1 *= 2; // MartinR: 2x Mittelung
if(PlatinenVersion == 10) roll1 /= 2; //else roll1 *= 1; // MartinR: 4x Mittelung
// AdWertRoll = roll1 / 8; // MartinR: so war es
//AdWertRoll = roll1 / 4; // MartinR: 2x Mittelung
AdWertRoll = roll1 / 2; // MartinR: 4x Mittelung

//roll_filter = (roll_filter + roll1) / 2; // MartinR: so war es
//roll_filter = (roll_alt1 + roll1); // MartinR: anderer Nick-Filter: 2x Mittelung
roll_filter = (roll_alt3 + roll_alt2 + roll_alt1 + roll1); // MartinR: anderer Nick-Filter: 4x Mittelung
roll_alt3 = roll_alt2; // MartinR: anderer Nick-Filter
roll_alt2 = roll_alt1; // MartinR: anderer Nick-Filter
roll_alt1 = roll1; // MartinR: anderer Nick-Filter

HiResRoll = roll_filter - AdNeutralRoll;
//AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2; // MartinR: so war es
AdWertRollFilter = HiResRoll; // MartinR: Filter deaktiviert

kanal = AD_UBAT;

break;

case 14:
UBat = (3 * UBat + ADC / 3) / 4;
kanal = AD_ACC_Z;
break;

case 15:
// AdWertAccHoch = (signed int) ADC - NeutralAccZ; // MartinR : so war es
//AdWertAccHoch = ADC - NeutralAccZ; // MartinR: modifiziert für Tests
/* // MartinR: deaktiviert

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;
}
}
*/ // MartinR: deaktiviert Ende

// messanzahl_AccHoch = 1; MartinR: deaktiviert, da nicht verwendet

Aktuell_az = (Aktuell_az + ADC) / 2;
AdWertAccHoch = Aktuell_az - NeutralAccZ; // MartinR: modifiziert für Tests
//Mess_Integral_Hoch += AdWertAccHoch; // Integrieren // MartinR: zu Höhenregler in fc.c verschoben
//Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen // MartinR: zu Höhenregler in fc.c verschoben

AdReady = 1;
state = 0;
kanal = AD_NICK;
break;

default:
kanal = 0; state = 0; kanal = AD_NICK;
break;
}

// MartinR : geändert Ende

ADMUX = kanal;
// if(state != 0) ANALOG_ON; // MartinR : so war es:
// Bei jedem Reglerzyklus wurde nur der Ablauf der Reihenfolge der AD-Wandlungen angestoßen.
// Solange die Reihe nicht durchgelaufen ist, wurde nach jeder AD-Wandlung die nächste AD-Wandlung über ANALOG_ON gestartet.

// if(state != 0) ANALOG_START; // MartinR verschoben zu timer0

}

/*
//#######################################################################################
//
SIGNAL(SIG_ADC)
//#######################################################################################
{
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;
switch(state++)
{
case 0:
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:
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:
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:
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:
Aktuell_ay = NeutralAccY - ADC;
accy += Aktuell_ay;
kanal = AD_NICK;
break;
case 15:
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:
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:
Aktuell_ay = NeutralAccY - ADC;
accy += Aktuell_ay;
kanal = AD_NICK;
break;
case 22:
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:
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:
nick1 += ADC;
kanal = AD_ROLL;
break;
case 30:
roll1 += ADC;
kanal = AD_ACC_Y;
break;
case 31:
Aktuell_ay = NeutralAccY - ADC;
AdWertAccRoll = (Aktuell_ay + accy);
kanal = AD_NICK;
break;
case 32:
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;
}
*/

Subversion Repositories FlightCtrl

(root)/branches/V0.74d_ACC-HH_MartinR/analog.c @ 2147 - Rev 1286