| 0,0 → 1,149 |
|---|
| /*####################################################################################### |
| Attitude sense system (processing of gyro, accelerometer and altimeter data) |
| #######################################################################################*/ |
| |
| #ifndef _ATTITUDE_H |
| #define _ATTITUDE_H |
| |
| #include <inttypes.h> |
| |
| #include "analog.h" |
| |
| // For debugging only. |
| #include "uart0.h" |
| |
| /* |
| * If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
| * acc. sensor to be ignored at attitude calibration. |
| */ |
| #define ATTITUDE_USE_ACC_SENSORS yes |
| |
| /* |
| * Default hysteresis to use for the -180 degree to 180 degree wrap. |
| */ |
| #define PITCHOVER_HYSTERESIS 0L |
| #define ROLLOVER_HYSTERESIS 0L |
| |
| /* |
| * The frequency at which numerical integration takes place. 488 in original code. |
| */ |
| #define INTEGRATION_FREQUENCY 488 |
| |
| /* |
| * Gyro readings are divided by this before being used in attitude control. This will scale them |
| * to match the scale of the stick control etc. variables. This is just a rough non-precision |
| * scaling - the below definitions make precise conversion factors. |
| * Will be about 4 for InvenSense, 8 for FC1.3 and 13 for ADXRS610 (hmmm - originally is was only 8???) |
| * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
| */ |
| #define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
| |
| /* |
| * Constant for deriving an attitude angle from acceleration measurement. |
| * |
| * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
| * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
| * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
| * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
| * In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
| * The sine of v is the far side / the hypothenusis: |
| * sin v = acc / sqrt(acc^2 + acc_z^2) |
| * Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
| * sin v ~= acc / acc_z |
| * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
| * and sin v ~= v (small angles, in radians): |
| * sin v ~= acc / 410 |
| * v / 57.3 ~= acc / 410 |
| * v ~= acc * 57.3 / 410 |
| * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
| * |
| * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
| */ |
| #define DEG_ACC_FACTOR 7 |
| |
| /* |
| * Growth of the integral per degree: |
| * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
| * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
| * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
| */ |
| #define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
| #define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
| |
| /* |
| * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
| * = the factor an acc. sensor should be multiplied by to get the gyro integral |
| * value for the same (small) angle. |
| * The value is about 200. |
| */ |
| #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| |
| /* |
| * Rotation rates |
| */ |
| extern int16_t pitchRate_PID, rollRate_PID, yawRate; |
| extern int16_t pitchDifferential, rollDifferential; |
| |
| /* |
| * Attitudes calcualted by numerical integration of gyro rates |
| */ |
| extern int32_t pitchAngle, rollAngle, yawAngle; |
| |
| // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| |
| /* |
| * Compass navigation |
| */ |
| extern int16_t compassHeading; |
| extern int16_t compassCourse; |
| extern int16_t compassOffCourse; |
| extern uint8_t compassCalState; |
| extern int32_t yawGyroHeading; |
| extern int16_t yawGyroHeadingInDeg; |
| extern uint16_t updateCompassCourse; |
| extern uint16_t badCompassHeading; |
| |
| /* |
| * Height control |
| */ |
| extern int readingHeight; |
| extern int setPointHeight; |
| |
| /* |
| * Interval wrap-over values for attitude integrals |
| */ |
| extern long turnOver180Pitch, turnOver180Roll; |
| |
| // No longer used. |
| // extern uint8_t FunnelCourse; |
| |
| /* |
| * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
| * to help cancelling out drift and vibration noise effects. The dynamic offsets themselves |
| * can be updated in flight by different ways, for example: |
| * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
| * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
| * integration, and then adding the sum / n to the dynamic offset |
| * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
| * - Invent your own... |
| */ |
| extern int16_t dynamicOffsetPitch, dynamicOffsetRoll, dynamicOffsetYaw; |
| // extern int16_t savedDynamicOffsetPitch, savedDynamicOffsetRoll; |
| |
| /* |
| * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
| * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
| */ |
| void attitude_setNeutral(void); |
| |
| /* |
| * Experiment. |
| */ |
| // void attitude_startDynamicCalibration(void); |
| // void attitude_continueDynamicCalibration(void); |
| |
| /* |
| * Main routine, called from the flight loop. |
| */ |
| void calculateFlightAttitude(void); |
| #endif //_ATTITUDE_H |