WebSVN - FlightCtrl - Diff - Rev 1021 and 1036


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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;
//#######################################################################################
//
void ADC_Init(void)
//#######################################################################################
{
    ADMUX = 0;//Referenz ist extern
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
    //Free Running Mode, Division Factor 128, Interrupt 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;
    ANALOG_OFF;
    switch(state++)
        {
        case 0:
            gier1 = ADC;
            kanal = 1;
            ZaehlMessungen++;
            break;
        case 1:
            roll1 = ADC;
            kanal = 2;
            break;
        case 2:
            nick1 = ADC;
            kanal = 4;
            break;
        case 3:
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
            kanal = 6;
            break;
        case 4:
            Aktuell_ay = NeutralAccY - ADC;
            AdWertAccRoll = Aktuell_ay;
            kanal = 7;
            break;
        case 5:
            Aktuell_ax = ADC - NeutralAccX;
            AdWertAccNick =  Aktuell_ax;
                    kanal = 0;
            break;
        case 6:
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1) / 2;
            else
            if(PlatinenVersion == 20)  AdWertGier = 1023 - (ADC + gier1);
                        else                                       AdWertGier = ADC + gier1;
            kanal = 1;
            break;
        case 7:
            if(PlatinenVersion == 10)  AdWertRoll = (ADC + roll1) / 2;
                        else                                       AdWertRoll = ADC + roll1;
            kanal = 2;
            break;
        case 8:
            if(PlatinenVersion == 10)  AdWertNick = (ADC + nick1) / 2;
                        else                                       AdWertNick = ADC + nick1;
//AdWertNick = 0;
//AdWertNick += Poti2;            
            kanal = 5;
            break;
       case 9:
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
//            AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
            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 = 3;
            break;
        case 10:
            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 = 0;
            state = 0;
            break;
        default:
            kanal = 0;
            state = 0;
            break;
        }
    ADMUX = kanal;
    if(state != 0) ANALOG_ON;
}
 

