Subversion Repositories FlightCtrl

Rev

Rev 2106 | Rev 2133 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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