Subversion Repositories FlightCtrl

Rev

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

Rev 2097 Rev 2189
Line 1... Line 1...
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
#include "configuration.h"
4
#include "configuration.h"
5
 
-
 
6
/*
-
 
7
 About setting constants for different gyros:
-
 
8
 Main parameters are positive directions and voltage/angular speed gain.
-
 
9
 The "Positive direction" is the rotation direction around an axis where
-
 
10
 the corresponding gyro outputs a voltage > the no-rotation voltage.
-
 
11
 A gyro is considered, in this code, to be "forward" if its positive
-
 
12
 direction is:
-
 
13
 - Nose down for pitch
5
#include "Vector3.h"
14
 - Left hand side down for roll
-
 
15
 - Clockwise seen from above for yaw.
-
 
16
 
-
 
17
 Setting gyro gain correctly: All sensor measurements in analog.c take
-
 
18
 place in a cycle, each cycle comprising all sensors. Some sensors are
-
 
19
 sampled more than once (oversampled), and the results added.
-
 
20
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
-
 
21
 or 4 (other versions), offset to zero, low pass filtered and then assigned
-
 
22
 to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
-
 
23
 roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
-
 
24
*/
-
 
25
#define GYRO_FACTOR_PITCHROLL 1
-
 
Line 26... Line 6...
26
 
6
 
27
/*
7
/*
28
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
8
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
29
 ADCValue [units] =
9
 ADCValue [units] =
30
 rotational speed [deg/s] *
10
 rotational speed [rad/s] *
31
 gyro sensitivity [V / deg/s] *
11
 gyro sensitivity [V/rad/s] *
32
 amplifier gain [units] *
12
 amplifier gain [units] *
33
 1024 [units] /
13
 1024 [units] /
Line 34... Line 14...
34
 3V full range [V]
14
 3V full range [V]
35
 
15
 
36
 GYRO_HW_FACTOR is:
16
 GYRO_HW_FACTOR is:
37
 gyro sensitivity [V / deg/s] *
17
 gyro sensitivity [V/rad/s] *
38
 amplifier gain [units] *
18
 amplifier gain [units] *
Line 39... Line 19...
39
 1024 [units] /
19
 1024 [units] /
40
 3V full range [V]
20
 3V full range [V]
41
 
21
 
Line 42... Line 22...
42
 Examples:
22
 Examples:
43
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
23
 FC1.3 has 38.39 mV/rad/s gyros and amplifiers with a gain of 5.7:
Line 44... Line 24...
44
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
24
 GYRO_HW_FACTOR = 0.03839 V/deg/s*5.7*1024/3V = 74.688 units/rad/s.
45
 
25
 
46
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
26
 FC2.0 has 6*(3/5) mV/rad/s gyros (they are ratiometric) and no amplifiers:
47
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
-
 
Line 48... Line 27...
48
 
27
 GYRO_HW_FACTOR = 0.006 V/rad/s*1*1024*3V/5V/3V = 70.405 units/(rad/s).
49
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
28
 
Line -... Line 29...
-
 
29
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
-
 
30
 GYRO_HW_FACTOR = 0.002 V/rad/s*1*1024/3V = 39.1139 units/(rad/s)
-
 
31
 (only about half as sensitive as V1.3. But it will take about twice the rotation rate!)
-
 
32
 
-
 
33
 GYRO_HW_FACTOR is given in the makefile.
50
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
34
*/
51
 (only about half as sensitive as V1.3. But it will take about twice the
35
 
52
 rotation rate!)
36
#define ACCEL_HW_FACTOR 204.8f
53
 
37
 
54
 GYRO_HW_FACTOR is given in the makefile.
38
// If defined, acceleration is scaled to m/s^2. Else it is scaled to g's.
55
*/
39
#define USE_MSSQUARED 1
-
 
40
 
-
 
41
/*
-
 
42
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
56
 
43
 * respectively. This is = the number of occurences of each channel in the
-
 
44
 * channelsForStates array in analog.c.
-
 
45
 */
57
/*
46
#define GYRO_OVERSAMPLING_XY 8
58
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
47
#define GYRO_RATE_FACTOR_XY (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_XY * GYRO_XY_CORRECTION)
59
 * respectively. This is = the number of occurences of each channel in the
48
#define GYRO_RATE_FACTOR_XY_MS (GYRO_RATE_FACTOR_XY * 1000.0)
-
 
49
 
60
 * channelsForStates array in analog.c.
50
#define GYRO_OVERSAMPLING_Z 4
-
 
51
#define GYRO_RATE_FACTOR_Z (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_Z * GYRO_Z_CORRECTION)
61
 */
52
#define GYRO_RATE_FACTOR_Z_MS (GYRO_RATE_FACTOR_Z * 1000.0)
62
#define GYRO_OVERSAMPLING_PITCHROLL 4
53
 
