Rev 1111 |
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 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 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;
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;
GyroDefektN
= 0; GyroDefektR
= 0; GyroDefektG
= 0;
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
);
messanzahl_Druck
= 0;
ANALOG_ON
;
while(messanzahl_Druck
== 0);
if(i
<10) Delay_ms_Mess
(10);
}
Delay_ms_Mess
(70);
}
//#######################################################################################
//
SIGNAL
(SIG_ADC
)
//#######################################################################################
{
static unsigned char kanal
=0,state
= 0;
static unsigned int gier1
, roll1
, nick1
, accy
, accx
;
ANALOG_OFF
;
switch(state
++)
{
case 0:
gier1
= ADC
;
kanal
= AD_ROLL
;
ZaehlMessungen
++;
break;
case 1:
roll1
= ADC
;
kanal
= AD_NICK
;
break;
case 2:
nick1
= 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_GIER
;
break;
case 5:
gier1
+= ADC
;
kanal
= AD_ROLL
;
break;
case 6:
roll1
+= ADC
;
kanal
= AD_NICK
;
break;
case 7:
nick1
+= ADC
;
kanal
= AD_UBAT
;
break;
case 8:
UBat
= (3 * UBat
+ ADC
/ 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
kanal
= AD_ACC_Z
;
break;
case 9:
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_GIER
;
break;
case 10:
gier1
+= ADC
;
kanal
= AD_ROLL
;
break;
case 11:
roll1
+= ADC
;
kanal
= AD_NICK
;
break;
case 12:
nick1
+= ADC
;
kanal
= AD_DRUCK
;
break;
case 13:
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_ACC_Y
;
break;
case 14:
Aktuell_ay
= NeutralAccY
- ADC
;
AdWertAccRoll
= (Aktuell_ay
+ accy
);
kanal
= AD_ACC_X
;
break;
case 15:
Aktuell_ax
= ADC
- NeutralAccX
;
AdWertAccNick
= (Aktuell_ax
+ accx
);
kanal
= AD_GIER
;
break;
case 16:
if(PlatinenVersion
== 10) AdWertGier
= (ADC
+ gier1
) / 4;
else
if(PlatinenVersion
== 20) AdWertGier
= 2047 - (ADC
+ gier1
)/2;
else AdWertGier
= (ADC
+ gier1
) / 2;
kanal
= AD_ROLL
;
break;
case 17:
if(PlatinenVersion
== 10) AdWertRoll
= (ADC
+ roll1
) / 4;
else AdWertRoll
= (ADC
+ roll1
) / 2;
kanal
= AD_NICK
;
break;
case 18:
if(PlatinenVersion
== 10) AdWertNick
= (ADC
+ nick1
) / 4;
else AdWertNick
= (ADC
+ nick1
) / 2;
kanal
= AD_GIER
;
state
= 0;
AdReady
= 1;
break;
default:
kanal
= 0;
state
= 0;
break;
}
ADMUX
= kanal
;
if(state
!= 0) ANALOG_ON
;
}