Rev 1021	Rev 1036
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + only for non-profit use	3	// + only for non-profit use
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// + see the File "License.txt" for further Informations	5	// + see the File "License.txt" for further Informations
6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7		7
8	#include "main.h"	8	#include "main.h"
9		9
10	volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;	10	volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
11	volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;	11	volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
12	volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;	12	volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
13	volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;	13	volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;
14	volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;	14	volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;
15	volatile long Luftdruck = 32000;	15	volatile long Luftdruck = 32000;
16	volatile int StartLuftdruck;	16	volatile int StartLuftdruck;
17	volatile unsigned int MessLuftdruck = 1023;	17	volatile unsigned int MessLuftdruck = 1023;
18	unsigned char DruckOffsetSetting;	18	unsigned char DruckOffsetSetting;
-		19	signed char ExpandBaro = 0;
19	volatile int HoeheD = 0;	20	volatile int HoeheD = 0;
20	volatile char messanzahl_Druck;	21	volatile char messanzahl_Druck;
21	volatile int tmpLuftdruck;	22	volatile int tmpLuftdruck;
22	volatile unsigned int ZaehlMessungen = 0;	23	volatile unsigned int ZaehlMessungen = 0;
23	unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;	24	unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
24	unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;	25	unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
25	//#######################################################################################	26	//#######################################################################################
26	//	27	//
27	void ADC_Init(void)	28	void ADC_Init(void)
28	//#######################################################################################	29	//#######################################################################################
29	{	30	{
30	ADMUX = 0;//Referenz ist extern	31	ADMUX = 0;//Referenz ist extern
31	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);	32	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);
32	//Free Running Mode, Division Factor 128, Interrupt on	33	//Free Running Mode, Division Factor 128, Interrupt on
33	}	34	}
34		35
35	void SucheLuftruckOffset(void)	36	void SucheLuftruckOffset(void)
36	{	37	{
37	unsigned int off;	38	unsigned int off;
38	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);	39	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
39	if(off > 20) off -= 10;	40	if(off > 20) off -= 10;
40	OCR0A = off;	41	OCR0A = off;
-		42	ExpandBaro = 0;
41	Delay_ms_Mess(100);	43	Delay_ms_Mess(100);
42	if(MessLuftdruck < 850) off = 0;	44	if(MessLuftdruck < 850) off = 0;
43	for(; off < 250;off++)	45	for(; off < 250;off++)
44	{	46	{
45	OCR0A = off;	47	OCR0A = off;
46	Delay_ms_Mess(50);	48	Delay_ms_Mess(50);
47	printf(".");	49	printf(".");
48	if(MessLuftdruck < 900) break;	50	if(MessLuftdruck < 850) break;
49	}	51	}
50	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);	52	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
51	DruckOffsetSetting = off;	53	DruckOffsetSetting = off;
52	Delay_ms_Mess(300);	54	Delay_ms_Mess(300);
53	}	55	}
54		56
55	void SucheGyroOffset(void)	57	void SucheGyroOffset(void)
56	{	58	{
57	unsigned char i, ready = 0;	59	unsigned char i, ready = 0;
58	GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;	60	GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
59	for(i=140; i != 0; i--)	61	for(i=140; i != 0; i--)
60	{	62	{
61	if(ready == 3 && i > 10) i = 9;	63	if(ready == 3 && i > 10) i = 9;
62	ready = 0;	64	ready = 0;
63	if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;	65	if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
64	if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;	66	if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
65	if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;	67	if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
66	twi_state = 8;	68	twi_state = 8;
67	i2c_start();	69	i2c_start();
68	if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};	70	if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
69	if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};	71	if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
70	if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};	72	if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
71	while(twi_state);	73	while(twi_state);
72	messanzahl_Druck = 0;	74	messanzahl_Druck = 0;
73	ANALOG_ON;	75	ANALOG_ON;
74	while(messanzahl_Druck == 0);	76	while(messanzahl_Druck == 0);
75	if(i<10) Delay_ms_Mess(10);	77	if(i<10) Delay_ms_Mess(10);
76	}	78	}
77	Delay_ms_Mess(70);	79	Delay_ms_Mess(70);
78	}	80	}
79		81
80		82
81	//#######################################################################################	83	//#######################################################################################
82	//	84	//
83	SIGNAL(SIG_ADC)	85	SIGNAL(SIG_ADC)
84	//#######################################################################################	86	//#######################################################################################
85	{	87	{
86	static unsigned char kanal=0,state = 0;	88	static unsigned char kanal=0,state = 0;
87	static unsigned int gier1, roll1, nick1;	89	static unsigned int gier1, roll1, nick1;
88	ANALOG_OFF;	90	ANALOG_OFF;
89	switch(state++)	91	switch(state++)
90	{	92	{
91	case 0:	93	case 0:
92	gier1 = ADC;	94	gier1 = ADC;
93	kanal = 1;	95	kanal = 1;
94	ZaehlMessungen++;	96	ZaehlMessungen++;
95	break;	97	break;
96	case 1:	98	case 1:
97	roll1 = ADC;	99	roll1 = ADC;
98	kanal = 2;	100	kanal = 2;
99	break;	101	break;
100	case 2:	102	case 2:
101	nick1 = ADC;	103	nick1 = ADC;
102	kanal = 4;	104	kanal = 4;
103	break;	105	break;
104	case 3:	106	case 3:
105	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;	107	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
106	kanal = 6;	108	kanal = 6;
107	break;	109	break;
108	case 4:	110	case 4:
109	Aktuell_ay = NeutralAccY - ADC;	111	Aktuell_ay = NeutralAccY - ADC;
110	AdWertAccRoll = Aktuell_ay;	112	AdWertAccRoll = Aktuell_ay;
111	kanal = 7;	113	kanal = 7;
112	break;	114	break;
113	case 5:	115	case 5:
114	Aktuell_ax = ADC - NeutralAccX;	116	Aktuell_ax = ADC - NeutralAccX;
115	AdWertAccNick = Aktuell_ax;	117	AdWertAccNick = Aktuell_ax;
116	kanal = 0;	118	kanal = 0;
117	break;	119	break;
118	case 6:	120	case 6:
119	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1) / 2;	121	if(PlatinenVersion == 10) AdWertGier = (ADC + gier1) / 2;
120	else	122	else
121	if(PlatinenVersion == 20) AdWertGier = 1023 - (ADC + gier1);	123	if(PlatinenVersion == 20) AdWertGier = 1023 - (ADC + gier1);
122	else AdWertGier = ADC + gier1;	124	else AdWertGier = ADC + gier1;
123	kanal = 1;	125	kanal = 1;
124	break;	126	break;
125	case 7:	127	case 7:
126	if(PlatinenVersion == 10) AdWertRoll = (ADC + roll1) / 2;	128	if(PlatinenVersion == 10) AdWertRoll = (ADC + roll1) / 2;
127	else AdWertRoll = ADC + roll1;	129	else AdWertRoll = ADC + roll1;
128	kanal = 2;	130	kanal = 2;
129	break;	131	break;
130	case 8:	132	case 8:
131	if(PlatinenVersion == 10) AdWertNick = (ADC + nick1) / 2;	133	if(PlatinenVersion == 10) AdWertNick = (ADC + nick1) / 2;
132	else AdWertNick = ADC + nick1;	134	else AdWertNick = ADC + nick1;
133	//AdWertNick = 0;	135	//AdWertNick = 0;
134	//AdWertNick += Poti2;	136	//AdWertNick += Poti2;
135	kanal = 5;	137	kanal = 5;
136	break;	138	break;
137	case 9:	139	case 9:
138	AdWertAccHoch = (signed int) ADC - NeutralAccZ;	140	AdWertAccHoch = (signed int) ADC - NeutralAccZ;
139	// AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;	141	// AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
140	if(AdWertAccHoch > 1)	142	if(AdWertAccHoch > 1)
141	{	143	{
142	if(NeutralAccZ < 750)	144	if(NeutralAccZ < 750)
143	{	145	{
144	NeutralAccZ += 0.02;	146	NeutralAccZ += 0.02;
145	if(modell_fliegt < 500) NeutralAccZ += 0.1;	147	if(modell_fliegt < 500) NeutralAccZ += 0.1;
146	}	148	}
147	}	149	}
148	else if(AdWertAccHoch < -1)	150	else if(AdWertAccHoch < -1)
149	{	151	{
150	if(NeutralAccZ > 550)	152	if(NeutralAccZ > 550)
151	{	153	{
152	NeutralAccZ-= 0.02;	154	NeutralAccZ-= 0.02;
153	if(modell_fliegt < 500) NeutralAccZ -= 0.1;	155	if(modell_fliegt < 500) NeutralAccZ -= 0.1;
154	}	156	}
155	}	157	}
156	messanzahl_AccHoch = 1;	158	messanzahl_AccHoch = 1;
157	Aktuell_az = ADC;	159	Aktuell_az = ADC;
158	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren	160	Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
159	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen	161	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
160	kanal = 3;	162	kanal = 3;
161	break;	163	break;
162	case 10:	164	case 10:
163	tmpLuftdruck += ADC;	165	tmpLuftdruck += ADC;
164	if(++messanzahl_Druck >= 5)	166	if(++messanzahl_Druck >= 5)
165	{	167	{
166	MessLuftdruck = ADC;	168	MessLuftdruck = ADC;
167	messanzahl_Druck = 0;	169	messanzahl_Druck = 0;
168	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(StartLuftdruck - tmpLuftdruck - HoehenWert))/8; // D-Anteil = neuerWert - AlterWert	170	HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/8; // D-Anteil = neuerWert - AlterWert
169	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;	171	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
170	HoehenWert = StartLuftdruck - Luftdruck;	172	HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
171	tmpLuftdruck = 0;	173	tmpLuftdruck = 0;
172	}	174	}
173	kanal = 0;	175	kanal = 0;
174	state = 0;	176	state = 0;
175	break;	177	break;
176	default:	178	default:
177	kanal = 0;	179	kanal = 0;
178	state = 0;	180	state = 0;
179	break;	181	break;
180	}	182	}
181	ADMUX = kanal;	183	ADMUX = kanal;
182	if(state != 0) ANALOG_ON;	184	if(state != 0) ANALOG_ON;
183	}	185	}
184		186

Subversion Repositories FlightCtrl

(root)/tags/V0.80f/analog.c @ 2278 - Rev 1021 → 1036