63
#define GYRO_OVERSAMPLING_YAW 2
-
 
64
 
54
#define ACCEL_OVERSAMPLING_XY 4
-
 
55
#define ACCEL_G_FACTOR_XY (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_XY)
65
#define ACC_OVERSAMPLING_XY 2
56
#define ACCEL_M_SSQUARED_FACTOR_XY (ACCEL_G_FACTOR_XY / 9.82f)
66
#define ACC_OVERSAMPLING_Z 1
57
 
67
 
58
#define ACCEL_OVERSAMPLING_Z 2
68
/*
59
 
69
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
60
// number of counts per g
70
 */
61
#define ACCEL_G_FACTOR_Z (ACCEL_HW_FACTOR * ACCEL_OVERSAMPLING_Z)
-
 
62
// number of counts per m/s^2
-
 
63
#define ACCEL_M_SSQUARED_FACTOR_Z (ACCEL_G_FACTOR_Z / 9.82f)
71
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
64
 
72
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
65
#ifdef USE_MSSQUARED
73
 
66
#define ACCEL_FACTOR_XY ACCEL_M_SSQUARED_FACTOR_XY
74
/*
67
#define ACCEL_FACTOR_Z ACCEL_M_SSQUARED_FACTOR_Z
75
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
68
#else
76
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
69
#define ACCEL_FACTOR_XY ACCEL_G_FACTOR_XY
77
 */
70
#define ACCEL_FACTOR_Z ACCEL_G_FACTOR_Z
78
 
71
#endif
79
/*
72
/*
80
 * Gyro saturation prevention.
73
 * Gyro saturation prevention.
81
 */
74
 */
82
// How far from the end of its range a gyro is considered near-saturated.
75
// How far from the end of its range a gyro is considered near-saturated.
83
#define SENSOR_MIN_PITCHROLL 32
-
 
84
// Other end of the range (calculated)
-
 
85
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
-
 
86
// Max. boost to add "virtually" to gyro signal at total saturation.
-
 
87
#define EXTRAPOLATION_LIMIT 2500
-
 
88
// Slope of the boost (calculated)
-
 
Line 89... Line 76...
89
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
76
#define SENSOR_MIN_XY (32 * GYRO_OVERSAMPLING_XY)
90
 
-
 
91
/*
77
// Other end of the range (calculated)
92
 * This value is subtracted from the gyro noise measurement in each iteration,
78
#define SENSOR_MAX_XY (GYRO_OVERSAMPLING_XY * 1023 - SENSOR_MIN_XY)
-
 
79
// Max. boost to add "virtually" to gyro signal at total saturation.
-
 
80
#define EXTRAPOLATION_LIMIT 2500
-
 
81
// Slope of the boost (calculated)
-
 
82
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_XY)
-
 
83
 
-
 
84
#define X 0
-
 
85
#define Y 1
-
 
86
#define Z 2
-
 
87
 
-
 
88
// ADC channels
-
 
89
#define AD_GYRO_Z          0
93
 * making it return towards zero.
90
#define AD_GYRO_X          1
94
 */
91
#define AD_GYRO_Y          2
95
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
92
#define AD_AIRPRESSURE     3
96
 
93
#define AD_UBAT            4
97
#define PITCH 0
94
#define AD_ACCEL_Z         5
98
#define ROLL 1
95
#define AD_ACCEL_Y         6
99
#define YAW 2
96
#define AD_ACCEL_X         7
100
#define Z 2
97
 
101
/*
98
/*
102
 * The values that this module outputs
99
 * The values that this module outputs
-
 
100
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
-
 
101
 * in the attitude control as rotation rates. The "ATT" ones are for
-
 
102
 * integration to angles. For the same axis, the PID and ATT variables
-
 
103
 * generally have about the same values. There are just some differences
103
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
104
 * in filtering, and when a gyro becomes near saturated.
-
 
105
 * Maybe this distinction is not really necessary.
104
 * in the attitude control as rotation rates. The "ATT" ones are for
106
 */
-
 
107
extern Vector3f gyro_attitude;
-
 
108
 
105
 * integration to angles. For the same axis, the PID and ATT variables
109
/*
106
 * generally have about the same values. There are just some differences
-
 
-
 
110
 * The acceleration values that this module outputs. They are zero based.
107
 * in filtering, and when a gyro becomes near saturated.
111
 */
Line 108... Line 112...
108
 * Maybe this distinction is not really necessary.
112
extern Vector3f accel;
109
 */
113
 
Line 110... Line 114...
110
extern int16_t gyro_PID[2];
114
#define GYRO_D_WINDOW_LENGTH 8
111
extern int16_t gyro_ATT[2];
115
 
