WebSVN - FlightCtrl - Diff - Rev 174 and 1340


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + 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  AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0;
volatile int  accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 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;
volatile int HoeheD = 0;
volatile char messanzahl_Druck;
volatile int  tmpLuftdruck;
volatile unsigned int ZaehlMessungen = 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;
 Delay_ms(100);
 if(MessLuftdruck < 850) off = 0;
 for(; off < 250;off++)
  {
  OCR0A = off;
  Delay_ms(50);
  printf(".");  
  if(MessLuftdruck < 900) break;
  }
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
 DruckOffsetSetting = off;
 Delay_ms(300);
}
 
 
//#######################################################################################
//
SIGNAL(SIG_ADC)
//#######################################################################################
{
    static unsigned char kanal=0,state = 0;
    signed int wert;   
    ANALOG_OFF;
    switch(state++)
        {
        case 0:
            wert = (signed int) AdNeutralGier - ADC;
            if(PlatinenVersion != 10) wert *= 2;
            AccumulateGier += wert;     //
            MessanzahlGier++;
            Mess_Integral_Gier += wert;// / 16;
            Mess_Integral_Gier2 += wert;
            kanal = 1;
            ZaehlMessungen++;
            break;
        case 1:
            wert = (signed int) ADC - AdNeutralRoll;
            if(PlatinenVersion != 10) wert *= 2;
            Mess_IntegralRoll += wert;
            Mess_IntegralRoll2 += wert;
            if(Mess_IntegralRoll > 310000L) Mess_IntegralRoll = -290000L;
            if(Mess_IntegralRoll <-310000L) Mess_IntegralRoll = 290000L;
            if(ADC < 15)   wert = -1000;
            if(ADC <  7)   wert = -2000;
            if(ADC > 1010) wert = +1000;
            if(ADC > 1017) wert = +2000;
            AccumulateRoll += wert;
            MessanzahlRoll++;
            kanal = 2;
            break;
        case 2:
            wert = (signed int) ADC - AdNeutralNick;
            if(PlatinenVersion != 10) wert *= 2;
            Mess_IntegralNick += wert;
            Mess_IntegralNick2 += wert;
            if(Mess_IntegralNick > 310000L) Mess_IntegralNick = -290000L;
            if(Mess_IntegralNick <-310000L) Mess_IntegralNick = 290000L;
            if(ADC < 15)   wert = -1000;
            if(ADC <  7)   wert = -2000;
            if(ADC > 1010) wert = +1000;
            if(ADC > 1017) wert = +2000;
            AccumulateNick += wert;
            MessanzahlNick++;
            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;
            accumulate_AccRoll += Aktuell_ay;
            messanzahl_AccRoll++;
            kanal = 7;
            break;
        case 5:
            Aktuell_ax = ADC - NeutralAccX;
            accumulate_AccNick +=  Aktuell_ax;
            messanzahl_AccNick++;
                    kanal = 5;
                    state = 6;
            break;
        case 6:
            accumulate_AccHoch =  (signed int) ADC - NeutralAccZ;
            accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
            if(accumulate_AccHoch > 1)
             {
              if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
             }  
             else if(accumulate_AccHoch < -1)
             {
              if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
             }
            messanzahl_AccHoch = 1;
            Aktuell_az = ADC;
            Mess_Integral_Hoch += accumulate_AccHoch;      // Integrieren
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
//            Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen
 /*           if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
             {
                      kanal = 3;
                      state = 7;
             }
            else  
             {
                      kanal = 0;
                      state = 0;
             }*/
                      kanal = 3;
                      state = 7;
            break;
        case 7:
            tmpLuftdruck += ADC;
            if(++messanzahl_Druck >= 5)
                {
                MessLuftdruck = ADC;
                messanzahl_Druck = 0;
                                HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert);  // D-Anteil = neuerWert - AlterWert
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
                HoehenWert = StartLuftdruck - Luftdruck;
                tmpLuftdruck = 0;
                }
            kanal = 0;
            state = 0;
            break;
        default:
            kanal = 0;
            state = 0;
            break;
        }
    ADMUX = kanal;
    ANALOG_ON;
}
 

