Rev 701 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 701 | Rev 703 | ||
---|---|---|---|
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 | #include <stdlib.h> |
7 | #include <stdlib.h> |
8 | #include <avr/io.h> |
8 | #include <avr/io.h> |
9 | #include <avr/interrupt.h> |
9 | #include <avr/interrupt.h> |
10 | 10 | ||
11 | #include "analog.h" |
11 | #include "analog.h" |
12 | #include "main.h" |
12 | #include "main.h" |
13 | #include "timer0.h" |
13 | #include "timer0.h" |
14 | #include "fc.h" |
14 | #include "fc.h" |
15 | #include "printf_P.h" |
15 | #include "printf_P.h" |
16 | #include "eeprom.h" |
16 | #include "eeprom.h" |
17 | 17 | ||
18 | volatile int16_t Current_Pitch = 0, Current_Roll = 0, Current_Yaw = 0; |
18 | volatile int16_t Current_Pitch = 0, Current_Roll = 0, Current_Yaw = 0; |
19 | volatile int16_t Current_AccX = 0, Current_AccY = 0, Current_AccZ = 0; |
19 | volatile int16_t Current_AccX = 0, Current_AccY = 0, Current_AccZ = 0; |
20 | volatile int16_t UBat = 100; |
20 | volatile int16_t UBat = 100; |
21 | volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
21 | volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
22 | volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0; |
22 | volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0; |
23 | volatile uint8_t messanzahl_AccHoch = 0; |
23 | volatile uint8_t messanzahl_AccHoch = 0; |
24 | volatile int32_t AirPressure = 32000; |
24 | volatile int32_t AirPressure = 32000; |
25 | volatile int16_t StartAirPressure; |
25 | volatile int16_t StartAirPressure; |
26 | volatile uint16_t ReadingAirPressure = 1023; |
26 | volatile uint16_t ReadingAirPressure = 1023; |
27 | uint8_t DruckOffsetSetting; |
27 | uint8_t DruckOffsetSetting; |
28 | volatile int16_t HoeheD = 0; |
28 | volatile int16_t HightD = 0; |
29 | volatile int16_t tmpAirPressure; |
29 | volatile int16_t tmpAirPressure; |
30 | volatile uint16_t ZaehlMessungen = 0; |
30 | volatile uint16_t ZaehlMessungen = 0; |
31 | 31 | ||
32 | /*****************************************************/ |
32 | /*****************************************************/ |
33 | /* Initialize Analog Digital Converter */ |
33 | /* Initialize Analog Digital Converter */ |
34 | /*****************************************************/ |
34 | /*****************************************************/ |
35 | void ADC_Init(void) |
35 | void ADC_Init(void) |
36 | { |
36 | { |
37 | uint8_t sreg = SREG; |
37 | uint8_t sreg = SREG; |
38 | // disable all interrupts before reconfiguration |
38 | // disable all interrupts before reconfiguration |
39 | cli(); |
39 | cli(); |
40 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
40 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
41 | DDRA = 0x00; |
41 | DDRA = 0x00; |
42 | PORTA = 0x00; |
42 | PORTA = 0x00; |
43 | // Digital Input Disable Register 0 |
43 | // Digital Input Disable Register 0 |
44 | // Disable digital input buffer for analog adc_channel pins |
44 | // Disable digital input buffer for analog adc_channel pins |
45 | DIDR0 = 0xFF; |
45 | DIDR0 = 0xFF; |
46 | // external reference, adjust data to the right |
46 | // external reference, adjust data to the right |
47 | ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR)); |
47 | ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR)); |
48 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
48 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
49 | ADMUX = (ADMUX & 0xE0) | 0x00; |
49 | ADMUX = (ADMUX & 0xE0) | 0x00; |
50 | //Set ADC Control and Status Register A |
50 | //Set ADC Control and Status Register A |
51 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
51 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
52 | ADCSRA = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
52 | ADCSRA = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
53 | //Set ADC Control and Status Register B |
53 | //Set ADC Control and Status Register B |
54 | //Trigger Source to Free Running Mode |
54 | //Trigger Source to Free Running Mode |
55 | ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
55 | ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
56 | // Enable AD conversion |
56 | // Enable AD conversion |
57 | ADC_Enable(); |
57 | ADC_Enable(); |
58 | // restore global interrupt flags |
58 | // restore global interrupt flags |
59 | SREG = sreg; |
59 | SREG = sreg; |
60 | } |
60 | } |
61 | 61 | ||
62 | void SearchAirPressureOffset(void) |
62 | void SearchAirPressureOffset(void) |
63 | { |
63 | { |
64 | uint8_t off; |
64 | uint8_t off; |
65 | off = GetParamByte(PID_LAST_OFFSET); |
65 | off = GetParamByte(PID_LAST_OFFSET); |
66 | if(off > 20) off -= 10; |
66 | if(off > 20) off -= 10; |
67 | OCR0A = off; |
67 | OCR0A = off; |
68 | Delay_ms_Mess(100); |
68 | Delay_ms_Mess(100); |
69 | if(ReadingAirPressure < 850) off = 0; |
69 | if(ReadingAirPressure < 850) off = 0; |
70 | for(; off < 250;off++) |
70 | for(; off < 250;off++) |
71 | { |
71 | { |
72 | OCR0A = off; |
72 | OCR0A = off; |
73 | Delay_ms_Mess(50); |
73 | Delay_ms_Mess(50); |
74 | printf("."); |
74 | printf("."); |
75 | if(ReadingAirPressure < 900) break; |
75 | if(ReadingAirPressure < 900) break; |
76 | } |
76 | } |
77 | SetParamByte(PID_LAST_OFFSET, off); |
77 | SetParamByte(PID_LAST_OFFSET, off); |
78 | DruckOffsetSetting = off; |
78 | DruckOffsetSetting = off; |
79 | Delay_ms_Mess(300); |
79 | Delay_ms_Mess(300); |
80 | } |
80 | } |
81 | 81 | ||
82 | 82 | ||
83 | /*****************************************************/ |
83 | /*****************************************************/ |
84 | /* Interrupt Service Routine for ADC */ |
84 | /* Interrupt Service Routine for ADC */ |
85 | /*****************************************************/ |
85 | /*****************************************************/ |
86 | ISR(ADC_vect) |
86 | ISR(ADC_vect) |
87 | { |
87 | { |
88 | static uint8_t adc_channel = 0, state = 0; |
88 | static uint8_t adc_channel = 0, state = 0; |
89 | static uint16_t yaw1, roll1, pitch1; |
89 | static uint16_t yaw1, roll1, pitch1; |
90 | static uint8_t messanzahl_Druck = 0; |
90 | static uint8_t average_pressure = 0; |
91 | // disable further AD conversion |
91 | // disable further AD conversion |
92 | ADC_Disable(); |
92 | ADC_Disable(); |
93 | // state machine |
93 | // state machine |
94 | switch(state++) |
94 | switch(state++) |
95 | { |
95 | { |
96 | case 0: |
96 | case 0: |
97 | yaw1 = ADC; // get Gyro Yaw Voltage 1st sample |
97 | yaw1 = ADC; // get Gyro Yaw Voltage 1st sample |
98 | adc_channel = 1; // set next channel to ADC1 = ROLL GYRO |
98 | adc_channel = 1; // set next channel to ADC1 = ROLL GYRO |
99 | ZaehlMessungen++; // increment total measurement counter |
99 | ZaehlMessungen++; // increment total measurement counter |
100 | break; |
100 | break; |
101 | case 1: |
101 | case 1: |
102 | roll1 = ADC; // get Gyro Roll Voltage 1st sample |
102 | roll1 = ADC; // get Gyro Roll Voltage 1st sample |
103 | adc_channel = 2; // set next channel to ADC2 = PITCH GYRO |
103 | adc_channel = 2; // set next channel to ADC2 = PITCH GYRO |
104 | break; |
104 | break; |
105 | case 2: |
105 | case 2: |
106 | pitch1 = ADC; // get Gyro Pitch Voltage 1st sample |
106 | pitch1 = ADC; // get Gyro Pitch Voltage 1st sample |
107 | adc_channel = 4; // set next channel to ADC4 = UBAT |
107 | adc_channel = 4; // set next channel to ADC4 = UBAT |
108 | break; |
108 | break; |
109 | case 3: |
109 | case 3: |
110 | // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3 |
110 | // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3 |
111 | UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value |
111 | UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value |
112 | adc_channel = 6; // set next channel to ADC6 = ACC_Y |
112 | adc_channel = 6; // set next channel to ADC6 = ACC_Y |
113 | break; |
113 | break; |
114 | case 4: |
114 | case 4: |
115 | Current_AccY = NeutralAccY - ADC; // get acceleration in Y direction |
115 | Current_AccY = NeutralAccY - ADC; // get acceleration in Y direction |
116 | AdValueAccRoll = Current_AccY; |
116 | AdValueAccRoll = Current_AccY; |
117 | adc_channel = 7; // set next channel to ADC7 = ACC_X |
117 | adc_channel = 7; // set next channel to ADC7 = ACC_X |
118 | break; |
118 | break; |
119 | case 5: |
119 | case 5: |
120 | Current_AccX = ADC - NeutralAccX; // get acceleration in X direction |
120 | Current_AccX = ADC - NeutralAccX; // get acceleration in X direction |
121 | AdValueAccPitch = Current_AccX; |
121 | AdValueAccPitch = Current_AccX; |
122 | adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
122 | adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
123 | break; |
123 | break; |
124 | case 6: |
124 | case 6: |
125 | // average over two samples to create current AdValueGyrYaw |
125 | // average over two samples to create current AdValueGyrYaw |
126 | if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
126 | if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
127 | else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
127 | else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
128 | adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
128 | adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
129 | break; |
129 | break; |
130 | case 7: |
130 | case 7: |
131 | // average over two samples to create current ADValueGyrRoll |
131 | // average over two samples to create current ADValueGyrRoll |
132 | if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2; |
132 | if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2; |
133 | else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
133 | else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
134 | adc_channel = 2; // set next channel to ADC2 = PITCH GYRO |
134 | adc_channel = 2; // set next channel to ADC2 = PITCH GYRO |
135 | break; |
135 | break; |
136 | case 8: |
136 | case 8: |
137 | // average over two samples to create current ADValuePitch |
137 | // average over two samples to create current ADValuePitch |
138 | if(BoardRelease == 10) AdValueGyrPitch = (ADC + pitch1) / 2; |
138 | if(BoardRelease == 10) AdValueGyrPitch = (ADC + pitch1) / 2; |
139 | else AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1 |
139 | else AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1 |
140 | adc_channel = 5; // set next channel to ADC5 = ACC_Z |
140 | adc_channel = 5; // set next channel to ADC5 = ACC_Z |
141 | break; |
141 | break; |
142 | case 9: |
142 | case 9: |
143 | // get z acceleration |
143 | // get z acceleration |
144 | AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction |
144 | AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction |
145 | AdValueAccTop += abs(Current_AccY) / 4 + abs(Current_AccX) / 4; |
145 | AdValueAccTop += abs(Current_AccY) / 4 + abs(Current_AccX) / 4; |
146 | if(AdValueAccTop > 1) |
146 | if(AdValueAccTop > 1) |
147 | { |
147 | { |
148 | if(NeutralAccZ < 800) NeutralAccZ+= 0.02; |
148 | if(NeutralAccZ < 800) NeutralAccZ+= 0.02; |
149 | } |
149 | } |
150 | else if(AdValueAccTop < -1) |
150 | else if(AdValueAccTop < -1) |
151 | { |
151 | { |
152 | if(NeutralAccZ > 600) NeutralAccZ-= 0.02; |
152 | if(NeutralAccZ > 600) NeutralAccZ-= 0.02; |
153 | } |
153 | } |
154 | messanzahl_AccHoch = 1; |
154 | messanzahl_AccHoch = 1; |
155 | Current_AccZ = ADC; |
155 | Current_AccZ = ADC; |
156 | Reading_Integral_Top += AdValueAccTop; // Integrieren |
156 | Reading_Integral_Top += AdValueAccTop; // Integrieren |
157 | Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen |
157 | Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen |
158 | adc_channel = 3; // set next channel to ADC3 = air pressure |
158 | adc_channel = 3; // set next channel to ADC3 = air pressure |
159 | break; |
159 | break; |
160 | case 10: |
160 | case 10: |
161 | tmpAirPressure += ADC; // sum vadc values |
161 | tmpAirPressure += ADC; // sum vadc values |
162 | if(++messanzahl_Druck >= 5) // if 5 values are summerized for averaging |
162 | if(++average_pressure >= 5) // if 5 values are summerized for averaging |
163 | { |
163 | { |
164 | ReadingAirPressure = ADC; // update measured air pressure |
164 | ReadingAirPressure = ADC; // update measured air pressure |
165 | messanzahl_Druck = 0; // reset air pressure measurement counter |
165 | average_pressure = 0; // reset air pressure measurement counter |
166 | HoeheD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight); // D-Anteil = neuerWert - AlterWert |
166 | HightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight); // D-Anteil = neuerWert - AlterWert |
167 | AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
167 | AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
168 | ReadingHight = StartAirPressure - AirPressure; |
168 | ReadingHight = StartAirPressure - AirPressure; |
169 | tmpAirPressure = 0; |
169 | tmpAirPressure = 0; |
170 | } |
170 | } |
171 | adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
171 | adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
172 | state = 0; // reset state |
172 | state = 0; // reset state |
173 | break; |
173 | break; |
174 | default: |
174 | default: |
175 | adc_channel = 0; |
175 | adc_channel = 0; |
176 | state = 0; |
176 | state = 0; |
177 | break; |
177 | break; |
178 | } |
178 | } |
179 | // set adc muxer to next adc_channel |
179 | // set adc muxer to next adc_channel |
180 | ADMUX = (ADMUX & 0xE0) | adc_channel; |
180 | ADMUX = (ADMUX & 0xE0) | adc_channel; |
181 | // ?? |
181 | // ?? |
182 | if(state != 0) ADC_Enable(); |
182 | if(state != 0) ADC_Enable(); |
183 | } |
183 | } |
184 | 184 |