112
#define GYRO_D_WINDOW_LENGTH 8
116
extern int16_t gyro_control[3];
Line 113... Line 117...
113
extern int16_t gyroD[2];
117
extern int16_t gyroD[2];
-
 
118
 
-
 
119
extern int16_t UBat;
114
extern int16_t yawGyro;
120
 
115
extern int16_t UBat;
121
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
-
 
122
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
-
 
123
 
-
 
124
extern sensorOffset_t gyroOffset;
-
 
125
extern sensorOffset_t accelOffset;
116
 
126
extern sensorOffset_t gyroAmplifierOffset;
Line 117... Line -...
117
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
-
 
118
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
-
 
119
 
-
 
120
extern sensorOffset_t gyroOffset;
-
 
121
extern sensorOffset_t accOffset;
-
 
122
extern sensorOffset_t gyroAmplifierOffset;
127
 
123
 
-
 
124
/*
-
 
125
 * This is not really for external use - but the ENC-03 gyro modules needs it.
-
 
126
 */
-
 
127
//extern volatile int16_t rawGyroSum[3];
-
 
128
 
-
 
129
/*
-
 
130
 * The acceleration values that this module outputs. They are zero based.
128
typedef enum {
Line 131... Line 129...
131
 */
129
    CONTROL_SENSOR_SAMPLING_DATA,
132
extern int16_t acc[3];
130
    CONTROL_SENSOR_DATA_READY,
133
extern int16_t filteredAcc[3];
131
} ControlSensorDataStatus;
134
// extern volatile int32_t stronglyFilteredAcc[3];
132
 
Line 169... Line 167...
169
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
167
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
170
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
168
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
171
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
169
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
172
*/
170
*/
Line 173... Line 171...
173
 
171
 
174
#define AIRPRESSURE_OVERSAMPLING 14
172
#define AIRPRESSURE_OVERSAMPLING 28
175
#define AIRPRESSURE_FILTER 8
173
#define AIRPRESSURE_FILTER 8
176
// Minimum A/D value before a range change is performed.
174
// Minimum A/D value before a range change is performed.
177
#define MIN_RAWPRESSURE (200 * 2)
175
#define MIN_RAWPRESSURE (200 * 2)
178
// Maximum A/D value before a range change is performed.
176
// Maximum A/D value before a range change is performed.
Line 184... Line 182...
184
#define PRESSURE_EXTRAPOLATION_COEFF 25L
182
#define PRESSURE_EXTRAPOLATION_COEFF 25L
185
#define AUTORANGE_WAIT_FACTOR 1
183
#define AUTORANGE_WAIT_FACTOR 1
Line 186... Line 184...
186
 
184
 
Line 187... Line -...
187
#define ABS_ALTITUDE_OFFSET 108205
-
 
188
 
-
 
189
extern uint16_t simpleAirPressure;
-
 
190
/*
-
 
191
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
-
 
192
 */
-
 
193
extern int32_t filteredAirPressure;
-
 
194
 
-
 
195
extern int16_t magneticHeading;
-
 
196
 
-
 
197
extern uint32_t gyroActivity;
-
 
198
 
-
 
199
/*
-
 
200
 * Flag: Interrupt handler has done all A/D conversion and processing.
-
 
201
 */
-
 
202
extern volatile uint8_t analogDataReady;
-
 
203
 
185
#define ABS_ALTITUDE_OFFSET 108205
Line 204... Line 186...
204
 
186
 
205
void analog_init(void);
187
void analog_init(void);
206
 
188
 
207
/*
189
/*
208
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
190
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
Line 209... Line 191...
209
 * for its calibration. The raw value should not be used for anything else.
191
 * for its calibration. The raw value should not be used for anything else.
210
 */
192
 */
211
uint16_t rawGyroValue(uint8_t axis);
193
uint16_t gyroValueForFC13DACCalibration(uint8_t axis);
212
 
194
 
-
 
195
/*
Line 213... Line 196...
213
/*
196
 * Start the conversion cycle. It will stop automatically.
214
 * Start the conversion cycle. It will stop automatically.
-
 
215
 */
197
 */
216
void startAnalogConversionCycle(void);
198
void startADCCycle(void);
-
 
199
void waitADCCycle(uint16_t delay);
-
 
200
 
Line 217... Line 201...
217
 
201
/*
218
/*
202
 */
219
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
203
void analog_updateControlData(void);
220
 */
204
void analog_sumAttitudeData(void);
Line 235... Line 219...
235
 */
219
 */
236
void analog_calibrateAcc(void);
220
void analog_calibrateAcc(void);
Line 237... Line 221...
237
 
221
 
238
 
-
 
239
void analog_setGround(void);
222
 
240
 
223
void analog_setGround(void);
Line 241... Line 224...
241
int32_t analog_getHeight(void);
224
int32_t analog_getHeight(void);