Subversion Repositories FlightCtrl

Rev

Rev 1926 | Go to most recent revision | 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"
11 
 
12 
// For debugging only.
13 
#include "uart0.h"
14 
 
15 
/*
16 
 * If you have no acc. sensor or do not want to use it, remove this define. This will cause the
17 
 * acc. sensor to be ignored at attitude calibration.
18 
 */
19 
#define ATTITUDE_USE_ACC_SENSORS yes
20 
 
21 
/*
22 
 * The frequency at which numerical integration takes place. 488 in original code.
23 
 */
24 
#define INTEGRATION_FREQUENCY 488
25 
 
26 
/*
27 
 * Gyro readings are divided by this before being used in attitude control. This will scale them
28 
 * to match the scale of the stick control etc. variables. This is just a rough non-precision
29 
 * scaling - the below definitions make precise conversion factors.
30 
 * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
31 
 * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
32 
 */
33 
#define HIRES_GYRO_INTEGRATION_FACTOR 1
34 
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
35 
 
36 
/*
37 
 * Constant for deriving an attitude angle from acceleration measurement.
38 
 *
39 
 * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
40 
 * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
41 
 * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
42 
 * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
43 
 * In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
44 
 * The sine of v is the far side / the hypothenusis:
45 
 * sin v = acc / sqrt(acc^2 + acc_z^2)
46 
 * Using that v is a small angle, and the near side is about equal to the the hypothenusis:
47 
 * sin v ~= acc / acc_z
48 
 * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
49 
 * and sin v ~= v (small angles, in radians):
50 
 * sin v ~= acc / 410
51 
 * v / 57.3 ~= acc / 410
52 
 * v ~= acc * 57.3 / 410
53 
 * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
54 
 *
55 
 * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
56 
 */
57 
#define DEG_ACC_FACTOR 7
58 
 
59 
/*
60 
 * Growth of the integral per degree:
61 
 * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
62 
 * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration.
63 
 * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable).
64 
 */
65 
#define GYRO_DEG_FACTOR_PITCHROLL (GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
66 
#define GYRO_DEG_FACTOR_YAW (GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
67 
 
68 
/*
69 
 * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
70 
 * = the factor an acc. sensor should be multiplied by to get the gyro integral
71 
 * value for the same (small) angle.
72 
 * The value is about 200.
73 
 */
74 
#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
75 
 
76 
#define PITCHROLLOVER180 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 180)
77 
#define PITCHROLLOVER360 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 360)
78 
#define YAWOVER360       ((int32_t)GYRO_DEG_FACTOR_YAW * 360)
79 
 
80 
/*
81 
 * Rotation rates
82 
 */
83 
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
84 
extern int16_t differential[3];
85 
 
86 
/*
87 
 * Attitudes calculated by numerical integration of gyro rates
88 
 */
1927 - 89 
extern int32_t angle[3];
90 
extern int32_t error[3];
1910 - 91 
 
92 
/*
93 
 * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
94 
 * to help canceling out drift and vibration noise effects. The dynamic offsets themselves
95 
 * can be updated in flight by different ways, for example:
96 
 * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
97 
 * - Summing up how much acc. meter correction was done to the gyro integrals over the last n
98 
 *   integration, and then adding the sum / n to the dynamic offset
99 
 * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
100 
 * - Invent your own...
101 
 */
102 
extern int16_t dynamicOffset[2], dynamicOffsetYaw;
103 
 
104 
/*
105 
 * For NaviCtrl use.
106 
 */
107 
extern int16_t averageAcc[2], averageAccCount;
108 
 
109 
/*
110 
 * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
111 
 * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
112 
 */
113 
void attitude_setNeutral(void);
114 
 
115 
/*
116 
 * Experiment.
117 
 */
118 
// void attitude_startDynamicCalibration(void);
119 
// void attitude_continueDynamicCalibration(void);
120 
 
121 
int32_t getAngleEstimateFromAcc(uint8_t axis);
122 
 
123 
/*
124 
 * Main routine, called from the flight loop.
125 
 */
126 
void calculateFlightAttitude(void);
127 
#endif //_ATTITUDE_H