Subversion Repositories FlightCtrl

Rev

Rev 1634 | Rev 1646 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1634 Rev 1645
1 
#ifndef _ANALOG_H
 1 
#ifndef _ANALOG_H
2 
#define _ANALOG_H
 2 
#define _ANALOG_H
3 
#include <inttypes.h>
 3 
#include <inttypes.h>
4 
 
 4 
 
5 
//#include "invenSense.h"
 5 
// #include "invenSense.h"
6 
#include "ENC-03_FC1.3.h"
 - 
 
7 
 
 6 
#include "ENC-03_FC1.3.h"
8 
 
 7 
 
9 
/*
 8 
/*
10 
 * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro.
 9 
 * How much low pass filtering to apply for gyro_PID.
11 
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
 10 
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
12 
 * Temporarily replaced by userparam-configurable variable.
 11 
 * Temporarily replaced by userparam-configurable variable.
13 
 */
 12 
 */
14 
//#define GYROS_FIRSTORDERFILTER 2
 13 
#define GYROS_PIDFILTER 1
15 
 
 14 
 
16 
/*
 15 
/*
17 
 * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro.
 16 
 * How much low pass filtering to apply for gyro_ATT.
18 
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
 17 
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
19 
 * Temporarily replaced by userparam-configurable variable.
 18 
 * Temporarily replaced by userparam-configurable variable.
20 
 */
 19 
 */
21 
//#define GYROS_SECONDORDERFILTER 2
 20 
#define GYROS_INTEGRALFILTER 1
22 
 
 21 
 
23 
// Temporarily replaced by userparam-configurable variable.
 22 
// Temporarily replaced by userparam-configurable variable.
24 
//#define ACC_FILTER 4
 23 
//#define ACC_FILTER 4
25 
 
 24 
 
