WebSVN - FlightCtrl - Diff - Rev 966 and 973 - /branches/KalmanFilter MikeW/analog.c


/*
Copyright 2008, by Michael Walter
 
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/
 
#include "main.h"
#include "FlightControl.h"
 
 
int UBat = 100; /* Initial Battery Guess */
 
 
int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;
 
int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
int AccumulatedRoll = 0, AccumulatedNick = 0, AccumulatedGier = 0;
int AccumulatedRoll_cnt = 0, AccumulatedNick_cnt = 0, AccumulatedGier_cnt = 0;
 
unsigned int AdWertAirPressure_Raw = 1023;
 
/* ****************************************************************************
Functionname:     ADC_Init                      */ /*!
Description:
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void ADC_Init(void)
{
#ifdef INTERNAL_REFERENCE 
  ADMUX = 64;/* Internal Reference 5V */
#else
  ADMUX = 0; /* External Reference */
#endif
  ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
}
 
/* ****************************************************************************
Functionname:     SucheLuftruckOffset                      */ /*!
Description:
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void SucheLuftruckOffset(void)
{
  unsigned int off;
  off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
  if(off > 20) off -= 10;
  OCR0A = off;
  Delay_ms_Mess(100);
  if(AdWertAirPressure_Raw < 850) off = 0;
  for(; off < 250;off++)
  {
    OCR0A = off;
    Delay_ms_Mess(50);
    printf(".");  
    if(AdWertAirPressure_Raw < 900) break;
  }
  eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
  Delay_ms_Mess(300);
}
 
/* ****************************************************************************
Functionname:     SIGNAL                      */ /*!
Description:
 
  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
SIGNAL(SIG_ADC)
{
  static unsigned char kanal=0,state = 0;
  ANALOG_OFF;
  switch(state++)
  {
  case 0:
 
    AdWertGier_Raw = ADC;
    AccumulatedGier += (ADC - AdNeutralGier);
    AccumulatedGier_cnt++;
    kanal = 1;
    break;     
  case 1:
 
    AdWertRoll_Raw = ADC;
    AccumulatedRoll += (ADC - AdNeutralRoll);
    AccumulatedRoll_cnt++;
    kanal = 2;
    break;             
  case 2:
 
    AdWertNick_Raw = ADC;
    AccumulatedNick += (ADC - AdNeutralNick);
    AccumulatedNick_cnt++;
    kanal = 4;
    break;                     
  case 3:
#ifdef INTERNAL_REFERENCE 
    UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */
#else
    UBat = (3 * UBat + ADC / 3) / 4;
#endif
    kanal = 6;
    break;
  case 4:
    AdWertAccRoll = NeutralAccY - ADC;
    AdWertAccRoll_Raw = ADC;
    AccumulatedACC_Y += (NeutralAccY - ADC);
    AccumulatedACC_Y_cnt++;
    kanal = 7;
    break;
  case 5:
    AdWertAccNick = ADC - NeutralAccX;
    AdWertAccNick_Raw = ADC;
    AccumulatedACC_X += (ADC - NeutralAccX);
    AccumulatedACC_X_cnt++;
    kanal = 5;
    break;
  case 6:      
    AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
    AdWertAccHoch_Raw = ADC;
    AccumulatedACC_Z += (ADC - NeutralAccZ);
    AccumulatedACC_Z_cnt++;
    kanal = 3;
    break;
  case 7:
    AdWertAirPressure_Raw = ADC;
    AccumulatedAirPressure += ADC;
    AccumulatedAirPressure_cnt++;
    kanal = 0;
    state = 0;
    break;
  default:
    kanal = 0;
    state = 0;
    break;
  }
  ADMUX = kanal;
#ifdef INTERNAL_REFERENCE 
  /* Add 64 in order to use the internal 5v refenrence */
  ADMUX += 64;
#endif
  ANALOG_ON;
}
 

