Subversion Repositories FlightCtrl

Rev

Rev 1634 | Rev 1646 | 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
 
1645 - 5
// #include "invenSense.h"
1634 - 6
#include "ENC-03_FC1.3.h"
1612 dongfang 7
 
8
/*
1645 - 9
 * How much low pass filtering to apply for gyro_PID.
1612 dongfang 10
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
11
 * Temporarily replaced by userparam-configurable variable.
12
 */
1645 - 13
#define GYROS_PIDFILTER 1
1612 dongfang 14
 
15
/*
1645 - 16
 * How much low pass filtering to apply for gyro_ATT.
1612 dongfang 17
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
18
 * Temporarily replaced by userparam-configurable variable.
19
 */
1645 - 20
#define GYROS_INTEGRALFILTER 1
1612 dongfang 21
 
22
// Temporarily replaced by userparam-configurable variable.
23
//#define ACC_FILTER 4
24
 
25
/*
1645 - 26
  About setting constants for different gyros:
1612 dongfang 27
  Main parameters are positive directions and voltage/angular speed gain.
28
  The "Positive direction" is the rotation direction around an axis where
1645 - 29
  the corresponding gyro outputs a voltage > the no-rotation voltage.
1612 dongfang 30
  A gyro is considered, in this code, to be "forward" if its positive
1645 - 31
  direction is the same as in FC1.0-1.3, and reverse otherwise.
1612 dongfang 32
  Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
33
  GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
34
 
35
  Setting gyro gain correctly: All sensor measurements in analog.c take
36
  place in a cycle, each cycle comprising all sensors. Some sensors are
37
  sampled more than ones, and the results added. The pitch and roll gyros
38
  are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
39
  code.
40
  In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
41
  or 4 (other versions), offset to zero, low pass filtered and then assigned
42
  to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
43
  roll.
44
  So:
45
 
1645 - 46
  gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
1612 dongfang 47
    where V is 2 for FC1.0 and 4 for all others.
48
 
49
  Assuming constant ADCValue, in the H&I code:
50
 
1645 - 51
  gyro = I * ADCValue.
1612 dongfang 52
 
53
  where I is 8 for FC1.0 and 16 for all others.
54
 
55
  The relation between rotation rate and ADCValue:
56
  ADCValue [units] =
57
    rotational speed [deg/s] *
58
    gyro sensitivity [V / deg/s] *
59
    amplifier gain [units] *
60
    1024 [units] /
61
    3V full range [V]
62
 
63
  or: H is the number of steps the ADC value changes with,
64
  for a 1 deg/s change in rotational velocity:
65
  H = ADCValue [units] / rotation rate [deg/s] =
66
    gyro sensitivity [V / deg/s] *
67
    amplifier gain [units] *
68
    1024 [units] /
69
    3V full range [V]
70
 
71
  Examples:
72
  FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
73
    H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
74
  FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
75
    H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
76
  My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
77
    H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
78
    (only about half as sensitive as V1.3. But it will take about twice the
79
     rotation rate!)
80
 
1645 - 81
  All together: gyro = I * H * rotation rate [units / (deg/s)].
1612 dongfang 82
*/
83
 
84
/*
85
 * A factor that the raw gyro values are multiplied by,
1645 - 86
 * before being filtered and passed to the attitude module.
1612 dongfang 87
 * A value of 1 would cause a little bit of loss of precision in the
88
 * filtering (on the other hand the values are so noisy in flight that
89
 * it will not really matter - but when testing on the desk it might be
90
 * noticeable). 4 is fine for the default filtering.
1645 - 91
 * Experiment: Set it to 1.
1612 dongfang 92
 */
93
#define GYRO_FACTOR_PITCHROLL 4
94
 
95
/*
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
98
 * channelsForStates array in analog.c.
99
 */
100
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
101
#define GYRO_SUMMATION_FACTOR_YAW 2
102
 
103
/*
104
  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
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
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
111
  constant is there.
112
 
113
  The control loop executes every 2ms, and for each iteration
1645 - 114
  gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
115
  Assuming a constant rotation rate v and a zero initial gyroIntegral
116
  (for this explanation), we get:
1612 dongfang 117
 
1645 - 118
  gyroIntegral =
119
    N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
1612 dongfang 120
    N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
121
 
122
  where N is the number of summations; N = t/2ms.
123
 
124
  For one degree of rotation: t*v = 1:
125
 
126
  gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
127
 
128
  This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
129
 
130
  Examples:
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
133
  My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
134
*/
135
 
136
/*
1645 - 137
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
1612 dongfang 138
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
139
 */
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)
142
 
143
/*
1645 - 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
/*
1612 dongfang 156
 * This value is subtracted from the gyro noise measurement in each iteration,
157
 * making it return towards zero.
158
 */
159
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
160
 
1645 - 161
#define PITCH 0
162
#define ROLL 1
1612 dongfang 163
/*
164
 * The values that this module outputs
1645 - 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.
170
 * Maybe this distinction is not really necessary.
1612 dongfang 171
 */
1645 - 172
extern volatile int16_t gyro_PID[2];
173
extern volatile int16_t gyro_ATT[2];
174
extern volatile int16_t gyroD[2];
175
 
1612 dongfang 176
extern volatile uint16_t ADCycleCount;
177
extern volatile int16_t UBat;
178
extern volatile int16_t yawGyro;
179
 
180
/*
181
 * This is not really for external use - but the ENC-03 gyro modules needs it.
182
 */
1645 - 183
extern volatile int16_t rawGyroSum[2], rawYawGyroSum;
1612 dongfang 184
 
185
/*
1645 - 186
 * The acceleration values that this module outputs. They are zero based.
1612 dongfang 187
 */
1645 - 188
extern volatile int16_t acc[2], ZAcc;
189
extern volatile int16_t filteredAcc[2];
1612 dongfang 190
 
191
/*
192
 * Flag: Interrupt handler has done all A/D conversion and processing.
193
 */
194
extern volatile uint8_t analogDataReady;
195
 
196
// Diagnostics: Gyro noise level because of motor vibrations. The variables
197
// only really reflect the noise level when the copter stands still but with 
198
// its motors running.
1645 - 199
extern volatile uint16_t gyroNoisePeak[2];
200
extern volatile uint16_t accNoisePeak[2];
1612 dongfang 201
 
202
// void SearchAirPressureOffset(void);
203
 
204
void analog_init(void);
205
 
206
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
207
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
208
 
209
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
210
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
211
 
212
/*
213
 * "Warm" calibration: Zero-offset gyros.
214
 */
215
void analog_calibrate(void);
216
 
217
/*
218
 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
219
 */
220
void analog_calibrateAcc(void);
221
#endif //_ANALOG_H