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