Subversion Repositories FlightCtrl

Rev

Rev 2049 | Rev 2055 | 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>
1965 - 4
#include "configuration.h"
1612 dongfang 5
 
6
/*
1821 - 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
14
 - Left hand side down for roll
15
 - Clockwise seen from above for yaw.
2018 - 16
 
1821 - 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
2049 - 19
 sampled more than once (oversampled), and the results added.
1821 - 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
2049 - 23
 roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
24
*/
25
#define GYRO_FACTOR_PITCHROLL 1
1821 - 26
 
2049 - 27
/*
28
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
1821 - 29
 ADCValue [units] =
30
 rotational speed [deg/s] *
31
 gyro sensitivity [V / deg/s] *
32
 amplifier gain [units] *
33
 1024 [units] /
34
 3V full range [V]
1612 dongfang 35
 
2049 - 36
 GYRO_HW_FACTOR is:
1821 - 37
 gyro sensitivity [V / deg/s] *
38
 amplifier gain [units] *
39
 1024 [units] /
40
 3V full range [V]
1612 dongfang 41
 
1821 - 42
 Examples:
43
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
2049 - 44
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
45
 
1821 - 46
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
2049 - 47
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
48
 
1821 - 49
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
2049 - 50
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
1821 - 51
 (only about half as sensitive as V1.3. But it will take about twice the
52
 rotation rate!)
1612 dongfang 53
 
2049 - 54
 GYRO_HW_FACTOR is given in the makefile.
55
*/
1612 dongfang 56
 
57
/*
2049 - 58
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
1612 dongfang 59
 * respectively. This is = the number of occurences of each channel in the
60
 * channelsForStates array in analog.c.
61
 */
2019 - 62
#define GYRO_OVERSAMPLING_PITCHROLL 4
63
#define GYRO_OVERSAMPLING_YAW 2
1612 dongfang 64
 
2019 - 65
#define ACC_OVERSAMPLING_XY 2
66
#define ACC_OVERSAMPLING_Z 1
1646 - 67
 
1612 dongfang 68
/*
2049 - 69
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
1821 - 70
 */
2049 - 71
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
72
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
1821 - 73
 
1612 dongfang 74
/*
1645 - 75
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
1612 dongfang 76
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
77
 */
78
 
79
/*
1645 - 80
 * Gyro saturation prevention.
81
 */
82
// 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)
2019 - 85
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
1645 - 86
// Max. boost to add "virtually" to gyro signal at total saturation.
87
#define EXTRAPOLATION_LIMIT 2500
88
// Slope of the boost (calculated)
89
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
90
 
91
/*
1612 dongfang 92
 * This value is subtracted from the gyro noise measurement in each iteration,
93
 * making it return towards zero.
94
 */
95
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
96
 
1645 - 97
#define PITCH 0
98
#define ROLL 1
1646 - 99
#define YAW 2
100
#define Z 2
1612 dongfang 101
/*
102
 * The values that this module outputs
1645 - 103
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
104
 * in the attitude control as rotation rates. The "ATT" ones are for
105
 * integration to angles. For the same axis, the PID and ATT variables
106
 * generally have about the same values. There are just some differences
107
 * in filtering, and when a gyro becomes near saturated.
108
 * Maybe this distinction is not really necessary.
1612 dongfang 109
 */
2015 - 110
extern int16_t gyro_PID[2];
111
extern int16_t gyro_ATT[2];
112
extern int16_t gyroD[2];
113
extern int16_t yawGyro;
1612 dongfang 114
extern volatile uint16_t ADCycleCount;
2015 - 115
extern int16_t UBat;
1612 dongfang 116
 
1775 - 117
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
1869 - 118
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
1775 - 119
 
1969 - 120
extern sensorOffset_t gyroOffset;
121
extern sensorOffset_t accOffset;
122
extern sensorOffset_t gyroAmplifierOffset;
1960 - 123
 
