Subversion Repositories FlightCtrl

Rev

Rev 1796 | Rev 1872 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1796 Rev 1821
Line 22... Line 22...
22 
 
 22 
 
23 
// Temporarily replaced by userparam-configurable variable.
 23 
// Temporarily replaced by userparam-configurable variable.
Line 24... Line 24...
24 
// #define ACC_FILTER 4
 24 
// #define ACC_FILTER 4
25 
 
 25 
 
26 
/*
 26 
/*
27 
  About setting constants for different gyros:
 27 
 About setting constants for different gyros:
28 
  Main parameters are positive directions and voltage/angular speed gain.
 28 
 Main parameters are positive directions and voltage/angular speed gain.
29 
  The "Positive direction" is the rotation direction around an axis where
 29 
 The "Positive direction" is the rotation direction around an axis where
30 
  the corresponding gyro outputs a voltage > the no-rotation voltage.
 30 
 the corresponding gyro outputs a voltage > the no-rotation voltage.
31 
  A gyro is considered, in this code, to be "forward" if its positive
 31 
 A gyro is considered, in this code, to be "forward" if its positive
32 
  direction is:
 32 
 direction is:
33 
  - Nose down for pitch
 33 
 - Nose down for pitch
34 
  - Left hand side down for roll
 34 
 - Left hand side down for roll
35 
  - Clockwise seen from above for yaw.
 35 
 - Clockwise seen from above for yaw.
- 
 
 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 
 
- 
 
 50 
 gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
- 
 
 51 
 where V is 2 for FC1.0 and 4 for all others.
Line 36... Line -...
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 
 
 - 
 
50 
  gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
 - 
 
51 
    where V is 2 for FC1.0 and 4 for all others.
 - 
 
52 
 
 52 
 
53 
  Assuming constant ADCValue, in the H&I code:
 - 
 
54 
 
 - 
 
55 
  gyro = I * ADCValue.
 - 
 
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:
 - 
 
Line -... Line 53...
- 
 
 53 
 Assuming constant ADCValue, in the H&I code:
- 
 
 54 
 
- 
 
 55 
 gyro = I * ADCValue.
- 
 
 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)
 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
 82 
 (only about half as sensitive as V1.3. But it will take about twice the
Line 83... Line 83...
83 
     rotation rate!)
 83 
 rotation rate!)
84 
 
 84 
 
85 
  All together: gyro = I * H * rotation rate [units / (deg/s)].
 85 
 All together: gyro = I * H * rotation rate [units / (deg/s)].
86 
*/
 86 
 */
Line 106... Line 106...
106 
 
 106 
 
107 
#define ACC_SUMMATION_FACTOR_PITCHROLL 2
 107 
#define ACC_SUMMATION_FACTOR_PITCHROLL 2
Line 108... Line 108...
108 
#define ACC_SUMMATION_FACTOR_Z 1
 108 
#define ACC_SUMMATION_FACTOR_Z 1
109 
 
 109 
 
110 
/*
 110 
/*
111 
  Integration:
 111 
 Integration:
112 
  The HiResXXXX values are divided by 8 (in H&I firmware) before 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
 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,
 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
 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
 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
 117 
 division by the constant takes place in the flight attitude code, the
118 
  constant is there.
 118 
 constant is there.
119 
 
 119 
 
120 
  The control loop executes every 2ms, and for each iteration
 120 
 The control loop executes every 2ms, and for each iteration
121 
  gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
 121 
 gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
122 
  Assuming a constant rotation rate v and a zero initial gyroIntegral
 122 
 Assuming a constant rotation rate v and a zero initial gyroIntegral
123 
  (for this explanation), we get:
 123 
 (for this explanation), we get:
124 
 
 124 
 
125 
  gyroIntegral =
 125 
 gyroIntegral =
126 
    N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
 126 
 N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
127 
    N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
 127 
 N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
128 
 
 128 
 
129 
  where N is the number of summations; N = t/2ms.
 129 
 where N is the number of summations; N = t/2ms.
130 
 
 130 
 
131 
  For one degree of rotation: t*v = 1:
 131 
 For one degree of rotation: t*v = 1:
132 
 
 132 
 
133 
  gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
 133 
 gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
134 
 
 134 
 
135 
  This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
 135 
 This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
136 
 
 136 
 
137 
  Examples:
 137 
 Examples:
138 
  FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
 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
 139 
 FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
Line 140... Line 140...
140 
  My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
 140 
 My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
141 
*/
 141 
 */
142 
 
 142 
 
143 
/*
 143 
/*
Line 237... Line 237...
237 
/*
 237 
/*
238 
 * Flag: Interrupt handler has done all A/D conversion and processing.
 238 
 * Flag: Interrupt handler has done all A/D conversion and processing.
239 
 */
 239 
 */
240 
extern volatile uint8_t analogDataReady;
 240 
extern volatile uint8_t analogDataReady;
Line 241... Line -...
241 
 
 - 
 
242 
 
 241 
 
Line 243... Line 242...
243 
void analog_init(void);
 242 
void analog_init(void);
244 
 
 243