Subversion Repositories FlightCtrl

Rev

Rev 2135 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2108 - 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"
2108 - 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
 */
20
//#define ATTITUDE_USE_ACC_SENSORS yes
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)
2108 - 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
 */
56
#define GYRO_DEG_FACTOR (GYRO_RATE_FACTOR * INTEGRATION_FREQUENCY * GYRO_CORRECTION)
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
 */
64
//#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
65
 
2141 - 66
#define OVER180 ((int32_t)(GYRO_DEG_FACTOR * 180.0f))
67
#define OVER360 ((int32_t)(GYRO_DEG_FACTOR * 360.0f))
2108 - 68
 
69
/*
70
 * Rotation rates
71
 */
72
extern int16_t rate_PID[3], rate_ATT[3];
73
extern int16_t differential[3];
74
 
75
/*
76
 * Attitudes calculated by numerical integration of gyro rates
77
 */
78
extern int32_t attitude[3];
79
 
80
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
81
 
82
/*
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
/*
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
 */
104
// extern int16_t dynamicOffset[2], dynamicOffsetYaw;
105
 
106
/*
107
 * For NaviCtrl use.
108
 */
109
// extern int16_t averageAcc[2], averageAccCount;
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