26 
/*
 25 
/*
27 
  About setting constants right for different gyros:
 26 
  About setting constants for different gyros:
28 
  Main parameters are positive directions and voltage/angular speed gain.
 27 
  Main parameters are positive directions and voltage/angular speed gain.
29 
  The "Positive direction" is the rotation direction around an axis where
 28 
  The "Positive direction" is the rotation direction around an axis where
30 
  the corresponding gyro gives a voltage > the no-rotation voltage.
 29 
  the corresponding gyro outputs a voltage > the no-rotation voltage.
31 
  A gyro is considered, in this code, to be "forward" if its positive
 30 
  A gyro is considered, in this code, to be "forward" if its positive
32 
  direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise.
 31 
  direction is the same as in FC1.0-1.3, and reverse otherwise.
33 
  Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
 32 
  Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
34 
  GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
 33 
  GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
35 
 
 34 
 
36 
  Setting gyro gain correctly: All sensor measurements in analog.c take
 35 
  Setting gyro gain correctly: All sensor measurements in analog.c take
37 
  place in a cycle, each cycle comprising all sensors. Some sensors are
 36 
  place in a cycle, each cycle comprising all sensors. Some sensors are
38 
  sampled more than ones, and the results added. The pitch and roll gyros
 37 
  sampled more than ones, and the results added. The pitch and roll gyros
39 
  are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
 38 
  are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
40 
  code.
 39 
  code.
41 
  In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
 40 
  In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
42 
  or 4 (other versions), offset to zero, low pass filtered and then assigned
 41 
  or 4 (other versions), offset to zero, low pass filtered and then assigned
43 
  to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
 42 
  to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
44 
  roll.
 43 
  roll.
45 
  So:
 44 
  So:
46 
 
 45 
 
47 
  HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
 46 
  gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
48 
    where V is 2 for FC1.0 and 4 for all others.
 47 
    where V is 2 for FC1.0 and 4 for all others.
49 
 
 48 
 
50 
  Assuming constant ADCValue, in the H&I code:
 49 
  Assuming constant ADCValue, in the H&I code:
51 
 
 50 
 
52 
  HiResXXXX = I * ADCValue.
 51 
  gyro = I * ADCValue.
53 
 
 52 
 
54 
  where I is 8 for FC1.0 and 16 for all others.
 53 
  where I is 8 for FC1.0 and 16 for all others.
55 
 
 54 
 
56 
  The relation between rotation rate and ADCValue:
 55 
  The relation between rotation rate and ADCValue:
57 
  ADCValue [units] =
 56 
  ADCValue [units] =
58 
    rotational speed [deg/s] *
 57 
    rotational speed [deg/s] *
59 
    gyro sensitivity [V / deg/s] *
 58 
    gyro sensitivity [V / deg/s] *
60 
    amplifier gain [units] *
 59 
    amplifier gain [units] *
61 
    1024 [units] /
 60 
    1024 [units] /
62 
    3V full range [V]
 61 
    3V full range [V]
63 
 
 62 
 
64 
  or: H is the number of steps the ADC value changes with,
 63 
  or: H is the number of steps the ADC value changes with,
65 
  for a 1 deg/s change in rotational velocity:
 64 
  for a 1 deg/s change in rotational velocity:
66 
  H = ADCValue [units] / rotation rate [deg/s] =
 65 
  H = ADCValue [units] / rotation rate [deg/s] =
67 
    gyro sensitivity [V / deg/s] *
 66 
    gyro sensitivity [V / deg/s] *
68 
    amplifier gain [units] *
 67 
    amplifier gain [units] *
69 
    1024 [units] /
 68 
    1024 [units] /
70 
    3V full range [V]
 69 
    3V full range [V]
71 
 
 70 
 
72 
  Examples:
 71 
  Examples:
73 
  FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
 72 
  FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
74 
    H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
 73 
    H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
75 
  FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
 74 
  FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
76 
    H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
 75 
    H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
77 
  My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
 76 
  My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
78 
    H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
 77 
    H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
79 
    (only about half as sensitive as V1.3. But it will take about twice the
 78 
    (only about half as sensitive as V1.3. But it will take about twice the
80 
     rotation rate!)
 79 
     rotation rate!)
81 
 
 80 
 
82 
  All together: HiResXXXX = I * H * rotation rate [units / (deg/s)].
 81 
  All together: gyro = I * H * rotation rate [units / (deg/s)].
83 
*/
 82 
*/
84 
 
 83 
 
85 
/*
 84 
/*
86 
 * A factor that the raw gyro values are multiplied by,
 85 
 * A factor that the raw gyro values are multiplied by,
87 
 * before being zero-offset, filtered and passed to the attitude module.
 86 
 * before being filtered and passed to the attitude module.
88 
 * A value of 1 would cause a little bit of loss of precision in the
 87 
 * A value of 1 would cause a little bit of loss of precision in the
89 
 * filtering (on the other hand the values are so noisy in flight that
 88 
 * filtering (on the other hand the values are so noisy in flight that
90 
 * it will not really matter - but when testing on the desk it might be
 89 
 * it will not really matter - but when testing on the desk it might be
91 
 * noticeable). 4 is fine for the default filtering.
 90 
 * noticeable). 4 is fine for the default filtering.
- 
 
 91 
 * Experiment: Set it to 1.
92 
 */
 92 
 */
93 
#define GYRO_FACTOR_PITCHROLL 4
 93 
#define GYRO_FACTOR_PITCHROLL 4
94 
 
 94 
 
95 
/*
 95 
/*
96 
 * How many samples are summed in one ADC loop, for pitch&roll and yaw,
 96 
 * How many samples are summed in one ADC loop, for pitch&roll and yaw,
97 
 * respectively. This is = the number of occurences of each channel in the
 97 
 * respectively. This is = the number of occurences of each channel in the
98 
 * channelsForStates array in analog.c.
 98 
 * channelsForStates array in analog.c.
99 
 */
 99 
 */
100 
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
 100 
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
101 
#define GYRO_SUMMATION_FACTOR_YAW 2
 101 
