WebSVN - FlightCtrl - Diff - Rev 1821 and 1854 - /branches/dongfang_FC



#include "eeprom.h"

 
// For DebugOut.Digital

#include "output.h"
 
/*
 * For each A/D conversion cycle, each analog channel is sampled a number of times
 * (see array channelsForStates), and the results for each channel are summed.
 * Here are those for the gyros and the acc. meters. They are not zero-offset.
 * They are exported in the analog.h file - but please do not use them! The only
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating

volatile int16_t filteredAcc[2] = { 0, 0 };

 
/*
 * These 4 exported variables are zero-offset. The "PID" ones are used
 * in the attitude control as rotation rates. The "ATT" ones are for
 * integration to angles.
 */
volatile int16_t gyro_PID[2];
volatile int16_t gyro_ATT[2];
volatile int16_t gyroD[2];

 
// The number of samples summed into airPressureSum so far.

volatile uint8_t pressureMeasurementCount;
 
/*
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
 * That is divided by 3 below, for a final 10.34 per volt.
 * So the initial value of 100 is for 9.7 volts.

#define AD_ACC_ROLL       6
#define AD_ACC_PITCH      7

 
/*
 * Table of AD converter inputs for each state.
 * The number of samples summed for each channel is equal to
 * the number of times the channel appears in the array.
 * The max. number of samples that can be taken in 2 ms is:
 * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
 * loop needs a little time between reading AD values and
 * re-enabling ADC, the real limit is (how much?) lower.
 * The acc. sensor is sampled even if not used - or installed
 * at all. The cost is not significant.

uint16_t getSimplePressure(int advalue) {
        return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
}

 
/*****************************************************
 * Interrupt Service Routine for ADC
 * Runs at 312.5 kHz or 3.2 µs. When all states are
 * processed the interrupt is disabled and further
 * AD conversions are stopped.
 *****************************************************/
ISR(ADC_vect)
{

                DebugOut.Analog[31] = simpleAirPressure;

 
                if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
                        // Danger: pressure near lower end of range. If the measurement saturates, the
                        // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
                        DebugOut.Digital[1] |= DEBUG_SENSORLIMIT;
                        airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
                                        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
                                                        * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
                } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
                        // Danger: pressure near upper end of range. If the measurement saturates, the
                        // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
                        DebugOut.Digital[1] |= DEBUG_SENSORLIMIT;
                        airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
                                        + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
                                                        * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
                } else {
                        // normal case.
                        // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.
                        // The 2 cases above (end of range) are ignored for this.
                        DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT;
                        if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1)
                                airPressureSum += simpleAirPressure / 2;
                        else
                                airPressureSum += simpleAirPressure;

                // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
                //    gyro with a wider range, and helps counter saturation at full control.

 
                if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
                        if (tempGyro < SENSOR_MIN_PITCHROLL) {
                                DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
                                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
                        } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
                                DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
                                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
                                                + SENSOR_MAX_PITCHROLL;
                        } else {
                                DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT;
                        }

                }
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c @ 1871 - Rev 1821 → 1854