Rev 1266 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 VarioMeter
= 0;
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;}
AdReady
= 0;
ANALOG_ON
;
while(!AdReady
);
if(i
<10) Delay_ms_Mess
(10);
}
Delay_ms_Mess
(70);
}
/*
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
*/
//#######################################################################################
//
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
;
static long tmpLuftdruck
= 0;
static char messanzahl_Druck
= 0;
static long SummenHoehe
= 0;
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:
MessLuftdruck
= ADC
;
tmpLuftdruck
+= MessLuftdruck
;
#define SM_FILTER 16
if(++messanzahl_Druck
>= 18)
{
Luftdruck
= (7 * Luftdruck
+ tmpLuftdruck
- (18 * 523) * (long)ExpandBaro
+ 4) / 8; // -523.19 counts per 10 counts offset step
HoehenWert
= StartLuftdruck
- Luftdruck
;
SummenHoehe
-= SummenHoehe
/SM_FILTER
;
SummenHoehe
+= HoehenWert
;
VarioMeter
= (15 * VarioMeter
+ 8 * (int)(HoehenWert
- SummenHoehe
/SM_FILTER
))/16;
tmpLuftdruck
/= 2;
messanzahl_Druck
= 18/2;
}
kanal
= AD_NICK
;
break;
default:
kanal
= 0; state
= 0; kanal
= AD_NICK
;
break;
}
ADMUX
= kanal
;
if(state
!= 0) ANALOG_ON
;
}
/*
//#######################################################################################
//
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;
}
*/