Rev 966	Rev 973
1	/*	1	/*
2	Copyright 2008, by Michael Walter	2	Copyright 2008, by Michael Walter
3		3
4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser	4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but	5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public	7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
8	License along with this program. If not, see <http://www.gnu.org/licenses/>.	8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
9		9
10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that	10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de	11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12	unless it is stated otherwise.	12	unless it is stated otherwise.
13	*/	13	*/
14		14
15	#include "main.h"	15	#include "main.h"
16	#include "FlightControl.h"	16	#include "FlightControl.h"
17		17
18		18
19	int UBat = 100; /* Initial Battery Guess */	19	int UBat = 100; /* Initial Battery Guess */
20	int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;	-
21	int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;	-
22	int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;	20	int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
-		21	int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
23	int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;	22	int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;
24		23
25	int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;	24	int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
26	int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;	25	int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
27	int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0;	26	int AccumulatedRoll = 0, AccumulatedNick = 0, AccumulatedGier = 0;
28	int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0;	27	int AccumulatedRoll_cnt = 0, AccumulatedNick_cnt = 0, AccumulatedGier_cnt = 0;
29		28
30	unsigned int AdWertAirPressure_Raw = 1023;	29	unsigned int AdWertAirPressure_Raw = 1023;
31		30
32	/* ****************************************************************************	31	/* ****************************************************************************
33	Functionname: ADC_Init / /!	32	Functionname: ADC_Init / /!
34	Description:	33	Description:
35		34
36	@return void	35	@return void
37	@pre -	36	@pre -
38	@post -	37	@post -
39	@author H. Buss / I. Busker	38	@author H. Buss / I. Busker
40	**************************************************************************** */	39	**************************************************************************** */
41	void ADC_Init(void)	40	void ADC_Init(void)
42	{	41	{
43	#ifdef INTERNAL_REFERENCE	42	#ifdef INTERNAL_REFERENCE
44	ADMUX = 64;/* Internal Reference 5V */	43	ADMUX = 64;/* Internal Reference 5V */
45	#else	44	#else
46	ADMUX = 0; /* External Reference */	45	ADMUX = 0; /* External Reference */
47	#endif	46	#endif
48	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);	47	ADCSRA=(1<<ADEN)\|(1<<ADSC)\|(1<<ADATE)\|(1<<ADPS2)\|(1<<ADPS1)\|(1<<ADPS0)\|(1<<ADIE);
49	}	48	}
50		49
51	/* ****************************************************************************	50	/* ****************************************************************************
52	Functionname: SucheLuftruckOffset / /!	51	Functionname: SucheLuftruckOffset / /!
53	Description:	52	Description:
54		53
55	@return void	54	@return void
56	@pre -	55	@pre -
57	@post -	56	@post -
58	@author H. Buss / I. Busker	57	@author H. Buss / I. Busker
59	**************************************************************************** */	58	**************************************************************************** */
60	void SucheLuftruckOffset(void)	59	void SucheLuftruckOffset(void)
61	{	60	{
62	unsigned int off;	61	unsigned int off;
63	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);	62	off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
64	if(off > 20) off -= 10;	63	if(off > 20) off -= 10;
65	OCR0A = off;	64	OCR0A = off;
66	Delay_ms_Mess(100);	65	Delay_ms_Mess(100);
67	if(AdWertAirPressure_Raw < 850) off = 0;	66	if(AdWertAirPressure_Raw < 850) off = 0;
68	for(; off < 250;off++)	67	for(; off < 250;off++)
69	{	68	{
70	OCR0A = off;	69	OCR0A = off;
71	Delay_ms_Mess(50);	70	Delay_ms_Mess(50);
72	printf(".");	71	printf(".");
73	if(AdWertAirPressure_Raw < 900) break;	72	if(AdWertAirPressure_Raw < 900) break;
74	}	73	}
75	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);	74	eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
76	Delay_ms_Mess(300);	75	Delay_ms_Mess(300);
77	}	76	}
78		77
79	/* ****************************************************************************	78	/* ****************************************************************************
80	Functionname: SIGNAL / /!	79	Functionname: SIGNAL / /!
81	Description:	80	Description:
82		81
83	@return void	82	@return void
84	@pre -	83	@pre -
85	@post -	84	@post -
86	@author Michael Walter	85	@author Michael Walter
87	**************************************************************************** */	86	**************************************************************************** */
88	SIGNAL(SIG_ADC)	87	SIGNAL(SIG_ADC)
89	{	88	{
90	static unsigned char kanal=0,state = 0;	89	static unsigned char kanal=0,state = 0;
91	ANALOG_OFF;	90	ANALOG_OFF;
92	switch(state++)	91	switch(state++)
93	{	92	{
94	case 0:	93	case 0:
95	AdWertGier = ADC;	-
96	AdWertGier_Raw = ADC;	94	AdWertGier_Raw = ADC;
97	AccumulatedRoll_Z += (ADC - AdNeutralGier);	95	AccumulatedGier += (ADC - AdNeutralGier);
98	AccumulatedRoll_Z_cnt++;	96	AccumulatedGier_cnt++;
99	kanal = 1;	97	kanal = 1;
100	break;	98	break;
101	case 1:	99	case 1:
102	AdWertRoll = ADC;	-
103	AdWertRoll_Raw = ADC;	100	AdWertRoll_Raw = ADC;
104	AccumulatedRoll_X += (ADC - AdNeutralRoll);	101	AccumulatedRoll += (ADC - AdNeutralRoll);
105	AccumulatedRoll_X_cnt++;	102	AccumulatedRoll_cnt++;
106	kanal = 2;	103	kanal = 2;
107	break;	104	break;
108	case 2:	105	case 2:
109	AdWertNick = ADC;	-
110	AdWertNick_Raw = ADC;	106	AdWertNick_Raw = ADC;
111	AccumulatedRoll_Y += (ADC - AdNeutralNick);	107	AccumulatedNick += (ADC - AdNeutralNick);
112	AccumulatedRoll_Y_cnt++;	108	AccumulatedNick_cnt++;
113	kanal = 4;	109	kanal = 4;
114	break;	110	break;
115	case 3:	111	case 3:
116	#ifdef INTERNAL_REFERENCE	112	#ifdef INTERNAL_REFERENCE
117	UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */	113	UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */
118	#else	114	#else
119	UBat = (3 * UBat + ADC / 3) / 4;	115	UBat = (3 * UBat + ADC / 3) / 4;
120	#endif	116	#endif
121	kanal = 6;	117	kanal = 6;
122	break;	118	break;
123	case 4:	119	case 4:
124	AdWertAccRoll = NeutralAccY - ADC;	120	AdWertAccRoll = NeutralAccY - ADC;
125	AdWertAccRoll_Raw = ADC;	121	AdWertAccRoll_Raw = ADC;
126	AccumulatedACC_Y += (NeutralAccY - ADC);	122	AccumulatedACC_Y += (NeutralAccY - ADC);
127	AccumulatedACC_Y_cnt++;	123	AccumulatedACC_Y_cnt++;
128	kanal = 7;	124	kanal = 7;
129	break;	125	break;
130	case 5:	126	case 5:
131	AdWertAccNick = ADC - NeutralAccX;	127	AdWertAccNick = ADC - NeutralAccX;
132	AdWertAccNick_Raw = ADC;	128	AdWertAccNick_Raw = ADC;
133	AccumulatedACC_X += (ADC - NeutralAccX);	129	AccumulatedACC_X += (ADC - NeutralAccX);
134	AccumulatedACC_X_cnt++;	130	AccumulatedACC_X_cnt++;
135	kanal = 5;	131	kanal = 5;
136	break;	132	break;
137	case 6:	133	case 6:
138	AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);	134	AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
139	AdWertAccHoch_Raw = ADC;	135	AdWertAccHoch_Raw = ADC;
140	AccumulatedACC_Z += (ADC - NeutralAccZ);	136	AccumulatedACC_Z += (ADC - NeutralAccZ);
141	AccumulatedACC_Z_cnt++;	137	AccumulatedACC_Z_cnt++;
142	kanal = 3;	138	kanal = 3;
143	break;	139	break;
144	case 7:	140	case 7:
145	AdWertAirPressure_Raw = ADC;	141	AdWertAirPressure_Raw = ADC;
146	AccumulatedAirPressure += ADC;	142	AccumulatedAirPressure += ADC;
147	AccumulatedAirPressure_cnt++;	143	AccumulatedAirPressure_cnt++;
148	kanal = 0;	144	kanal = 0;
149	state = 0;	145	state = 0;
150	break;	146	break;
151	default:	147	default:
152	kanal = 0;	148	kanal = 0;
153	state = 0;	149	state = 0;
154	break;	150	break;
155	}	151	}
156	ADMUX = kanal;	152	ADMUX = kanal;
157	#ifdef INTERNAL_REFERENCE	153	#ifdef INTERNAL_REFERENCE
158	/* Add 64 in order to use the internal 5v refenrence */	154	/* Add 64 in order to use the internal 5v refenrence */
159	ADMUX += 64;	155	ADMUX += 64;
160	#endif	156	#endif
161	ANALOG_ON;	157	ANALOG_ON;
162	}	158	}
163		159

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/analog.c @ 2272 - Rev 966 → 973