1612 dongfang 124
/*
125
 * This is not really for external use - but the ENC-03 gyro modules needs it.
126
 */
2015 - 127
//extern volatile int16_t rawGyroSum[3];
1612 dongfang 128
 
129
/*
1645 - 130
 * The acceleration values that this module outputs. They are zero based.
1612 dongfang 131
 */
2015 - 132
extern int16_t acc[3];
133
extern int16_t filteredAcc[3];
1872 - 134
// extern volatile int32_t stronglyFilteredAcc[3];
1612 dongfang 135
 
136
/*
1775 - 137
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
138
 * only really reflect the noise level when the copter stands still but with
139
 * its motors running.
140
 */
2015 - 141
extern uint16_t gyroNoisePeak[3];
142
extern uint16_t accNoisePeak[3];
1775 - 143
 
144
/*
145
 * Air pressure.
1961 - 146
 * The sensor has a sensitivity of 45 mV/kPa.
1970 - 147
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
148
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
149
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
150
 * h = (101.3 - p[kPa])/0.01195
151
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
152
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
153
 
154
Display pressures
155
4165 mV-->1084.7
156
4090 mV-->1602.4   517.7
157
3877 mV-->3107.8  1503.4
158
 
159
4165 mV-->1419.1
160
3503 mV-->208.1
161
Diff.:   1211.0
162
 
163
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
164
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
165
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
166
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
167
 
168
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
169
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
171
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
172
*/
173
 
2019 - 174
#define AIRPRESSURE_OVERSAMPLING 14
1775 - 175
#define AIRPRESSURE_FILTER 8
176
// Minimum A/D value before a range change is performed.
177
#define MIN_RAWPRESSURE (200 * 2)
178
// Maximum A/D value before a range change is performed.
179
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
180
 
1796 - 181
#define MIN_RANGES_EXTRAPOLATION 15
182
#define MAX_RANGES_EXTRAPOLATION 240
1775 - 183
 
184
#define PRESSURE_EXTRAPOLATION_COEFF 25L
185
#define AUTORANGE_WAIT_FACTOR 1
186
 
1970 - 187
#define ABS_ALTITUDE_OFFSET 108205
188
 
2015 - 189
extern uint16_t simpleAirPressure;
1775 - 190
/*
191
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
192
 */
2015 - 193
extern int32_t filteredAirPressure;
1775 - 194
 
2051 - 195
extern int16_t magneticHeading;
196
 
1775 - 197
/*
1612 dongfang 198
 * Flag: Interrupt handler has done all A/D conversion and processing.
199
 */
200
extern volatile uint8_t analogDataReady;
201
 
2051 - 202
 
1612 dongfang 203
void analog_init(void);
204
 
1952 - 205
/*
2015 - 206
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
207
 * for its calibration. The raw value should not be used for anything else.
208
 */
209
uint16_t rawGyroValue(uint8_t axis);
210
 
211
/*
1952 - 212
 * Start the conversion cycle. It will stop automatically.
213
 */
214
void startAnalogConversionCycle(void);
1612 dongfang 215
 
1952 - 216
/*
217
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
218
 */
1955 - 219
void analog_update(void);
1612 dongfang 220
 
221
/*
1961 - 222
 * Read gyro and acc.meter calibration from EEPROM.
1612 dongfang 223
 */
1961 - 224
void analog_setNeutral(void);
1612 dongfang 225
 
226
/*
1961 - 227
 * Zero-offset gyros and write the calibration data to EEPROM.
1612 dongfang 228
 */
1961 - 229
void analog_calibrateGyros(void);
230
 
231
/*
232
 * Zero-offset accelerometers and write the calibration data to EEPROM.
233
 */
1612 dongfang 234
void analog_calibrateAcc(void);
2033 - 235
 
236
 
2035 - 237
void analog_setGround(void);
2033 - 238
 
239
int32_t analog_getHeight(void);
240
int16_t analog_getDHeight(void);
241
 
1612 dongfang 242
#endif //_ANALOG_H