Rev 935 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1 | ingob | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
||
3 | // + only for non-profit use |
||
4 | // + www.MikroKopter.com |
||
5 | // + see the File "License.txt" for further Informations |
||
6 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
886 | killagreg | 7 | #include <stdlib.h> |
8 | #include <avr/io.h> |
||
9 | #include <avr/interrupt.h> |
||
1 | ingob | 10 | |
886 | killagreg | 11 | #include "analog.h" |
1 | ingob | 12 | #include "main.h" |
886 | killagreg | 13 | #include "timer0.h" |
14 | #include "fc.h" |
||
15 | #include "printf_P.h" |
||
16 | #include "eeprom.h" |
||
936 | killagreg | 17 | #include "twimaster.h" |
1 | ingob | 18 | |
886 | killagreg | 19 | volatile int16_t Current_AccZ = 0; |
20 | volatile int16_t UBat = 100; |
||
911 | killagreg | 21 | volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
936 | killagreg | 22 | uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115; |
23 | uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
||
911 | killagreg | 24 | volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0; |
886 | killagreg | 25 | volatile int32_t AirPressure = 32000; |
936 | killagreg | 26 | volatile uint8_t average_pressure = 0; |
886 | killagreg | 27 | volatile int16_t StartAirPressure; |
28 | volatile uint16_t ReadingAirPressure = 1023; |
||
29 | uint8_t PressureSensorOffset; |
||
30 | volatile int16_t HeightD = 0; |
||
31 | volatile uint16_t MeasurementCounter = 0; |
||
1 | ingob | 32 | |
886 | killagreg | 33 | /*****************************************************/ |
34 | /* Initialize Analog Digital Converter */ |
||
35 | /*****************************************************/ |
||
1 | ingob | 36 | void ADC_Init(void) |
886 | killagreg | 37 | { |
38 | uint8_t sreg = SREG; |
||
39 | // disable all interrupts before reconfiguration |
||
40 | cli(); |
||
41 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
||
42 | DDRA = 0x00; |
||
43 | PORTA = 0x00; |
||
44 | // Digital Input Disable Register 0 |
||
45 | // Disable digital input buffer for analog adc_channel pins |
||
46 | DIDR0 = 0xFF; |
||
47 | // external reference, adjust data to the right |
||
48 | ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR)); |
||
49 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
||
50 | ADMUX = (ADMUX & 0xE0) | 0x00; |
||
51 | //Set ADC Control and Status Register A |
||
52 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
||
53 | ADCSRA = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
||
54 | //Set ADC Control and Status Register B |
||
55 | //Trigger Source to Free Running Mode |
||
56 | ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
||
57 | // Enable AD conversion |
||
58 | ADC_Enable(); |
||
59 | // restore global interrupt flags |
||
60 | SREG = sreg; |
||
1 | ingob | 61 | } |
62 | |||
886 | killagreg | 63 | void SearchAirPressureOffset(void) |
1 | ingob | 64 | { |
886 | killagreg | 65 | uint8_t off; |
66 | off = GetParamByte(PID_PRESSURE_OFFSET); |
||
67 | if(off > 20) off -= 10; |
||
68 | OCR0A = off; |
||
69 | Delay_ms_Mess(100); |
||
70 | if(ReadingAirPressure < 850) off = 0; |
||
71 | for(; off < 250;off++) |
||
72 | { |
||
73 | OCR0A = off; |
||
74 | Delay_ms_Mess(50); |
||
75 | printf("."); |
||
76 | if(ReadingAirPressure < 900) break; |
||
77 | } |
||
78 | SetParamByte(PID_PRESSURE_OFFSET, off); |
||
79 | PressureSensorOffset = off; |
||
80 | Delay_ms_Mess(300); |
||
1 | ingob | 81 | } |
82 | |||
83 | |||
936 | killagreg | 84 | void SearchGyroOffset(void) |
85 | { |
||
86 | uint8_t i, ready = 0; |
||
87 | |||
88 | GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
||
89 | for(i = 140; i != 0; i--) |
||
90 | { |
||
91 | if(ready == 3 && i > 10) i = 9; |
||
92 | ready = 0; |
||
93 | if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++; |
||
94 | if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++; |
||
95 | if(AdValueGyrYaw < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw > 1030) AnalogOffsetYaw++ ; else ready++; |
||
96 | twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset |
||
97 | I2C_Start(); // initiate data transmission |
||
98 | if(AnalogOffsetNick < 10) { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;}; |
||
99 | if(AnalogOffsetRoll < 10) { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;}; |
||
100 | if(AnalogOffsetYaw < 10) { GyroDefectYaw = 1; AnalogOffsetYaw = 10;}; if(AnalogOffsetYaw > 245) { GyroDefectYaw = 1; AnalogOffsetYaw = 245;}; |
||
101 | while(twi_state); // wait for end of data transmission |
||
102 | average_pressure = 0; |
||
103 | ADC_Enable(); |
||
104 | while(average_pressure == 0); |
||
105 | if(i < 10) Delay_ms_Mess(10); |
||
106 | } |
||
107 | Delay_ms_Mess(70); |
||
108 | } |
||
109 | |||
110 | |||
111 | |||
112 | |||
886 | killagreg | 113 | /*****************************************************/ |
114 | /* Interrupt Service Routine for ADC */ |
||
115 | /*****************************************************/ |
||
116 | // runs at 156.25 kHz or 6.4 µs |
||
117 | // if after (70.4µs) all 11 states are processed the interrupt is disabled |
||
118 | // and the update of further ads is stopped |
||
119 | // The routine changes the ADC input muxer running |
||
120 | // thru the state machine by the following order. |
||
121 | // state 0: ch0 (yaw gyro) |
||
122 | // state 1: ch1 (roll gyro) |
||
911 | killagreg | 123 | // state 2: ch2 (nick gyro) |
886 | killagreg | 124 | // state 3: ch4 (battery voltage -> UBat) |
125 | // state 4: ch6 (acc y -> Current_AccY) |
||
126 | // state 5: ch7 (acc x -> Current_AccX) |
||
127 | // state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw) |
||
128 | // state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll) |
||
911 | killagreg | 129 | // state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick) |
886 | killagreg | 130 | // state 9: ch5 (acc z add also 4th part of acc x and acc y to reading) |
131 | // state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure) |
||
132 | |||
133 | ISR(ADC_vect) |
||
1 | ingob | 134 | { |
886 | killagreg | 135 | static uint8_t adc_channel = 0, state = 0; |
911 | killagreg | 136 | static uint16_t yaw1, roll1, nick1; |
886 | killagreg | 137 | static int16_t tmpAirPressure = 0; |
138 | // disable further AD conversion |
||
139 | ADC_Disable(); |
||
140 | // state machine |
||
1 | ingob | 141 | switch(state++) |
142 | { |
||
143 | case 0: |
||
886 | killagreg | 144 | yaw1 = ADC; // get Gyro Yaw Voltage 1st sample |
145 | adc_channel = 1; // set next channel to ADC1 = ROLL GYRO |
||
146 | MeasurementCounter++; // increment total measurement counter |
||
1 | ingob | 147 | break; |
148 | case 1: |
||
886 | killagreg | 149 | roll1 = ADC; // get Gyro Roll Voltage 1st sample |
911 | killagreg | 150 | adc_channel = 2; // set next channel to ADC2 = NICK GYRO |
1 | ingob | 151 | break; |
152 | case 2: |
||
911 | killagreg | 153 | nick1 = ADC; // get Gyro Nick Voltage 1st sample |
886 | killagreg | 154 | adc_channel = 4; // set next channel to ADC4 = UBAT |
1 | ingob | 155 | break; |
156 | case 3: |
||
886 | killagreg | 157 | // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3 |
158 | UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value |
||
159 | adc_channel = 6; // set next channel to ADC6 = ACC_Y |
||
1 | ingob | 160 | break; |
161 | case 4: |
||
886 | killagreg | 162 | AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction |
163 | adc_channel = 7; // set next channel to ADC7 = ACC_X |
||
1 | ingob | 164 | break; |
165 | case 5: |
||
911 | killagreg | 166 | AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction |
886 | killagreg | 167 | adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
1 | ingob | 168 | break; |
169 | case 6: |
||
886 | killagreg | 170 | // average over two samples to create current AdValueGyrYaw |
171 | if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
||
172 | else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
||
173 | adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
||
401 | hbuss | 174 | break; |
175 | case 7: |
||
886 | killagreg | 176 | // average over two samples to create current ADValueGyrRoll |
177 | if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2; |
||
178 | else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
||
911 | killagreg | 179 | adc_channel = 2; // set next channel to ADC2 = NICK GYRO |
401 | hbuss | 180 | break; |
181 | case 8: |
||
911 | killagreg | 182 | // average over two samples to create current ADValueNick |
183 | if(BoardRelease == 10) AdValueGyrNick = (ADC + nick1) / 2; |
||
184 | else AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1 |
||
886 | killagreg | 185 | adc_channel = 5; // set next channel to ADC5 = ACC_Z |
401 | hbuss | 186 | break; |
187 | case 9: |
||
886 | killagreg | 188 | // get z acceleration |
189 | AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction |
||
911 | killagreg | 190 | AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4; |
886 | killagreg | 191 | if(AdValueAccTop > 1) |
1 | ingob | 192 | { |
886 | killagreg | 193 | if(NeutralAccZ < 750) |
194 | { |
||
195 | NeutralAccZ += 0.02; |
||
196 | if(Model_Is_Flying < 500) NeutralAccZ += 0.1; |
||
197 | } |
||
198 | } |
||
199 | else if(AdValueAccTop < -1) |
||
1 | ingob | 200 | { |
886 | killagreg | 201 | if(NeutralAccZ > 550) |
202 | { |
||
203 | NeutralAccZ-= 0.02; |
||
204 | if(Model_Is_Flying < 500) NeutralAccZ -= 0.1; |
||
205 | } |
||
206 | } |
||
207 | Current_AccZ = ADC; |
||
208 | Reading_Integral_Top += AdValueAccTop; // Integrieren |
||
209 | Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen |
||
210 | adc_channel = 3; // set next channel to ADC3 = air pressure |
||
1 | ingob | 211 | break; |
401 | hbuss | 212 | case 10: |
886 | killagreg | 213 | tmpAirPressure += ADC; // sum vadc values |
214 | if(++average_pressure >= 5) // if 5 values are summerized for averaging |
||
215 | { |
||
216 | ReadingAirPressure = ADC; // update measured air pressure |
||
217 | HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(StartAirPressure - tmpAirPressure - ReadingHeight))/8; // D-Part = CurrentValue - OldValue |
||
218 | AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
||
219 | ReadingHeight = StartAirPressure - AirPressure; |
||
220 | average_pressure = 0; // reset air pressure measurement counter |
||
221 | tmpAirPressure = 0; |
||
222 | } |
||
223 | adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
||
224 | state = 0; // reset state machine |
||
1 | ingob | 225 | break; |
886 | killagreg | 226 | default: |
227 | adc_channel = 0; |
||
1 | ingob | 228 | state = 0; |
229 | break; |
||
886 | killagreg | 230 | } |
231 | // set adc muxer to next adc_channel |
||
232 | ADMUX = (ADMUX & 0xE0) | adc_channel; |
||
233 | // after full cycle stop further interrupts |
||
234 | if(state != 0) ADC_Enable(); |
||
1 | ingob | 235 | } |