Rev 174	Rev 1340
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) Holger Buss, Ingo Busker
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 AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0;	11	volatile int AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0;
12	volatile int accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 0;	12	volatile int accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 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	volatile int HoeheD = 0;	19	volatile int HoeheD = 0;
20	volatile char messanzahl_Druck;	20	volatile char messanzahl_Druck;
21	volatile int tmpLuftdruck;	21	volatile int tmpLuftdruck;
22	volatile unsigned int ZaehlMessungen = 0;	22	volatile unsigned int ZaehlMessungen = 0;
23		23
24	//#######################################################################################	24	//#######################################################################################
25	//	25	//
26	void ADC_Init(void)	26	void ADC_Init(void)
27	//#######################################################################################	27	//#######################################################################################
28	{	28	{
29	ADMUX = 0;//Referenz ist extern	29	ADMUX = 0;//Referenz ist extern
30	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);	30	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);
31	//Free Running Mode, Division Factor 128, Interrupt on	31	//Free Running Mode, Division Factor 128, Interrupt on
32	}	32	}
33		33
34	void SucheLuftruckOffset(void)	34	void SucheLuftruckOffset(void)
35	{	35	{
36	unsigned int off;	36	unsigned int off;
37	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);	37	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
38	if(off > 20) off -= 10;	38	if(off > 20) off -= 10;
39	OCR0A = off;	39	OCR0A = off;
40	Delay_ms(100);	40	Delay_ms(100);
41	if(MessLuftdruck < 850) off = 0;	41	if(MessLuftdruck < 850) off = 0;
42	for(; off < 250;off++)	42	for(; off < 250;off++)
43	{	43	{
44	OCR0A = off;	44	OCR0A = off;
45	Delay_ms(50);	45	Delay_ms(50);
46	printf(".");	46	printf(".");
47	if(MessLuftdruck < 900) break;	47	if(MessLuftdruck < 900) break;
48	}	48	}
49	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);	49	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
50	DruckOffsetSetting = off;	50	DruckOffsetSetting = off;
51	Delay_ms(300);	51	Delay_ms(300);
52	}	52	}
53		53
54		54
55	//#######################################################################################	55	//#######################################################################################
56	//	56	//
57	SIGNAL(SIG_ADC)	57	SIGNAL(SIG_ADC)
58	//#######################################################################################	58	//#######################################################################################
59	{	59	{
60	static unsigned char kanal=0,state = 0;	60	static unsigned char kanal=0,state = 0;
61	signed int wert;	61	signed int wert;
62	ANALOG_OFF;	62	ANALOG_OFF;
63	switch(state++)	63	switch(state++)
64	{	64	{
65	case 0:	65	case 0:
66	wert = (signed int) AdNeutralGier - ADC;	66	wert = (signed int) AdNeutralGier - ADC;
67	if(PlatinenVersion != 10) wert *= 2;	67	if(PlatinenVersion != 10) wert *= 2;
68	AccumulateGier += wert; //	68	AccumulateGier += wert; //
69	MessanzahlGier++;	69	MessanzahlGier++;
70	Mess_Integral_Gier += wert;// / 16;	70	Mess_Integral_Gier += wert;// / 16;
71	Mess_Integral_Gier2 += wert;	71	Mess_Integral_Gier2 += wert;
72	kanal = 1;	72	kanal = 1;
73	ZaehlMessungen++;	73	ZaehlMessungen++;
74	break;	74	break;
75	case 1:	75	case 1:
76	wert = (signed int) ADC - AdNeutralRoll;	76	wert = (signed int) ADC - AdNeutralRoll;
77	if(PlatinenVersion != 10) wert *= 2;	77	if(PlatinenVersion != 10) wert *= 2;
78	Mess_IntegralRoll += wert;	78	Mess_IntegralRoll += wert;
79	Mess_IntegralRoll2 += wert;	79	Mess_IntegralRoll2 += wert;
80	if(Mess_IntegralRoll > 310000L) Mess_IntegralRoll = -290000L;	80	if(Mess_IntegralRoll > 310000L) Mess_IntegralRoll = -290000L;
81	if(Mess_IntegralRoll <-310000L) Mess_IntegralRoll = 290000L;	81	if(Mess_IntegralRoll <-310000L) Mess_IntegralRoll = 290000L;
82	if(ADC < 15) wert = -1000;	82	if(ADC < 15) wert = -1000;
83	if(ADC < 7) wert = -2000;	83	if(ADC < 7) wert = -2000;
84	if(ADC > 1010) wert = +1000;	84	if(ADC > 1010) wert = +1000;
85	if(ADC > 1017) wert = +2000;	85	if(ADC > 1017) wert = +2000;
86	AccumulateRoll += wert;	86	AccumulateRoll += wert;
87	MessanzahlRoll++;	87	MessanzahlRoll++;
88	kanal = 2;	88	kanal = 2;
89	break;	89	break;
90	case 2:	90	case 2:
91	wert = (signed int) ADC - AdNeutralNick;	91	wert = (signed int) ADC - AdNeutralNick;
92	if(PlatinenVersion != 10) wert *= 2;	92	if(PlatinenVersion != 10) wert *= 2;
93	Mess_IntegralNick += wert;	93	Mess_IntegralNick += wert;
94	Mess_IntegralNick2 += wert;	94	Mess_IntegralNick2 += wert;
95	if(Mess_IntegralNick > 310000L) Mess_IntegralNick = -290000L;	95	if(Mess_IntegralNick > 310000L) Mess_IntegralNick = -290000L;
96	if(Mess_IntegralNick <-310000L) Mess_IntegralNick = 290000L;	96	if(Mess_IntegralNick <-310000L) Mess_IntegralNick = 290000L;
97	if(ADC < 15) wert = -1000;	97	if(ADC < 15) wert = -1000;
98	if(ADC < 7) wert = -2000;	98	if(ADC < 7) wert = -2000;
99	if(ADC > 1010) wert = +1000;	99	if(ADC > 1010) wert = +1000;
100	if(ADC > 1017) wert = +2000;	100	if(ADC > 1017) wert = +2000;
101	AccumulateNick += wert;	101	AccumulateNick += wert;
102	MessanzahlNick++;	102	MessanzahlNick++;
103	kanal = 4;	103	kanal = 4;
104	break;	104	break;
105	case 3:	105	case 3:
106	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;	106	UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2;
107	kanal = 6;	107	kanal = 6;
108	break;	108	break;
109	case 4:	109	case 4:
110	Aktuell_ay = NeutralAccY - ADC;	110	Aktuell_ay = NeutralAccY - ADC;
111	accumulate_AccRoll += Aktuell_ay;	111	accumulate_AccRoll += Aktuell_ay;
112	messanzahl_AccRoll++;	112	messanzahl_AccRoll++;
113	kanal = 7;	113	kanal = 7;
114	break;	114	break;
115	case 5:	115	case 5:
116	Aktuell_ax = ADC - NeutralAccX;	116	Aktuell_ax = ADC - NeutralAccX;
117	accumulate_AccNick += Aktuell_ax;	117	accumulate_AccNick += Aktuell_ax;
118	messanzahl_AccNick++;	118	messanzahl_AccNick++;
119	kanal = 5;	119	kanal = 5;
120	state = 6;	120	state = 6;
121	break;	121	break;
122	case 6:	122	case 6:
123	accumulate_AccHoch = (signed int) ADC - NeutralAccZ;	123	accumulate_AccHoch = (signed int) ADC - NeutralAccZ;
124	accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;	124	accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
125	if(accumulate_AccHoch > 1)	125	if(accumulate_AccHoch > 1)
126	{	126	{
127	if(NeutralAccZ < 800) NeutralAccZ+= 0.02;	127	if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
128	}	128	}
129	else if(accumulate_AccHoch < -1)	129	else if(accumulate_AccHoch < -1)
130	{	130	{
131	if(NeutralAccZ > 600) NeutralAccZ-= 0.02;	131	if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
132	}	132	}
133	messanzahl_AccHoch = 1;	133	messanzahl_AccHoch = 1;
134	Aktuell_az = ADC;	134	Aktuell_az = ADC;
135	Mess_Integral_Hoch += accumulate_AccHoch; // Integrieren	135	Mess_Integral_Hoch += accumulate_AccHoch; // Integrieren
136	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen	136	Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
137	// Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen	137	// Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen
138	/* if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)	138	/* if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
139	{	139	{
140	kanal = 3;	140	kanal = 3;
141	state = 7;	141	state = 7;
142	}	142	}
143	else	143	else
144	{	144	{
145	kanal = 0;	145	kanal = 0;
146	state = 0;	146	state = 0;
147	}*/	147	}*/
148	kanal = 3;	148	kanal = 3;
149	state = 7;	149	state = 7;
150	break;	150	break;
151	case 7:	151	case 7:
152	tmpLuftdruck += ADC;	152	tmpLuftdruck += ADC;
153	if(++messanzahl_Druck >= 5)	153	if(++messanzahl_Druck >= 5)
154	{	154	{
155	MessLuftdruck = ADC;	155	MessLuftdruck = ADC;
156	messanzahl_Druck = 0;	156	messanzahl_Druck = 0;
157	HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert); // D-Anteil = neuerWert - AlterWert	157	HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert); // D-Anteil = neuerWert - AlterWert
158	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;	158	Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
159	HoehenWert = StartLuftdruck - Luftdruck;	159	HoehenWert = StartLuftdruck - Luftdruck;
160	tmpLuftdruck = 0;	160	tmpLuftdruck = 0;
161	}	161	}
162	kanal = 0;	162	kanal = 0;
163	state = 0;	163	state = 0;
164	break;	164	break;
165	default:	165	default:
166	kanal = 0;	166	kanal = 0;
167	state = 0;	167	state = 0;
168	break;	168	break;
169	}	169	}
170	ADMUX = kanal;	170	ADMUX = kanal;
171	ANALOG_ON;	171	ANALOG_ON;
172	}	172	}
173		173

Subversion Repositories FlightCtrl

(root)/tags/V0.63/analog.c - Rev 174 → 1340