#define GYRO_SUMMATION_FACTOR_YAW 2
102 
 
 102 
 
103 
/*
 103 
/*
104 
  Integration:
 104 
  Integration:
105 
  The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
 105 
  The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
106 
  In the Killagreg rewrite of the H&I firmware, the factor 8 is called
 106 
  In the Killagreg rewrite of the H&I firmware, the factor 8 is called
107 
  HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
 107 
  HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
108 
  and care has been taken that all other constants (gyro to degree factor, and
 108 
  and care has been taken that all other constants (gyro to degree factor, and
109 
  180 degree flip-over detection limits) are corrected to it. Because the
 109 
  180 degree flip-over detection limits) are corrected to it. Because the
110 
  division by the constant takes place in the flight attitude code, the
 110 
  division by the constant takes place in the flight attitude code, the
111 
  constant is there.
 111 
  constant is there.
112 
 
 112 
 
113 
  The control loop executes every 2ms, and for each iteration
 113 
  The control loop executes every 2ms, and for each iteration
114 
  HiResXXXX is added to gyroIntegralXXXX.
 114 
  gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
115 
  Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this
 115 
  Assuming a constant rotation rate v and a zero initial gyroIntegral
116 
  explanation), we get:
 116 
  (for this explanation), we get:
117 
 
 117 
 
118 
  gyroIntegralXXXX =
 118 
  gyroIntegral =
119 
    N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR =
 119 
    N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
120 
    N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
 120 
    N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
121 
 
 121 
 
122 
  where N is the number of summations; N = t/2ms.
 122 
  where N is the number of summations; N = t/2ms.
123 
 
 123 
 
124 
  For one degree of rotation: t*v = 1:
 124 
  For one degree of rotation: t*v = 1:
125 
 
 125 
 
126 
  gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
 126 
  gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
127 
 
 127 
 
128 
  This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
 128 
  This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
129 
 
 129 
 
130 
  Examples:
 130 
  Examples:
131 
  FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
 131 
  FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
132 
  FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
 132 
  FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
133 
  My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
 133 
  My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
134 
*/
 134 
*/
135 
 
 135 
 
136 
/*
 136 
/*
137 
 * The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor.
 137 
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
138 
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
 138 
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
139 
 */
 139 
 */
140 
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
 140 
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
141 
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
 141 
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
142 
 
 142 
 
143 
/*
 143 
/*
- 
 
 144 
 * Gyro saturation prevention.
- 
 
 145 
 */
- 
 
 146 
// How far from the end of its range a gyro is considered near-saturated.
- 
 
 147 
#define SENSOR_MIN_PITCHROLL 32
- 
 
 148 
// Other end of the range (calculated)
- 
 
 149 
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
- 
 
 150 
// Max. boost to add "virtually" to gyro signal at total saturation.
- 
 
 151 
#define EXTRAPOLATION_LIMIT 2500
- 
 
 152 
// Slope of the boost (calculated)
- 
 
 153 
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
- 
 
 154 
 
- 
 
 155 
/*
144 
 * This value is subtracted from the gyro noise measurement in each iteration,
 156 
 * This value is subtracted from the gyro noise measurement in each iteration,
145 
 * making it return towards zero.
 157 
 * making it return towards zero.
146 
 */
 158 
 */
147 
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
 159 
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
- 
 
 160 
 
- 
 
 161 
#define PITCH 0
148 
 
 162 
#define ROLL 1
149 
/*
 163 
/*
- 
 
 164 
 * The values that this module outputs
- 
 
 165 
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
- 
 
 166 
 * in the attitude control as rotation rates. The "ATT" ones are for
- 
 
 167 
 * integration to angles. For the same axis, the PID and ATT variables
- 
 
 168 
 * generally have about the same values. There are just some differences
- 
 
 169 
 * in filtering, and when a gyro becomes near saturated.
150 
 * The values that this module outputs
 170 
 * Maybe this distinction is not really necessary.
151 
 */
 171 
 */
