Subversion Repositories FlightCtrl

Rev

Rev 1617 | Go to most recent revision | Details | 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
 
5
//#include "invenSense.h"
6
#include "ENC-03_FC1.3.h"
7
 
8
 
9
/*
10
 * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro.
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
 */
14
//#define GYROS_FIRSTORDERFILTER 2
15
 
16
/*
17
 * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro.
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
 */
21
//#define GYROS_SECONDORDERFILTER 2
22
 
23
// Temporarily replaced by userparam-configurable variable.
24
//#define ACC_FILTER 4
25
 
26
/*
27
  About setting constants right for different gyros:
28
  Main parameters are positive directions and voltage/angular speed gain.
29
  The "Positive direction" is the rotation direction around an axis where
30
  the corresponding gyro gives a voltage > the no-rotation voltage.
31
  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.
33
  Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
34
  GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
35
 
36
  Setting gyro gain correctly: All sensor measurements in analog.c take
37
  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
39
  are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
40
  code.
41
  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
43
  to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
44
  roll.
45
  So:
46
 
47
  HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
48
    where V is 2 for FC1.0 and 4 for all others.
49
 
50
  Assuming constant ADCValue, in the H&I code:
51
 
52
  HiResXXXX = I * ADCValue.
53
 
54
  where I is 8 for FC1.0 and 16 for all others.
55
 
56
  The relation between rotation rate and ADCValue:
57
  ADCValue [units] =
58
    rotational speed [deg/s] *
59
    gyro sensitivity [V / deg/s] *
60
    amplifier gain [units] *
61
    1024 [units] /
62
    3V full range [V]
63
 
64
  or: H is the number of steps the ADC value changes with,
65
  for a 1 deg/s change in rotational velocity:
66
  H = ADCValue [units] / rotation rate [deg/s] =
67
    gyro sensitivity [V / deg/s] *
68
    amplifier gain [units] *
69
    1024 [units] /
70
    3V full range [V]
71
 
72
  Examples:
73
  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).
75
  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).
77
  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)
79
    (only about half as sensitive as V1.3. But it will take about twice the
80
     rotation rate!)
81
 
82
  All together: HiResXXXX = I * H * rotation rate [units / (deg/s)].
83
*/
84
 
85
/*
86
 * A factor that the raw gyro values are multiplied by,
87
 * before being zero-offset, filtered and passed to the attitude module.
88
 * 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
90
 * it will not really matter - but when testing on the desk it might be
91
 * noticeable). 4 is fine for the default filtering.
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
114
  HiResXXXX is added to gyroIntegralXXXX.
115
  Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this
116
  explanation), we get:
117
 
118
  gyroIntegralXXXX =
119
    N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR =
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
/*
137
 * The value of hiResXXXX 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.
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
/*
144
 * This value is subtracted from the gyro noise measurement in each iteration,
145
 * making it return towards zero.
146
 */
147
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
148
 
149
/*
150
 * The values that this module outputs
151
 */
152
extern volatile int16_t hiResPitchGyro, hiResRollGyro;
153
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro;
154
extern volatile int16_t pitchGyroD, rollGyroD;
155
extern volatile uint16_t ADCycleCount;
156
extern volatile int16_t UBat;
157
extern volatile int16_t yawGyro;
158
 
159
/*
160
 * This is not really for external use - but the ENC-03 gyro modules needs it.
161
 */
162
extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
163
 
164
/*
165
 * The acceleration values that this module outputs
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;
175
// extern volatile int16_t HeightD;
176
// extern volatile uint16_t ReadingAirPressure;
177
// extern volatile int16_t StartAirPressure;
178
// extern uint8_t PressureSensorOffset;
179
// extern int8_t ExpandBaro;
180
 
181
/*
182
 * Flag: Interrupt handler has done all A/D conversion and processing.
183
 */
184
extern volatile uint8_t analogDataReady;
185
 
186
// Diagnostics: Gyro noise level because of motor vibrations. The variables
187
// only really reflect the noise level when the copter stands still but with 
188
// its motors running.
189
extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
190
extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
191
 
192
// void SearchAirPressureOffset(void);
193
 
194
void analog_init(void);
195
 
196
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
197
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
198
 
199
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
200
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
201
 
202
/*
203
 * "Warm" calibration: Zero-offset gyros.
204
 */
205
void analog_calibrate(void);
206
 
207
/*
208
 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
209
 */
210
void analog_calibrateAcc(void);
211
#endif //_ANALOG_H