Rev 2099 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1910 | - | 1 | /*********************************************************************************/ |
| 2 | /* Attitude sense system (processing of gyro, accelerometer and altimeter data) */ |
||
| 3 | /*********************************************************************************/ |
||
| 4 | |||
| 5 | #ifndef _ATTITUDE_H |
||
| 6 | #define _ATTITUDE_H |
||
| 7 | |||
| 8 | #include <inttypes.h> |
||
| 9 | |||
| 10 | #include "analog.h" |
||
| 2141 | - | 11 | #include "timer0.h" |
| 1910 | - | 12 | |
| 13 | // For debugging only. |
||
| 14 | #include "uart0.h" |
||
| 15 | |||
| 16 | /* |
||
| 17 | * If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
||
| 18 | * acc. sensor to be ignored at attitude calibration. |
||
| 19 | */ |
||
| 2099 | - | 20 | //#define ATTITUDE_USE_ACC_SENSORS yes |
| 1910 | - | 21 | |
| 22 | /* |
||
| 23 | * The frequency at which numerical integration takes place. 488 in original code. |
||
| 24 | */ |
||
| 2141 | - | 25 | #define INTEGRATION_FREQUENCY (uint16_t)(F_MAINLOOP) |
| 1910 | - | 26 | |
| 27 | /* |
||
| 28 | * Constant for deriving an attitude angle from acceleration measurement. |
||
| 29 | * |
||
| 30 | * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
||
| 31 | * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
||
| 32 | * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
||
| 33 | * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
||
| 34 | * In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
||
| 35 | * The sine of v is the far side / the hypothenusis: |
||
| 36 | * sin v = acc / sqrt(acc^2 + acc_z^2) |
||
| 37 | * Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
||
| 38 | * sin v ~= acc / acc_z |
||
| 39 | * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
||
| 40 | * and sin v ~= v (small angles, in radians): |
||
| 41 | * sin v ~= acc / 410 |
||
| 42 | * v / 57.3 ~= acc / 410 |
||
| 43 | * v ~= acc * 57.3 / 410 |
||
| 44 | * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
||
| 45 | * |
||
| 46 | * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
||
| 47 | */ |
||
| 48 | #define DEG_ACC_FACTOR 7 |
||
| 49 | |||
| 50 | /* |
||
| 51 | * Growth of the integral per degree: |
||
| 52 | * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
||
| 53 | * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
||
| 54 | * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
||
| 55 | */ |
||
| 2099 | - | 56 | #define GYRO_DEG_FACTOR (GYRO_RATE_FACTOR * INTEGRATION_FREQUENCY * GYRO_CORRECTION) |
| 1910 | - | 57 | |
| 58 | /* |
||
| 59 | * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
||
| 60 | * = the factor an acc. sensor should be multiplied by to get the gyro integral |
||
| 61 | * value for the same (small) angle. |
||
| 62 | * The value is about 200. |
||
| 63 | */ |
||
| 2099 | - | 64 | //#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| 1910 | - | 65 | |
| 2141 | - | 66 | #define OVER180 ((int32_t)(GYRO_DEG_FACTOR * 180.0f)) |
| 67 | #define OVER360 ((int32_t)(GYRO_DEG_FACTOR * 360.0f)) |
||
| 1910 | - | 68 | |
| 69 | /* |
||
| 70 | * Rotation rates |
||
| 71 | */ |
||
| 2099 | - | 72 | extern int16_t rate_PID[3], rate_ATT[3]; |
| 1910 | - | 73 | extern int16_t differential[3]; |
| 74 | |||
| 75 | /* |
||
| 76 | * Attitudes calculated by numerical integration of gyro rates |
||
| 77 | */ |
||
| 2099 | - | 78 | extern int32_t attitude[3]; |
| 1910 | - | 79 | |
| 2099 | - | 80 | // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| 81 | |||
| 1910 | - | 82 | /* |
| 2099 | - | 83 | * Compass navigation |
| 84 | */ |
||
| 85 | // extern int16_t compassHeading; |
||
| 86 | // extern int16_t compassCourse; |
||
| 87 | // extern int16_t compassOffCourse; |
||
| 88 | // extern uint8_t compassCalState; |
||
| 89 | // extern int32_t yawGyroHeading; |
||
| 90 | // extern int16_t yawGyroHeadingInDeg; |
||
| 91 | // extern uint8_t updateCompassCourse; |
||
| 92 | // extern uint16_t ignoreCompassTimer; |
||
| 93 | |||
| 94 | /* |
||
| 1910 | - | 95 | * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
| 96 | * to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
||
| 97 | * can be updated in flight by different ways, for example: |
||
| 98 | * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
||
| 99 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
||
| 100 | * integration, and then adding the sum / n to the dynamic offset |
||
| 101 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
||
| 102 | * - Invent your own... |
||
| 103 | */ |
||
| 2099 | - | 104 | // extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
| 1910 | - | 105 | |
| 106 | /* |
||
| 107 | * For NaviCtrl use. |
||
| 108 | */ |
||
| 2099 | - | 109 | // extern int16_t averageAcc[2], averageAccCount; |
| 1910 | - | 110 | |
| 111 | /* |
||
| 112 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
||
| 113 | * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
||
| 114 | */ |
||
| 115 | void attitude_setNeutral(void); |
||
| 116 | |||
| 117 | /* |
||
| 118 | * Experiment. |
||
| 119 | */ |
||
| 120 | // void attitude_startDynamicCalibration(void); |
||
| 121 | // void attitude_continueDynamicCalibration(void); |
||
| 122 | |||
| 123 | int32_t getAngleEstimateFromAcc(uint8_t axis); |
||
| 124 | |||
| 125 | /* |
||
| 126 | * Main routine, called from the flight loop. |
||
| 127 | */ |
||
| 128 | void calculateFlightAttitude(void); |
||
| 129 | #endif //_ATTITUDE_H |