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