Rev 1821 | Rev 1864 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1821 | Rev 1854 | ||
---|---|---|---|
Line 65... | Line 65... | ||
65 | #include "eeprom.h" |
65 | #include "eeprom.h" |
Line 66... | Line 66... | ||
66 | 66 | ||
67 | // For DebugOut.Digital |
67 | // For DebugOut.Digital |
Line 68... | Line 68... | ||
68 | #include "output.h" |
68 | #include "output.h" |
69 | 69 | ||
70 | /* |
70 | /* |
71 | * For each A/D conversion cycle, each analog channel is sampled a number of times |
71 | * For each A/D conversion cycle, each analog channel is sampled a number of times |
72 | * (see array channelsForStates), and the results for each channel are summed. |
72 | * (see array channelsForStates), and the results for each channel are summed. |
73 | * Here are those for the gyros and the acc. meters. They are not zero-offset. |
73 | * Here are those for the gyros and the acc. meters. They are not zero-offset. |
74 | * They are exported in the analog.h file - but please do not use them! The only |
74 | * They are exported in the analog.h file - but please do not use them! The only |
75 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
75 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
Line 80... | Line 80... | ||
80 | volatile int16_t filteredAcc[2] = { 0, 0 }; |
80 | volatile int16_t filteredAcc[2] = { 0, 0 }; |
Line 81... | Line 81... | ||
81 | 81 | ||
82 | /* |
82 | /* |
83 | * These 4 exported variables are zero-offset. The "PID" ones are used |
83 | * These 4 exported variables are zero-offset. The "PID" ones are used |
84 | * in the attitude control as rotation rates. The "ATT" ones are for |
84 | * in the attitude control as rotation rates. The "ATT" ones are for |
85 | * integration to angles. |
85 | * integration to angles. |
86 | */ |
86 | */ |
87 | volatile int16_t gyro_PID[2]; |
87 | volatile int16_t gyro_PID[2]; |
88 | volatile int16_t gyro_ATT[2]; |
88 | volatile int16_t gyro_ATT[2]; |
89 | volatile int16_t gyroD[2]; |
89 | volatile int16_t gyroD[2]; |
Line 128... | Line 128... | ||
128 | 128 | ||
129 | // The number of samples summed into airPressureSum so far. |
129 | // The number of samples summed into airPressureSum so far. |
Line 130... | Line 130... | ||
130 | volatile uint8_t pressureMeasurementCount; |
130 | volatile uint8_t pressureMeasurementCount; |
131 | 131 | ||
132 | /* |
132 | /* |
133 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
133 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
134 | * That is divided by 3 below, for a final 10.34 per volt. |
134 | * That is divided by 3 below, for a final 10.34 per volt. |
135 | * So the initial value of 100 is for 9.7 volts. |
135 | * So the initial value of 100 is for 9.7 volts. |
Line 158... | Line 158... | ||
158 | #define AD_ACC_ROLL 6 |
158 | #define AD_ACC_ROLL 6 |
159 | #define AD_ACC_PITCH 7 |
159 | #define AD_ACC_PITCH 7 |
Line 160... | Line 160... | ||
160 | 160 | ||
161 | /* |
161 | /* |
162 | * Table of AD converter inputs for each state. |
162 | * Table of AD converter inputs for each state. |
163 | * The number of samples summed for each channel is equal to |
163 | * The number of samples summed for each channel is equal to |
164 | * the number of times the channel appears in the array. |
164 | * the number of times the channel appears in the array. |
165 | * The max. number of samples that can be taken in 2 ms is: |
165 | * The max. number of samples that can be taken in 2 ms is: |
166 | * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
166 | * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
167 | * loop needs a little time between reading AD values and |
167 | * loop needs a little time between reading AD values and |
168 | * re-enabling ADC, the real limit is (how much?) lower. |
168 | * re-enabling ADC, the real limit is (how much?) lower. |
169 | * The acc. sensor is sampled even if not used - or installed |
169 | * The acc. sensor is sampled even if not used - or installed |
170 | * at all. The cost is not significant. |
170 | * at all. The cost is not significant. |
Line 239... | Line 239... | ||
239 | uint16_t getSimplePressure(int advalue) { |
239 | uint16_t getSimplePressure(int advalue) { |
240 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
240 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
241 | } |
241 | } |
Line 242... | Line 242... | ||
242 | 242 | ||
243 | /***************************************************** |
243 | /***************************************************** |
244 | * Interrupt Service Routine for ADC |
244 | * Interrupt Service Routine for ADC |
245 | * Runs at 312.5 kHz or 3.2 µs. When all states are |
245 | * Runs at 312.5 kHz or 3.2 µs. When all states are |
246 | * processed the interrupt is disabled and further |
246 | * processed the interrupt is disabled and further |
247 | * AD conversions are stopped. |
247 | * AD conversions are stopped. |
248 | *****************************************************/ |
248 | *****************************************************/ |
249 | ISR(ADC_vect) |
249 | ISR(ADC_vect) |
250 | { |
250 | { |
Line 345... | Line 345... | ||
345 | DebugOut.Analog[31] = simpleAirPressure; |
345 | DebugOut.Analog[31] = simpleAirPressure; |
Line 346... | Line 346... | ||
346 | 346 | ||
347 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
347 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
348 | // Danger: pressure near lower end of range. If the measurement saturates, the |
348 | // Danger: pressure near lower end of range. If the measurement saturates, the |
- | 349 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
|
349 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
350 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
350 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
351 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
351 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
352 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
352 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
353 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
353 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
354 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
354 | // Danger: pressure near upper end of range. If the measurement saturates, the |
355 | // Danger: pressure near upper end of range. If the measurement saturates, the |
- | 356 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
|
355 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
357 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
356 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
358 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
357 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
359 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
358 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
360 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
359 | } else { |
361 | } else { |
360 | // normal case. |
362 | // normal case. |
361 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
363 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
- | 364 | // The 2 cases above (end of range) are ignored for this. |
|
362 | // The 2 cases above (end of range) are ignored for this. |
365 | DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT; |
363 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
366 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
364 | airPressureSum += simpleAirPressure / 2; |
367 | airPressureSum += simpleAirPressure / 2; |
365 | else |
368 | else |
366 | airPressureSum += simpleAirPressure; |
369 | airPressureSum += simpleAirPressure; |
Line 387... | Line 390... | ||
387 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
390 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
388 | // gyro with a wider range, and helps counter saturation at full control. |
391 | // gyro with a wider range, and helps counter saturation at full control. |
Line 389... | Line 392... | ||
389 | 392 | ||
390 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
393 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
- | 394 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
|
391 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
395 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
392 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
396 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
- | 397 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
|
393 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
398 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
394 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
399 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
- | 400 | + SENSOR_MAX_PITCHROLL; |
|
- | 401 | } else { |
|
395 | + SENSOR_MAX_PITCHROLL; |
402 | DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT; |
396 | } |
403 | } |
Line 397... | Line 404... | ||
397 | } |
404 | } |
398 | 405 |