152 
extern volatile int16_t hiResPitchGyro, hiResRollGyro;
 172 
extern volatile int16_t gyro_PID[2];
153 
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro;
 173 
extern volatile int16_t gyro_ATT[2];
- 
 
 174 
extern volatile int16_t gyroD[2];
154 
extern volatile int16_t pitchGyroD, rollGyroD;
 175 
 
155 
extern volatile uint16_t ADCycleCount;
 176 
extern volatile uint16_t ADCycleCount;
156 
extern volatile int16_t UBat;
 177 
extern volatile int16_t UBat;
157 
extern volatile int16_t yawGyro;
 178 
extern volatile int16_t yawGyro;
158 
 
 179 
 
159 
/*
 180 
/*
160 
 * This is not really for external use - but the ENC-03 gyro modules needs it.
 181 
 * This is not really for external use - but the ENC-03 gyro modules needs it.
161 
 */
 182 
 */
162 
extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
 183 
extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
163 
 
 184 
 
164 
/*
 185 
/*
165 
 * The acceleration values that this module outputs
 186 
 * The acceleration values that this module outputs. They are zero based.
166 
 */
 - 
 
167 
extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc;
 - 
 
168 
extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc;
 - 
 
169 
 
 - 
 
170 
// Only for debugging! Not to be exported! Remove when finished.
 - 
 
171 
// extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
 - 
 
172 
 
 - 
 
173 
// Air pressure measurement not supported right now.
 - 
 
174 
// extern volatile int32_t AirPressure;
 187 
 */
175 
// extern volatile int16_t HeightD;
 - 
 
176 
// extern volatile uint16_t ReadingAirPressure;
 188 
extern volatile int16_t acc[2], ZAcc;
177 
// extern volatile int16_t StartAirPressure;
 - 
 
178 
// extern uint8_t PressureSensorOffset;
 - 
 
179 
// extern int8_t ExpandBaro;
 189 
extern volatile int16_t filteredAcc[2];
180 
 
 190 
 
181 
/*
 191 
/*
182 
 * Flag: Interrupt handler has done all A/D conversion and processing.
 192 
 * Flag: Interrupt handler has done all A/D conversion and processing.
183 
 */
 193 
 */
184 
extern volatile uint8_t analogDataReady;
 194 
extern volatile uint8_t analogDataReady;
185 
 
 195 
 
186 
// Diagnostics: Gyro noise level because of motor vibrations. The variables
 196 
// Diagnostics: Gyro noise level because of motor vibrations. The variables
187 
// only really reflect the noise level when the copter stands still but with
197 
// only really reflect the noise level when the copter stands still but with
188 
// its motors running.
 198 
// its motors running.
189 
extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
 199 
extern volatile uint16_t gyroNoisePeak[2];
190 
extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
 200 
extern volatile uint16_t accNoisePeak[2];
191 
 
 201 
 
192 
// void SearchAirPressureOffset(void);
 202 
// void SearchAirPressureOffset(void);
193 
 
 203 
 
194 
void analog_init(void);
 204 
void analog_init(void);
195 
 
 205 
 
196 
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
 206 
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
197 
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
 207 
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
198 
 
 208 
 
199 
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
 209 
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
200 
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
 210 
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
201 
 
 211 
 
202 
/*
 212 
/*
203 
 * "Warm" calibration: Zero-offset gyros.
 213 
 * "Warm" calibration: Zero-offset gyros.
204 
 */
 214 
 */
205 
void analog_calibrate(void);
 215 
void analog_calibrate(void);
206 
 
 216 
 
207 
/*
 217 
/*
208 
 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
 218 
 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
209 
 */
 219 
 */
210 
void analog_calibrateAcc(void);
 220 
void analog_calibrateAcc(void);
211 
#endif //_ANALOG_H
 221 
#endif //_ANALOG_H
212 
 
 222