Subversion Repositories FlightCtrl

Rev

Rev 1874 | Rev 2035 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1645 - 1
/*********************************************************************************/
2
/* Attitude sense system (processing of gyro, accelerometer and altimeter data)  */
3
/*********************************************************************************/
1612 dongfang 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
 * Default hysteresis to use for the -180 degree to 180 degree wrap.
23
 */
24
#define PITCHOVER_HYSTERESIS 0L
25
#define ROLLOVER_HYSTERESIS 0L
26
 
27
/*
28
 * The frequency at which numerical integration takes place. 488 in original code.
29
 */
30
#define INTEGRATION_FREQUENCY 488
31
 
32
/*
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
35
 * scaling - the below definitions make precise conversion factors.
1645 - 36
 * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
1612 dongfang 37
 * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
38
 */
1874 - 39
#define HIRES_GYRO_INTEGRATION_FACTOR 1
40
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
1612 dongfang 41
 
42
/*
43
 * Constant for deriving an attitude angle from acceleration measurement.
44
 *
45
 * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
46
 * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
47
 * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
48
 * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
49
 * In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
50
 * The sine of v is the far side / the hypothenusis:
51
 * sin v = acc / sqrt(acc^2 + acc_z^2)
52
 * Using that v is a small angle, and the near side is about equal to the the hypothenusis:
53
 * sin v ~= acc / acc_z
54
 * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
55
 * and sin v ~= v (small angles, in radians):
56
 * sin v ~= acc / 410
57
 * v / 57.3 ~= acc / 410
58
 * v ~= acc * 57.3 / 410
59
 * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
60
 *
61
 * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
62
 */
63
#define DEG_ACC_FACTOR 7
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
 */
1872 - 71
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
72
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
1612 dongfang 73
 
74
/*
75
 * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
76
 * = the factor an acc. sensor should be multiplied by to get the gyro integral
77
 * value for the same (small) angle.
78
 * The value is about 200.
79
 */
80
#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
81
 
82
/*
83
 * Rotation rates
84
 */
1775 - 85
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
1645 - 86
extern int16_t differential[2];
1612 dongfang 87
 
88
/*
1645 - 89
 * Attitudes calculated by numerical integration of gyro rates
1612 dongfang 90
 */
1775 - 91
extern int32_t angle[2], yawAngleDiff;
1612 dongfang 92
 
93
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
94
 
95
/*
96
 * Compass navigation
97
 */
98
extern int16_t compassHeading;
99
extern int16_t compassCourse;
1805 - 100
// extern int16_t compassOffCourse;
1612 dongfang 101
extern uint8_t compassCalState;
102
extern int32_t yawGyroHeading;
103
extern int16_t yawGyroHeadingInDeg;
1805 - 104
extern uint8_t updateCompassCourse;
105
extern uint16_t ignoreCompassTimer;
1980 - 106
extern uint16_t accVector;
1612 dongfang 107
 
1775 - 108
void updateCompass(void);
109
 
1612 dongfang 110
/*
111
 * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
1872 - 112
 * to help canceling out drift and vibration noise effects. The dynamic offsets themselves
1612 dongfang 113
 * can be updated in flight by different ways, for example:
114
 * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
115
 * - Summing up how much acc. meter correction was done to the gyro integrals over the last n
116
 *   integration, and then adding the sum / n to the dynamic offset
117
 * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
118
 * - Invent your own...
119
 */
1645 - 120
extern int16_t dynamicOffset[2], dynamicOffsetYaw;
1612 dongfang 121
 
122
/*
1775 - 123
 * For NaviCtrl use.
124
 */
125
extern int16_t averageAcc[2], averageAccCount;
126
 
127
/*
1612 dongfang 128
 * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
129
 * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
130
 */
131
void attitude_setNeutral(void);
132
 
133
/*
134
 * Experiment.
135
 */
136
// void attitude_startDynamicCalibration(void);
137
// void attitude_continueDynamicCalibration(void);
138
 
1775 - 139
int32_t getAngleEstimateFromAcc(uint8_t axis);
140
 
1612 dongfang 141
/*
142
 * Main routine, called from the flight loop.
143
 */
144
void calculateFlightAttitude(void);
145
#endif //_ATTITUDE_H