Subversion Repositories FlightCtrl

Rev

Rev 2096 | Rev 2102 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2096 Rev 2099
Line 20... Line 20...
20 
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
 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
 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
 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.
 23 
 roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
24 
*/
 24 
*/
25 
#define GYRO_FACTOR_PITCHROLL 1
 - 
 
Line 26... Line 25...
26 
 
 25 
 
27 
/*
 26 
/*
28 
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
 27 
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
29 
 ADCValue [units] =
 28 
 ADCValue [units] =
Line 57... Line 56...
57 
/*
 56 
/*
58 
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
 57 
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
59 
 * respectively. This is = the number of occurences of each channel in the
 58 
 * respectively. This is = the number of occurences of each channel in the
60 
 * channelsForStates array in analog.c.
 59 
 * channelsForStates array in analog.c.
61 
 */
 60 
 */
62 
#define GYRO_OVERSAMPLING_PITCHROLL 4
 - 
 
63 
#define GYRO_OVERSAMPLING_YAW 2
 61 
#define GYRO_OVERSAMPLING 4
Line 64... Line 62...
64 
 
 62 
 
65 
#define ACC_OVERSAMPLING_XY 2
 63 
//#define ACC_OVERSAMPLING_XY 2
Line 66... Line 64...
66 
#define ACC_OVERSAMPLING_Z 1
 64 
//#define ACC_OVERSAMPLING_Z 1
67 
 
 65 
 
68 
/*
 66 
/*
69 
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
 - 
 
70 
 */
 67 
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
Line 71... Line 68...
71 
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
 68 
 */
72 
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
 69 
#define GYRO_RATE_FACTOR (GYRO_HW_FACTOR * GYRO_OVERSAMPLING)
73 
 
 70 
 
74 
/*
 71 
/*
Line 75... Line 72...
75 
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
 72 
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
76 
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
 73 
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
77 
 */
 74 
 */
78 
 
 75 
 
79 
/*
 76 
/*
80 
 * Gyro saturation prevention.
 77 
 * Gyro saturation prevention.
81 
 */
 78 
 */
82 
// How far from the end of its range a gyro is considered near-saturated.
 79 
// How far from the end of its range a gyro is considered near-saturated.
83 
#define SENSOR_MIN_PITCHROLL 32
 80 
#define SENSOR_MIN 32
84 
// Other end of the range (calculated)
 81 
// Other end of the range (calculated)
85 
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
 82 
#define SENSOR_MAX (GYRO_OVERSAMPLING * 1023 - SENSOR_MIN)
Line 86... Line 83...
86 
// Max. boost to add "virtually" to gyro signal at total saturation.
 83 
// Max. boost to add "virtually" to gyro signal at total saturation.
87 
#define EXTRAPOLATION_LIMIT 2500
 84 
#define EXTRAPOLATION_LIMIT 2500
88 
// Slope of the boost (calculated)
 85 
// Slope of the boost (calculated)
89 
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
 86 
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN)
90 
 
 87 
 
Line 91... Line 88...
91 
/*
 88 
/*
92 
 * This value is subtracted from the gyro noise measurement in each iteration,
 89 
 * This value is subtracted from the gyro noise measurement in each iteration,
93 
 * making it return towards zero.
 90 
 * making it return towards zero.
94 
 */
 91 
 */
95 
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
 92 
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
96 
 
 93 
 
97 
#define PITCH 0
 94 
#define PITCH 0
98 
#define ROLL 1
 95 
#define ROLL 1
99 
#define YAW 2
 96 
#define YAW 2
100 
#define Z 2
 97 
//#define Z 2
101 
/*
 98 
/*
102 
 * The values that this module outputs
 99 
 * The values that this module outputs
103 
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
 100 
 * 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
 101 
 * 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
 102 
 * integration to angles. For the same axis, the PID and ATT variables
106 
 * generally have about the same values. There are just some differences
 103 
 * generally have about the same values. There are just some differences
- 
 
 104 
 * in filtering, and when a gyro becomes near saturated.
107 
 * in filtering, and when a gyro becomes near saturated.
 105 
 * Maybe this distinction is not really necessary.
108 
 * Maybe this distinction is not really necessary.
 - 
 
109 
 */
 106 
 */
Line 110... Line 107...
110 
extern int16_t gyro_PID[2];
 107 
extern int16_t gyro_PID[3];
111 
extern int16_t gyro_ATT[2];
 108 
extern int16_t gyro_ATT[3];
Line 112... Line 109...
112 
#define GYRO_D_WINDOW_LENGTH 8
 109 
#define GYRO_D_WINDOW_LENGTH 8
113 
extern int16_t gyroD[3];
 110 
 
114 
extern int16_t yawGyro;
 111 
extern int16_t gyroD[3];
Line 115... Line 112...
115 
extern int16_t UBat;
 112 
extern int16_t UBat;
116 
 
 113 
 
117 
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
 114 
// 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))
 115 
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
Line 119... Line 116...
119 
 
 116 
 
120 
extern sensorOffset_t gyroOffset;
 117 
extern sensorOffset_t gyroOffset;
121 
extern sensorOffset_t accOffset;
 118 
//extern sensorOffset_t accOffset;
122 
extern sensorOffset_t gyroAmplifierOffset;
 119 
extern sensorOffset_t gyroAmplifierOffset;
123 
 
 120 
 
124 
/*
 121 
/*
Line 125... Line 122...
125 
 * This is not really for external use - but the ENC-03 gyro modules needs it.
 122 
 * This is not really for external use - but the ENC-03 gyro modules needs it.
126 
 */
 123 
 */
127 
//extern volatile int16_t rawGyroSum[3];
 124 
//extern volatile int16_t rawGyroSum[3];
Line 231... Line 228...
231 
void analog_calibrateGyros(void);
 228 
void analog_calibrateGyros(void);
Line 232... Line 229...
232 
 
 229 
 
233 
/*
 230 
/*
234 
 * Zero-offset accelerometers and write the calibration data to EEPROM.
 231 
 * Zero-offset accelerometers and write the calibration data to EEPROM.
235 
 */
 232 
 */
Line 236... Line 233...
236 
void analog_calibrateAcc(void);
 233 
//void analog_calibrateAcc(void);
Line 237... Line 234...
237 
 
 234