Rev 1910 | Rev 2096 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1910 | Rev 1927 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
3 | #include <avr/pgmspace.h> |
3 | #include <avr/pgmspace.h> |
4 | 4 | ||
5 | #include "analog.h" |
5 | #include "analog.h" |
6 | #include "attitude.h" |
6 | #include "attitude.h" |
7 | #include "sensors.h" |
7 | #include "sensors.h" |
8 | 8 | ||
9 | // for Delay functions |
9 | // for Delay functions |
10 | #include "timer0.h" |
10 | #include "timer0.h" |
11 | 11 | ||
12 | // For DebugOut |
12 | // For DebugOut |
13 | #include "uart0.h" |
13 | #include "uart0.h" |
14 | 14 | ||
15 | // For reading and writing acc. meter offsets. |
15 | // For reading and writing acc. meter offsets. |
16 | #include "eeprom.h" |
16 | #include "eeprom.h" |
17 | 17 | ||
18 | // For DebugOut.Digital |
18 | // For DebugOut.Digital |
19 | #include "output.h" |
19 | #include "output.h" |
20 | 20 | ||
21 | /* |
21 | /* |
22 | * For each A/D conversion cycle, each analog channel is sampled a number of times |
22 | * For each A/D conversion cycle, each analog channel is sampled a number of times |
23 | * (see array channelsForStates), and the results for each channel are summed. |
23 | * (see array channelsForStates), and the results for each channel are summed. |
24 | * Here are those for the gyros and the acc. meters. They are not zero-offset. |
24 | * Here are those for the gyros and the acc. meters. They are not zero-offset. |
25 | * They are exported in the analog.h file - but please do not use them! The only |
25 | * They are exported in the analog.h file - but please do not use them! The only |
26 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
26 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
27 | * the offsets with the DAC. |
27 | * the offsets with the DAC. |
28 | */ |
28 | */ |
29 | volatile int16_t rawGyroSum[3]; |
29 | volatile int16_t rawGyroSum[3]; |
30 | volatile int16_t acc[3]; |
30 | volatile int16_t acc[3]; |
31 | volatile int16_t filteredAcc[2] = { 0,0 }; |
31 | volatile int16_t filteredAcc[2] = { 0,0 }; |
32 | 32 | ||
33 | /* |
33 | /* |
34 | * These 4 exported variables are zero-offset. The "PID" ones are used |
34 | * These 4 exported variables are zero-offset. The "PID" ones are used |
35 | * in the attitude control as rotation rates. The "ATT" ones are for |
35 | * in the attitude control as rotation rates. The "ATT" ones are for |
36 | * integration to angles. |
36 | * integration to angles. |
37 | */ |
37 | */ |
38 | volatile int16_t gyro_PID[2]; |
38 | volatile int16_t gyro_PID[2]; |
39 | volatile int16_t gyro_ATT[2]; |
39 | volatile int16_t gyro_ATT[2]; |
40 | volatile int16_t gyroD[3]; |
40 | volatile int16_t gyroD[3]; |
41 | volatile int16_t yawGyro; |
41 | volatile int16_t yawGyro; |
42 | 42 | ||
43 | /* |
43 | /* |
44 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
44 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
45 | * standing still. They are used for adjusting the gyro and acc. meter values |
45 | * standing still. They are used for adjusting the gyro and acc. meter values |
46 | * to be centered on zero. |
46 | * to be centered on zero. |
47 | */ |
47 | */ |
48 | volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
48 | volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
49 | * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
49 | * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
50 | 50 | ||
51 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
51 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
52 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
52 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
53 | 53 | ||
54 | /* |
54 | /* |
55 | * This allows some experimentation with the gyro filters. |
55 | * This allows some experimentation with the gyro filters. |
56 | * Should be replaced by #define's later on... |
56 | * Should be replaced by #define's later on... |
57 | */ |
57 | */ |
58 | volatile uint8_t GYROS_PID_FILTER; |
58 | volatile uint8_t GYROS_PID_FILTER; |
59 | volatile uint8_t GYROS_ATT_FILTER; |
59 | volatile uint8_t GYROS_ATT_FILTER; |
60 | volatile uint8_t GYROS_D_FILTER; |
60 | volatile uint8_t GYROS_D_FILTER; |
61 | volatile uint8_t ACC_FILTER; |
61 | volatile uint8_t ACC_FILTER; |
62 | 62 | ||
63 | /* |
63 | /* |
64 | * Air pressure |
64 | * Air pressure |
65 | */ |
65 | */ |
66 | volatile uint8_t rangewidth = 106; |
66 | volatile uint8_t rangewidth = 106; |
67 | 67 | ||
68 | // Direct from sensor, irrespective of range. |
68 | // Direct from sensor, irrespective of range. |
69 | // volatile uint16_t rawAirPressure; |
69 | // volatile uint16_t rawAirPressure; |
70 | 70 | ||
71 | // Value of 2 samples, with range. |
71 | // Value of 2 samples, with range. |
72 | volatile uint16_t simpleAirPressure; |
72 | volatile uint16_t simpleAirPressure; |
73 | 73 | ||
74 | // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
74 | // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
75 | volatile int32_t filteredAirPressure; |
75 | volatile int32_t filteredAirPressure; |
76 | 76 | ||
77 | // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
77 | // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
78 | volatile int32_t airPressureSum; |
78 | volatile int32_t airPressureSum; |
79 | 79 | ||
80 | // The number of samples summed into airPressureSum so far. |
80 | // The number of samples summed into airPressureSum so far. |
81 | volatile uint8_t pressureMeasurementCount; |
81 | volatile uint8_t pressureMeasurementCount; |
82 | 82 | ||
83 | /* |
83 | /* |
84 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
84 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
85 | * That is divided by 3 below, for a final 10.34 per volt. |
85 | * That is divided by 3 below, for a final 10.34 per volt. |
86 | * So the initial value of 100 is for 9.7 volts. |
86 | * So the initial value of 100 is for 9.7 volts. |
87 | */ |
87 | */ |
88 | volatile int16_t UBat = 100; |
88 | volatile int16_t UBat = 100; |
89 | 89 | ||
90 | /* |
90 | /* |
91 | * Control and status. |
91 | * Control and status. |
92 | */ |
92 | */ |
93 | volatile uint16_t ADCycleCount = 0; |
93 | volatile uint16_t ADCycleCount = 0; |
94 | volatile uint8_t analogDataReady = 1; |
94 | volatile uint8_t analogDataReady = 1; |
95 | 95 | ||
96 | /* |
96 | /* |
97 | * Experiment: Measuring vibration-induced sensor noise. |
97 | * Experiment: Measuring vibration-induced sensor noise. |
98 | */ |
98 | */ |
99 | volatile uint16_t gyroNoisePeak[2]; |
99 | volatile uint16_t gyroNoisePeak[2]; |
100 | volatile uint16_t accNoisePeak[2]; |
100 | volatile uint16_t accNoisePeak[2]; |
101 | 101 | ||
102 | // ADC channels |
102 | // ADC channels |
103 | #define AD_GYRO_YAW 0 |
103 | #define AD_GYRO_YAW 0 |
104 | #define AD_GYRO_ROLL 1 |
104 | #define AD_GYRO_ROLL 1 |
105 | #define AD_GYRO_PITCH 2 |
105 | #define AD_GYRO_PITCH 2 |
106 | #define AD_AIRPRESSURE 3 |
106 | #define AD_AIRPRESSURE 3 |
107 | #define AD_UBAT 4 |
107 | #define AD_UBAT 4 |
108 | #define AD_ACC_Z 5 |
108 | #define AD_ACC_Z 5 |
109 | #define AD_ACC_ROLL 6 |
109 | #define AD_ACC_ROLL 6 |
110 | #define AD_ACC_PITCH 7 |
110 | #define AD_ACC_PITCH 7 |
111 | 111 | ||
112 | /* |
112 | /* |
113 | * Table of AD converter inputs for each state. |
113 | * Table of AD converter inputs for each state. |
114 | * The number of samples summed for each channel is equal to |
114 | * The number of samples summed for each channel is equal to |
115 | * the number of times the channel appears in the array. |
115 | * the number of times the channel appears in the array. |
116 | * The max. number of samples that can be taken in 2 ms is: |
116 | * The max. number of samples that can be taken in 2 ms is: |
117 | * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
117 | * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
118 | * loop needs a little time between reading AD values and |
118 | * loop needs a little time between reading AD values and |
119 | * re-enabling ADC, the real limit is (how much?) lower. |
119 | * re-enabling ADC, the real limit is (how much?) lower. |
120 | * The acc. sensor is sampled even if not used - or installed |
120 | * The acc. sensor is sampled even if not used - or installed |
121 | * at all. The cost is not significant. |
121 | * at all. The cost is not significant. |
122 | */ |
122 | */ |
123 | 123 | ||
124 | const uint8_t channelsForStates[] PROGMEM = { |
124 | const uint8_t channelsForStates[] PROGMEM = { |
125 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, |
125 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, |
126 | AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE, |
126 | AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE, |
127 | 127 | ||
128 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc. |
128 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc. |
129 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro |
129 | AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro |
130 | 130 | ||
131 | AD_ACC_PITCH, // at 12, finish pitch axis acc. |
131 | AD_ACC_PITCH, // at 12, finish pitch axis acc. |
132 | AD_ACC_ROLL, // at 13, finish roll axis acc. |
132 | AD_ACC_ROLL, // at 13, finish roll axis acc. |
133 | AD_AIRPRESSURE, // at 14, finish air pressure. |
133 | AD_AIRPRESSURE, // at 14, finish air pressure. |
134 | 134 | ||
135 | AD_GYRO_PITCH, // at 15, finish pitch gyro |
135 | AD_GYRO_PITCH, // at 15, finish pitch gyro |
136 | AD_GYRO_ROLL, // at 16, finish roll gyro |
136 | AD_GYRO_ROLL, // at 16, finish roll gyro |
137 | AD_UBAT // at 17, measure battery. |
137 | AD_UBAT // at 17, measure battery. |
138 | }; |
138 | }; |
139 | 139 | ||
140 | // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
140 | // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
141 | // uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
141 | // uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
142 | 142 | ||
143 | void analog_init(void) { |
143 | void analog_init(void) { |
144 | uint8_t sreg = SREG; |
144 | uint8_t sreg = SREG; |
145 | // disable all interrupts before reconfiguration |
145 | // disable all interrupts before reconfiguration |
146 | cli(); |
146 | cli(); |
147 | 147 | ||
148 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
148 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
149 | DDRA = 0x00; |
149 | DDRA = 0x00; |
150 | PORTA = 0x00; |
150 | PORTA = 0x00; |
151 | // Digital Input Disable Register 0 |
151 | // Digital Input Disable Register 0 |
152 | // Disable digital input buffer for analog adc_channel pins |
152 | // Disable digital input buffer for analog adc_channel pins |
153 | DIDR0 = 0xFF; |
153 | DIDR0 = 0xFF; |
154 | // external reference, adjust data to the right |
154 | // external reference, adjust data to the right |
155 | ADMUX &= ~((1 << REFS1) | (1 << REFS0) | (1 << ADLAR)); |
155 | ADMUX &= ~((1 << REFS1) | (1 << REFS0) | (1 << ADLAR)); |
156 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
156 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
157 | ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH; |
157 | ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH; |
158 | //Set ADC Control and Status Register A |
158 | //Set ADC Control and Status Register A |
159 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
159 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
160 | ADCSRA = (0 << ADEN) | (0 << ADSC) | (0 << ADATE) | (1 << ADPS2) | (1 |
160 | ADCSRA = (0 << ADEN) | (0 << ADSC) | (0 << ADATE) | (1 << ADPS2) | (1 |
161 | << ADPS1) | (1 << ADPS0) | (0 << ADIE); |
161 | << ADPS1) | (1 << ADPS0) | (0 << ADIE); |
162 | //Set ADC Control and Status Register B |
162 | //Set ADC Control and Status Register B |
163 | //Trigger Source to Free Running Mode |
163 | //Trigger Source to Free Running Mode |
164 | ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0)); |
164 | ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0)); |
165 | // Start AD conversion |
165 | // Start AD conversion |
166 | analog_start(); |
166 | analog_start(); |
167 | // restore global interrupt flags |
167 | // restore global interrupt flags |
168 | SREG = sreg; |
168 | SREG = sreg; |
169 | } |
169 | } |
170 | 170 | ||
171 | void measureNoise(const int16_t sensor, |
171 | void measureNoise(const int16_t sensor, |
172 | volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
172 | volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
173 | if (sensor > (int16_t) (*noiseMeasurement)) { |
173 | if (sensor > (int16_t) (*noiseMeasurement)) { |
174 | *noiseMeasurement = sensor; |
174 | *noiseMeasurement = sensor; |
175 | } else if (-sensor > (int16_t) (*noiseMeasurement)) { |
175 | } else if (-sensor > (int16_t) (*noiseMeasurement)) { |
176 | *noiseMeasurement = -sensor; |
176 | *noiseMeasurement = -sensor; |
177 | } else if (*noiseMeasurement > damping) { |
177 | } else if (*noiseMeasurement > damping) { |
178 | *noiseMeasurement -= damping; |
178 | *noiseMeasurement -= damping; |
179 | } else { |
179 | } else { |
180 | *noiseMeasurement = 0; |
180 | *noiseMeasurement = 0; |
181 | } |
181 | } |
182 | } |
182 | } |
183 | 183 | ||
184 | /* |
184 | /* |
185 | * Min.: 0 |
185 | * Min.: 0 |
186 | * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
186 | * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
187 | */ |
187 | */ |
188 | uint16_t getSimplePressure(int advalue) { |
188 | uint16_t getSimplePressure(int advalue) { |
189 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
189 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
190 | } |
190 | } |
191 | 191 | ||
192 | void transformPRGyro(int16_t* result) { |
192 | void transformPRGyro(int16_t* result) { |
193 | static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1}; |
193 | static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1}; |
194 | int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
194 | int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
195 | int8_t pr = tab[(GYRO_QUADRANT+2)%8]; |
195 | int8_t pr = tab[(GYRO_QUADRANT+2)%8]; |
196 | int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
196 | int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
197 | int8_t rr = tab[GYRO_QUADRANT]; |
197 | int8_t rr = tab[GYRO_QUADRANT]; |
198 | 198 | ||
199 | int16_t temp = result[0]; |
199 | int16_t temp = result[0]; |
200 | result[0] = pp*result[0] + pr*result[1]; |
200 | result[0] = pp*result[0] + pr*result[1]; |
201 | result[1] = rp*temp + rr*result[1]; |
201 | result[1] = rp*temp + rr*result[1]; |
202 | } |
202 | } |
203 | 203 | ||
204 | /***************************************************** |
204 | /***************************************************** |
205 | * Interrupt Service Routine for ADC |
205 | * Interrupt Service Routine for ADC |
206 | * Runs at 312.5 kHz or 3.2 µs. When all states are |
206 | * Runs at 312.5 kHz or 3.2 µs. When all states are |
207 | * processed the interrupt is disabled and further |
207 | * processed the interrupt is disabled and further |
208 | * AD conversions are stopped. |
208 | * AD conversions are stopped. |
209 | *****************************************************/ |
209 | *****************************************************/ |
210 | ISR(ADC_vect) { |
210 | ISR(ADC_vect) { |
211 | static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
211 | static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
212 | static uint16_t sensorInputs[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; |
212 | static uint16_t sensorInputs[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; |
213 | static uint16_t pressureAutorangingWait = 25; |
213 | static uint16_t pressureAutorangingWait = 25; |
214 | uint16_t rawAirPressure; |
214 | uint16_t rawAirPressure; |
215 | uint8_t i, axis; |
215 | uint8_t i, axis; |
216 | int16_t newrange; |
216 | int16_t newrange; |
217 | 217 | ||
218 | // for various filters... |
218 | // for various filters... |
219 | int16_t tempOffsetGyro[2]; |
219 | int16_t tempOffsetGyro[2]; |
220 | 220 | ||
221 | sensorInputs[ad_channel] += ADC; |
221 | sensorInputs[ad_channel] += ADC; |
222 | 222 | ||
223 | /* |
223 | /* |
224 | * Actually we don't need this "switch". We could do all the sampling into the |
224 | * Actually we don't need this "switch". We could do all the sampling into the |
225 | * sensorInputs array first, and all the processing after the last sample. |
225 | * sensorInputs array first, and all the processing after the last sample. |
226 | */ |
226 | */ |
227 | switch (state++) { |
227 | switch (state++) { |
228 | 228 | ||
229 | case 8: // Z acc |
229 | case 8: // Z acc |
230 | if (Z_ACC_REVERSED) |
230 | if (Z_ACC_REVERSED) |
231 | acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
231 | acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
232 | else |
232 | else |
233 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
233 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
234 | 234 | ||
235 | /* |
235 | /* |
236 | stronglyFilteredAcc[Z] = |
236 | stronglyFilteredAcc[Z] = |
237 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
237 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
238 | */ |
238 | */ |
239 | 239 | ||
240 | break; |
240 | break; |
241 | 241 | ||
242 | case 11: // yaw gyro |
242 | case 11: // yaw gyro |
243 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
243 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
244 | if (YAW_REVERSED) |
244 | if (YAW_REVERSED) |
245 | tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
245 | tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
246 | else |
246 | else |
247 | tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
247 | tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
248 | gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER; |
248 | gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER; |
249 | yawGyro = tempOffsetGyro[0]; |
249 | yawGyro = tempOffsetGyro[0]; |
250 | break; |
250 | break; |
251 | case 13: // roll axis acc. |
251 | case 13: // roll axis acc. |
252 | /* |
252 | /* |
253 | for (axis=0; axis<2; axis++) { |
253 | for (axis=0; axis<2; axis++) { |
254 | if (ACC_REVERSED[axis]) |
254 | if (ACC_REVERSED[axis]) |
255 | tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
255 | tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
256 | else |
256 | else |
257 | tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
257 | tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
258 | } |
258 | } |
259 | if (AXIS_TRANSFORM) { |
259 | if (AXIS_TRANSFORM) { |
260 | acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL]; |
260 | acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL]; |
261 | acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH]; |
261 | acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH]; |
262 | } else { |
262 | } else { |
263 | acc[PITCH] = tempSensor[PITCH]; |
263 | acc[PITCH] = tempSensor[PITCH]; |
264 | acc[ROLL] = tempSensor[ROLL]; |
264 | acc[ROLL] = tempSensor[ROLL]; |
265 | } |
265 | } |
266 | */ |
266 | */ |
267 | 267 | ||
268 | // We have no sensor installed... |
268 | // We have no sensor installed... |
269 | acc[PITCH] = acc[ROLL] = 0; |
269 | acc[PITCH] = acc[ROLL] = 0; |
270 | 270 | ||
271 | for (axis=0; axis<2; axis++) { |
271 | for (axis=0; axis<2; axis++) { |
272 | filteredAcc[axis] = |
272 | filteredAcc[axis] = |
273 | (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER; |
273 | (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER; |
274 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
274 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
275 | } |
275 | } |
276 | break; |
276 | break; |
277 | 277 | ||
278 | case 14: // air pressure |
278 | case 14: // air pressure |
279 | if (pressureAutorangingWait) { |
279 | if (pressureAutorangingWait) { |
280 | //A range switch was done recently. Wait for steadying. |
280 | //A range switch was done recently. Wait for steadying. |
281 | pressureAutorangingWait--; |
281 | pressureAutorangingWait--; |
282 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
282 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
283 | DebugOut.Analog[31] = simpleAirPressure; |
283 | DebugOut.Analog[31] = simpleAirPressure; |
284 | break; |
284 | break; |
285 | } |
285 | } |
286 | 286 | ||
287 | rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
287 | rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
288 | if (rawAirPressure < MIN_RAWPRESSURE) { |
288 | if (rawAirPressure < MIN_RAWPRESSURE) { |
289 | // value is too low, so decrease voltage on the op amp minus input, making the value higher. |
289 | // value is too low, so decrease voltage on the op amp minus input, making the value higher. |
290 | newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1; |
290 | newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1; |
291 | if (newrange > MIN_RANGES_EXTRAPOLATION) { |
291 | if (newrange > MIN_RANGES_EXTRAPOLATION) { |
292 | pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 + |
292 | pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 + |
293 | OCR0A = newrange; |
293 | OCR0A = newrange; |
294 | } else { |
294 | } else { |
295 | if (OCR0A) { |
295 | if (OCR0A) { |
296 | OCR0A--; |
296 | OCR0A--; |
297 | pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
297 | pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
298 | } |
298 | } |
299 | } |
299 | } |
300 | } else if (rawAirPressure > MAX_RAWPRESSURE) { |
300 | } else if (rawAirPressure > MAX_RAWPRESSURE) { |
301 | // value is too high, so increase voltage on the op amp minus input, making the value lower. |
301 | // value is too high, so increase voltage on the op amp minus input, making the value lower. |
302 | // If near the end, make a limited increase |
302 | // If near the end, make a limited increase |
303 | newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1; |
303 | newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1; |
304 | if (newrange < MAX_RANGES_EXTRAPOLATION) { |
304 | if (newrange < MAX_RANGES_EXTRAPOLATION) { |
305 | pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
305 | pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
306 | OCR0A = newrange; |
306 | OCR0A = newrange; |
307 | } else { |
307 | } else { |
308 | if (OCR0A < 254) { |
308 | if (OCR0A < 254) { |
309 | OCR0A++; |
309 | OCR0A++; |
310 | pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
310 | pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
311 | } |
311 | } |
312 | } |
312 | } |
313 | } |
313 | } |
314 | 314 | ||
315 | // Even if the sample is off-range, use it. |
315 | // Even if the sample is off-range, use it. |
316 | simpleAirPressure = getSimplePressure(rawAirPressure); |
316 | simpleAirPressure = getSimplePressure(rawAirPressure); |
317 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
317 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
318 | DebugOut.Analog[31] = simpleAirPressure; |
318 | DebugOut.Analog[31] = simpleAirPressure; |
319 | 319 | ||
320 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
320 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
321 | // Danger: pressure near lower end of range. If the measurement saturates, the |
321 | // Danger: pressure near lower end of range. If the measurement saturates, the |
322 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
322 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
323 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
323 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
324 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
324 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
325 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
325 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
326 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
326 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
327 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
327 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
328 | // Danger: pressure near upper end of range. If the measurement saturates, the |
328 | // Danger: pressure near upper end of range. If the measurement saturates, the |
329 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
329 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
330 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
330 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
331 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
331 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
332 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
332 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
333 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
333 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
334 | } else { |
334 | } else { |
335 | // normal case. |
335 | // normal case. |
336 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
336 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
337 | // The 2 cases above (end of range) are ignored for this. |
337 | // The 2 cases above (end of range) are ignored for this. |
338 | DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT; |
338 | DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT; |
339 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
339 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
340 | airPressureSum += simpleAirPressure / 2; |
340 | airPressureSum += simpleAirPressure / 2; |
341 | else |
341 | else |
342 | airPressureSum += simpleAirPressure; |
342 | airPressureSum += simpleAirPressure; |
343 | } |
343 | } |
344 | 344 | ||
345 | // 2 samples were added. |
345 | // 2 samples were added. |
346 | pressureMeasurementCount += 2; |
346 | pressureMeasurementCount += 2; |
347 | if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) { |
347 | if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) { |
348 | filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
348 | filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
349 | + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
349 | + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
350 | pressureMeasurementCount = airPressureSum = 0; |
350 | pressureMeasurementCount = airPressureSum = 0; |
351 | } |
351 | } |
352 | 352 | ||
353 | break; |
353 | break; |
354 | 354 | ||
355 | case 16: // pitch and roll gyro. |
355 | case 16: // pitch and roll gyro. |
356 | for (axis=0; axis<2; axis++) { |
356 | for (axis=0; axis<2; axis++) { |
357 | tempOffsetGyro[axis] = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
357 | tempOffsetGyro[axis] = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
358 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
358 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
359 | // gyro with a wider range, and helps counter saturation at full control. |
359 | // gyro with a wider range, and helps counter saturation at full control. |
360 | 360 | ||
361 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
361 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
362 | if (tempOffsetGyro[axis] < SENSOR_MIN_PITCHROLL) { |
362 | if (tempOffsetGyro[axis] < SENSOR_MIN_PITCHROLL) { |
363 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
363 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
364 | tempOffsetGyro[axis] = tempOffsetGyro[axis] * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
364 | tempOffsetGyro[axis] = tempOffsetGyro[axis] * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
365 | } else if (tempOffsetGyro[axis] > SENSOR_MAX_PITCHROLL) { |
365 | } else if (tempOffsetGyro[axis] > SENSOR_MAX_PITCHROLL) { |
366 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
366 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
367 | tempOffsetGyro[axis] = (tempOffsetGyro[axis] - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
367 | tempOffsetGyro[axis] = (tempOffsetGyro[axis] - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
368 | } else { |
368 | } else { |
369 | DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT; |
369 | DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT; |
370 | } |
370 | } |
371 | } |
371 | } |
372 | 372 | ||
373 | // 2) Apply sign and offset, scale before filtering. |
373 | // 2) Apply sign and offset, scale before filtering. |
374 | tempOffsetGyro[axis] = (tempOffsetGyro[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
374 | tempOffsetGyro[axis] = (tempOffsetGyro[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
375 | } |
375 | } |
376 | 376 | ||
377 | // 2.1: Transform axis if configured to. |
377 | // 2.1: Transform axis if configured to. |
378 | transformPRGyro(tempOffsetGyro); |
378 | transformPRGyro(tempOffsetGyro); |
379 | 379 | ||
380 | // 3) Scale and filter. |
380 | // 3) Scale and filter. |
381 | for (axis=0; axis<2; axis++) { |
381 | for (axis=0; axis<2; axis++) { |
382 | tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER; |
382 | tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER; |
383 | 383 | ||
384 | // 4) Measure noise. |
384 | // 4) Measure noise. |
385 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
385 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
386 | 386 | ||
387 | // 5) Differential measurement. |
387 | // 5) Differential measurement. |
388 | gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER; |
388 | gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER; |
389 | 389 | ||
390 | // 6) Done. |
390 | // 6) Done. |
391 | gyro_PID[axis] = tempOffsetGyro[axis]; |
391 | gyro_PID[axis] = tempOffsetGyro[axis]; |
392 | } |
392 | } |
393 | 393 | ||
394 | /* |
394 | /* |
395 | * Now process the data for attitude angles. |
395 | * Now process the data for attitude angles. |
396 | */ |
396 | */ |
397 | for (axis=0; axis<2; axis++) { |
397 | for (axis=0; axis<2; axis++) { |
398 | tempOffsetGyro[axis] = (rawGyroSum[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
398 | tempOffsetGyro[axis] = (rawGyroSum[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
399 | } |
399 | } |
400 | 400 | ||
401 | transformPRGyro(tempOffsetGyro); |
401 | transformPRGyro(tempOffsetGyro); |
402 | 402 | ||
403 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
403 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
404 | gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER; |
404 | gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER; |
405 | gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER; |
405 | gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER; |
406 | break; |
406 | break; |
407 | 407 | ||
408 | case 17: |
408 | case 17: |
409 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
409 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
410 | // This is divided by 3 --> 10.34 counts per volt. |
410 | // This is divided by 3 --> 10.34 counts per volt. |
411 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
411 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
412 | DebugOut.Analog[20] = UBat; |
412 | DebugOut.Analog[20] = UBat; |
413 | analogDataReady = 1; // mark |
413 | analogDataReady = 1; // mark |
414 | ADCycleCount++; |
414 | ADCycleCount++; |
415 | // Stop the sampling. Cycle is over. |
415 | // Stop the sampling. Cycle is over. |
416 | state = 0; |
416 | state = 0; |
417 | for (i = 0; i < 8; i++) { |
417 | for (i = 0; i < 8; i++) { |
418 | sensorInputs[i] = 0; |
418 | sensorInputs[i] = 0; |
419 | } |
419 | } |
420 | break; |
420 | break; |
421 | default: { |
421 | default: { |
422 | } // do nothing. |
422 | } // do nothing. |
423 | } |
423 | } |
424 | 424 | ||
425 | // set up for next state. |
425 | // set up for next state. |
426 | ad_channel = pgm_read_byte(&channelsForStates[state]); |
426 | ad_channel = pgm_read_byte(&channelsForStates[state]); |
427 | // ad_channel = channelsForStates[state]; |
427 | // ad_channel = channelsForStates[state]; |
428 | 428 | ||
429 | // set adc muxer to next ad_channel |
429 | // set adc muxer to next ad_channel |
430 | ADMUX = (ADMUX & 0xE0) | ad_channel; |
430 | ADMUX = (ADMUX & 0xE0) | ad_channel; |
431 | // after full cycle stop further interrupts |
431 | // after full cycle stop further interrupts |
432 | if (state) |
432 | if (state) |
433 | analog_start(); |
433 | analog_start(); |
434 | } |
434 | } |
435 | 435 | ||
436 | void analog_calibrate(void) { |
436 | void analog_calibrate(void) { |
437 | #define GYRO_OFFSET_CYCLES 32 |
437 | #define GYRO_OFFSET_CYCLES 32 |
438 | uint8_t i, axis; |
438 | uint8_t i, axis; |
439 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
439 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
440 | 440 | ||
441 | // Set the filters... to be removed again, once some good settings are found. |
441 | // Set the filters... to be removed again, once some good settings are found. |
442 | GYROS_PID_FILTER = (dynamicParams.UserParams[4] & (0x7 & (1<<0))) + 1; |
442 | GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1; |
443 | GYROS_ATT_FILTER = 1; |
443 | GYROS_ATT_FILTER = 1; |
444 | GYROS_D_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<4))) + 1; |
444 | GYROS_D_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1; |
445 | ACC_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<6))) + 1; |
445 | ACC_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1; |
446 | 446 | ||
447 | gyro_calibrate(); |
447 | gyro_calibrate(); |
448 | 448 | ||
449 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
449 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
450 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
450 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
451 | delay_ms_Mess(20); |
451 | delay_ms_Mess(20); |
452 | for (axis = PITCH; axis <= YAW; axis++) { |
452 | for (axis = PITCH; axis <= YAW; axis++) { |
453 | deltaOffsets[axis] += rawGyroSum[axis]; |
453 | deltaOffsets[axis] += rawGyroSum[axis]; |
454 | } |
454 | } |
455 | } |
455 | } |
456 | 456 | ||
457 | for (axis = PITCH; axis <= YAW; axis++) { |
457 | for (axis = PITCH; axis <= YAW; axis++) { |
458 | gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
458 | gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
459 | // DebugOut.Analog[20 + axis] = gyroOffset[axis]; |
459 | // DebugOut.Analog[20 + axis] = gyroOffset[axis]; |
460 | } |
460 | } |
461 | 461 | ||
462 | // Noise is relativ to offset. So, reset noise measurements when changing offsets. |
462 | // Noise is relativ to offset. So, reset noise measurements when changing offsets. |
463 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
463 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
464 | 464 | ||
465 | accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
465 | accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
466 | accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
466 | accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
467 | accOffset[Z] = GetParamWord(PID_ACC_Z); |
467 | accOffset[Z] = GetParamWord(PID_ACC_Z); |
468 | 468 | ||
469 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
469 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
470 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
470 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
471 | // pressureMeasurementCount = 0; |
471 | // pressureMeasurementCount = 0; |
472 | 472 | ||
473 | delay_ms_Mess(100); |
473 | delay_ms_Mess(100); |
474 | } |
474 | } |
475 | 475 | ||
476 | /* |
476 | /* |
477 | * Find acc. offsets for a neutral reading, and write them to EEPROM. |
477 | * Find acc. offsets for a neutral reading, and write them to EEPROM. |
478 | * Does not (!} update the local variables. This must be done with a |
478 | * Does not (!} update the local variables. This must be done with a |
479 | * call to analog_calibrate() - this always (?) is done by the caller |
479 | * call to analog_calibrate() - this always (?) is done by the caller |
480 | * anyway. There would be nothing wrong with updating the variables |
480 | * anyway. There would be nothing wrong with updating the variables |
481 | * directly from here, though. |
481 | * directly from here, though. |
482 | */ |
482 | */ |
483 | void analog_calibrateAcc(void) { |
483 | void analog_calibrateAcc(void) { |
484 | #define ACC_OFFSET_CYCLES 10 |
484 | #define ACC_OFFSET_CYCLES 10 |
485 | /* |
485 | /* |
486 | uint8_t i, axis; |
486 | uint8_t i, axis; |
487 | int32_t deltaOffset[3] = { 0, 0, 0 }; |
487 | int32_t deltaOffset[3] = { 0, 0, 0 }; |
488 | int16_t filteredDelta; |
488 | int16_t filteredDelta; |
489 | // int16_t pressureDiff, savedRawAirPressure; |
489 | // int16_t pressureDiff, savedRawAirPressure; |
490 | 490 | ||
491 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
491 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
492 | delay_ms_Mess(10); |
492 | delay_ms_Mess(10); |
493 | for (axis = PITCH; axis <= YAW; axis++) { |
493 | for (axis = PITCH; axis <= YAW; axis++) { |
494 | deltaOffset[axis] += acc[axis]; |
494 | deltaOffset[axis] += acc[axis]; |
495 | } |
495 | } |
496 | } |
496 | } |
497 | 497 | ||
498 | for (axis = PITCH; axis <= YAW; axis++) { |
498 | for (axis = PITCH; axis <= YAW; axis++) { |
499 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
499 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
500 | / ACC_OFFSET_CYCLES; |
500 | / ACC_OFFSET_CYCLES; |
501 | accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
501 | accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
502 | } |
502 | } |
503 | 503 | ||
504 | // Save ACC neutral settings to eeprom |
504 | // Save ACC neutral settings to eeprom |
505 | SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
505 | SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
506 | SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
506 | SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
507 | SetParamWord(PID_ACC_Z, accOffset[Z]); |
507 | SetParamWord(PID_ACC_Z, accOffset[Z]); |
508 | 508 | ||
509 | // Noise is relative to offset. So, reset noise measurements when |
509 | // Noise is relative to offset. So, reset noise measurements when |
510 | // changing offsets. |
510 | // changing offsets. |
511 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
511 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
512 | 512 | ||
513 | // Setting offset values has an influence in the analog.c ISR |
513 | // Setting offset values has an influence in the analog.c ISR |
514 | // Therefore run measurement for 100ms to achive stable readings |
514 | // Therefore run measurement for 100ms to achive stable readings |
515 | delay_ms_Mess(100); |
515 | delay_ms_Mess(100); |
516 | 516 | ||
517 | */ |
517 | */ |
518 | // Set the feedback so that air pressure ends up in the middle of the range. |
518 | // Set the feedback so that air pressure ends up in the middle of the range. |
519 | // (raw pressure high --> OCR0A also high...) |
519 | // (raw pressure high --> OCR0A also high...) |
520 | /* |
520 | /* |
521 | OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1; |
521 | OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1; |
522 | delay_ms_Mess(1000); |
522 | delay_ms_Mess(1000); |
523 | 523 | ||
524 | pressureDiff = 0; |
524 | pressureDiff = 0; |
525 | // DebugOut.Analog[16] = rawAirPressure; |
525 | // DebugOut.Analog[16] = rawAirPressure; |
526 | 526 | ||
527 | #define PRESSURE_CAL_CYCLE_COUNT 5 |
527 | #define PRESSURE_CAL_CYCLE_COUNT 5 |
528 | for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
528 | for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
529 | savedRawAirPressure = rawAirPressure; |
529 | savedRawAirPressure = rawAirPressure; |
530 | OCR0A+=2; |
530 | OCR0A+=2; |
531 | delay_ms_Mess(500); |
531 | delay_ms_Mess(500); |
532 | // raw pressure will decrease. |
532 | // raw pressure will decrease. |
533 | pressureDiff += (savedRawAirPressure - rawAirPressure); |
533 | pressureDiff += (savedRawAirPressure - rawAirPressure); |
534 | savedRawAirPressure = rawAirPressure; |
534 | savedRawAirPressure = rawAirPressure; |
535 | OCR0A-=2; |
535 | OCR0A-=2; |
536 | delay_ms_Mess(500); |
536 | delay_ms_Mess(500); |
537 | // raw pressure will increase. |
537 | // raw pressure will increase. |
538 | pressureDiff += (rawAirPressure - savedRawAirPressure); |
538 | pressureDiff += (rawAirPressure - savedRawAirPressure); |
539 | } |
539 | } |
540 | 540 | ||
541 | rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2); |
541 | rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2); |
542 | DebugOut.Analog[27] = rangewidth; |
542 | DebugOut.Analog[27] = rangewidth; |
543 | */ |
543 | */ |
544 | } |
544 | } |
545 | 545 |