Subversion Repositories FlightCtrl

Rev

Rev 1646 | Rev 1796 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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