Subversion Repositories FlightCtrl

Rev

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

Rev 2048 Rev 2049
Line 31... Line 31...
31 
 
 31 
 
32 
/*
 32 
/*
33 
 * Gyro readings are divided by this before being used in attitude control. This will scale them
 33 
 * Gyro readings are divided by this before being used in attitude control. This will scale them
34 
 * to match the scale of the stick control etc. variables. This is just a rough non-precision
 34 
 * to match the scale of the stick control etc. variables. This is just a rough non-precision
35 
 * scaling - the below definitions make precise conversion factors.
 - 
 
36 
 * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
 - 
 
37 
 * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
 35 
 * scaling - the below definitions make precise conversion factors.
38 
 */
 36 
 */
39 
#define HIRES_GYRO_INTEGRATION_FACTOR 1
 37 
#define HIRES_GYRO_INTEGRATION_FACTOR 1
Line 40... Line 38...
40 
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
 38 
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
- 
 
 39 
 
- 
 
 40 
/*
- 
 
 41 
 Gyro integration:
- 
 
 42 
 
- 
 
 43 
 The control loop executes at INTEGRATION_FREQUENCY Hz, and for each iteration
- 
 
 44 
 gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
- 
 
 45 
 Assuming a constant rotation rate v and a zero initial gyroIntegral
- 
 
 46 
 (for this explanation), we get:
- 
 
 47 
 
- 
 
 48 
 gyroIntegral =
- 
 
 49 
 t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
- 
 
 50 
 
- 
 
 51 
 For one degree of rotation: t*v = 1:
- 
 
 52 
 
- 
 
 53 
 gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
- 
 
 54 
 
- 
 
 55 
 This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.
- 
 
 56 
 
- 
 
 57 
 Examples:
- 
 
 58 
 FC1.3:                 GYRO_DEG_FACTOR_PITCHROLL = 2545
- 
 
 59 
 FC2.0:                 GYRO_DEG_FACTOR_PITCHROLL = 2399
- 
 
 60 
 My InvenSense copter:  GYRO_DEG_FACTOR_PITCHROLL = 1333
- 
 
 61 
 */
- 
 
 62 
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
- 
 
 63 
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
41 
 
 64 
 
42 
/*
 65 
/*
43 
 * Constant for deriving an attitude angle from acceleration measurement.
 66 
 * Constant for deriving an attitude angle from acceleration measurement.
44 
 *
 67 
 *
45 
 * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
 68 
 * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
Line 60... Line 83...
60 
 *
 83 
 *
61 
 * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
 84 
 * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
62 
 */
 85 
 */
63 
#define DEG_ACC_FACTOR 7
 86 
#define DEG_ACC_FACTOR 7
Line 64... Line -...
64 
 
 - 
 
65 
/*
 - 
 
66 
 * Growth of the integral per degree:
 - 
 
67 
 * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
 - 
 
68 
 * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration.
 - 
 
69 
 * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable).
 - 
 
70 
 */
 - 
 
71 
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
 - 
 
Line 72... Line 87...
72 
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
 87 
 
73 
 
 88 
 
74 
/*
 89 
/*
75 
 * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
 90 
 * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value