WebSVN - FlightCtrl - Diff - Rev 2052 and 2160 - /branches/dongfang_FC


/*********************************************************************************/
/* Attitude sense system (processing of gyro, accelerometer and altimeter data)  */
/*********************************************************************************/
 
#ifndef _ATTITUDE_H
#define _ATTITUDE_H
 
#include <inttypes.h>
 
#include "analog.h"
#include "timer0.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 F_MAINLOOP
 
/*
 * 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.
 */
#define HIRES_GYRO_INTEGRATION_FACTOR 1
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
 
/*
 Gyro integration:
 
 The control loop executes at INTEGRATION_FREQUENCY Hz, and for each iteration
 gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
 Assuming a constant rotation rate v and a zero initial gyroIntegral
 (for this explanation), we get:
 
 gyroIntegral =
 t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
 
 For one degree of rotation: t*v = 1:
 
 gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
 
 This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.
 
 Examples:
 FC1.3:                 GYRO_DEG_FACTOR_PITCHROLL = 2545
 FC2.0:                 GYRO_DEG_FACTOR_PITCHROLL = 2399
 My InvenSense copter:  GYRO_DEG_FACTOR_PITCHROLL = 1333
 */
//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
 
/*
 * 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 multicopter 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
 
 
/*
 * 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))
 
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
#define YAWOVER180       (GYRO_DEG_FACTOR_YAW * 180L)
#define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)
 
/*
 * Rotation rates
 */
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
extern int16_t differential[2];
 
/*
 * Attitudes calculated by numerical integration of gyro rates
 */
extern int32_t attitude[2];
 
// This is really a flight module thing, but it should be corrected along
// when the yaw angle is corrected from the compass, and that happens here.
// extern int32_t yawAngleDiff;
 
/*
 * Compass navigation
 */
extern int32_t heading;
extern uint16_t ignoreCompassTimer;
extern uint16_t accVector;
 
extern int32_t headingError;
 
 
/*
 * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
 * to help canceling 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 dynamicOffset[2], dynamicOffsetYaw;
 
/*
 * For NaviCtrl use.
 */
extern int16_t averageAcc[2], averageAccCount;
 
/*
 * 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);
 
int32_t getAngleEstimateFromAcc(uint8_t axis);
 
/*
 * Main routine, called from the flight loop.
 */
void calculateFlightAttitude(void);
 
void attitude_resetHeadingToMagnetic(void);
 
#endif //_ATTITUDE_H
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.h @ 2240 - Rev 2052 → 2160

Rev 2052		Rev 2160
1	/*********************************************************************************/	1	/*********************************************************************************/
2	/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */	2	/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */
3	/*********************************************************************************/	3	/*********************************************************************************/
4		4
5	#ifndef _ATTITUDE_H	5	#ifndef _ATTITUDE_H
6	#define _ATTITUDE_H	6	#define _ATTITUDE_H
7		7
8	#include <inttypes.h>	8	#include <inttypes.h>
9		9
10	#include "analog.h"	10	#include "analog.h"
11	#include "timer0.h"	11	#include "timer0.h"
12		12
13	/*	13	/*
14	* If you have no acc. sensor or do not want to use it, remove this define. This will cause the	14	* If you have no acc. sensor or do not want to use it, remove this define. This will cause the
15	* acc. sensor to be ignored at attitude calibration.	15	* acc. sensor to be ignored at attitude calibration.
16	*/	16	*/
17	#define ATTITUDE_USE_ACC_SENSORS yes	17	#define ATTITUDE_USE_ACC_SENSORS yes
18		18
19	/*	19	/*
20	* Default hysteresis to use for the -180 degree to 180 degree wrap.	20	* Default hysteresis to use for the -180 degree to 180 degree wrap.
21	*/	21	*/
22	#define PITCHOVER_HYSTERESIS 0L	22	#define PITCHOVER_HYSTERESIS 0L
23	#define ROLLOVER_HYSTERESIS 0L	23	#define ROLLOVER_HYSTERESIS 0L
24		24
25	/*	25	/*
26	* The frequency at which numerical integration takes place. 488 in original code.	26	* The frequency at which numerical integration takes place. 488 in original code.
27	*/	27	*/
28	#define INTEGRATION_FREQUENCY F_MAINLOOP	28	#define INTEGRATION_FREQUENCY F_MAINLOOP
29		29
30	/*	30	/*
31	* Gyro readings are divided by this before being used in attitude control. This will scale them	31	* Gyro readings are divided by this before being used in attitude control. This will scale them
32	* to match the scale of the stick control etc. variables. This is just a rough non-precision	32	* to match the scale of the stick control etc. variables. This is just a rough non-precision
33	* scaling - the below definitions make precise conversion factors.	33	* scaling - the below definitions make precise conversion factors.
34	*/	34	*/
35	#define HIRES_GYRO_INTEGRATION_FACTOR 1	35	#define HIRES_GYRO_INTEGRATION_FACTOR 1
36	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)	36	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
37		37
38	/*	38	/*
39	Gyro integration:	39	Gyro integration:
40		40
41	The control loop executes at INTEGRATION_FREQUENCY Hz, and for each iteration	41	The control loop executes at INTEGRATION_FREQUENCY Hz, and for each iteration
42	gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].	42	gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
43	Assuming a constant rotation rate v and a zero initial gyroIntegral	43	Assuming a constant rotation rate v and a zero initial gyroIntegral
44	(for this explanation), we get:	44	(for this explanation), we get:
45		45
46	gyroIntegral =	46	gyroIntegral =
47	t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR	47	t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
48		48
49	For one degree of rotation: t*v = 1:	49	For one degree of rotation: t*v = 1:
50		50
51	gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR	51	gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR
52		52
53	This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.	53	This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.
54		54
55	Examples:	55	Examples:
56	FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545	56	FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545
57	FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399	57	FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399
58	My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333	58	My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333
59	*/	59	*/
60	//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time	60	//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time
61	#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)	61	#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
62	#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)	62	#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
63		63
64	/*	64	/*
65	* Constant for deriving an attitude angle from acceleration measurement.	65	* Constant for deriving an attitude angle from acceleration measurement.
66	*	66	*
67	* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.	67	* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
68	* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each	68	* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
69	* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.	69	* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
70	* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:	70	* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
71	* In this explanation it is assumed that the ADC values are 0 based, and gravity is included.	71	* In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
72	* The sine of v is the far side / the hypothenusis:	72	* The sine of v is the far side / the hypothenusis:
73	* sin v = acc / sqrt(acc^2 + acc_z^2)	73	* sin v = acc / sqrt(acc^2 + acc_z^2)
74	* Using that v is a small angle, and the near side is about equal to the the hypothenusis:	74	* Using that v is a small angle, and the near side is about equal to the the hypothenusis:
75	* sin v ~= acc / acc_z	75	* sin v ~= acc / acc_z
76	* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,	76	* Assuming that the multicopter is hovering at small pitch and roll angles, acc_z is about 410,
77	* and sin v ~= v (small angles, in radians):	77	* and sin v ~= v (small angles, in radians):
78	* sin v ~= acc / 410	78	* sin v ~= acc / 410
79	* v / 57.3 ~= acc / 410	79	* v / 57.3 ~= acc / 410
80	* v ~= acc * 57.3 / 410	80	* v ~= acc * 57.3 / 410
81	* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.	81	* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
82	*	82	*
83	* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)	83	* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
84	*/	84	*/
85	#define DEG_ACC_FACTOR 7	85	#define DEG_ACC_FACTOR 7
86		86
87		87
88	/*	88	/*
89	* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value	89	* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
90	* = the factor an acc. sensor should be multiplied by to get the gyro integral	90	* = the factor an acc. sensor should be multiplied by to get the gyro integral
91	* value for the same (small) angle.	91	* value for the same (small) angle.
92	* The value is about 200.	92	* The value is about 200.
93	*/	93	*/
94	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))	94	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
95		95
96	#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)	96	#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
97	#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)	97	#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
98	#define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L)	98	#define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L)
99	#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)	99	#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)
100		100
101	/*	101	/*
102	* Rotation rates	102	* Rotation rates
103	*/	103	*/
104	extern int16_t rate_PID[2], rate_ATT[2], yawRate;	104	extern int16_t rate_PID[2], rate_ATT[2], yawRate;
105	extern int16_t differential[2];	105	extern int16_t differential[2];
106		106
107	/*	107	/*
108	* Attitudes calculated by numerical integration of gyro rates	108	* Attitudes calculated by numerical integration of gyro rates
109	*/	109	*/
110	extern int32_t attitude[2];	110	extern int32_t attitude[2];
111		111
112	// This is really a flight module thing, but it should be corrected along	112	// This is really a flight module thing, but it should be corrected along
113	// when the yaw angle is corrected from the compass, and that happens here.	113	// when the yaw angle is corrected from the compass, and that happens here.
114	// extern int32_t yawAngleDiff;	114	// extern int32_t yawAngleDiff;
115		115
116	/*	116	/*
117	* Compass navigation	117	* Compass navigation
118	*/	118	*/
119	extern int32_t heading;	119	extern int32_t heading;
120	extern uint16_t ignoreCompassTimer;	120	extern uint16_t ignoreCompassTimer;
121	extern uint16_t accVector;	121	extern uint16_t accVector;
122		122
123	extern int32_t headingError;	123	extern int32_t headingError;
124		124
125		125
126	/*	126	/*
127	* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,	127	* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
128	* to help canceling out drift and vibration noise effects. The dynamic offsets themselves	128	* to help canceling out drift and vibration noise effects. The dynamic offsets themselves
129	* can be updated in flight by different ways, for example:	129	* can be updated in flight by different ways, for example:
130	* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool	130	* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
131	* - Summing up how much acc. meter correction was done to the gyro integrals over the last n	131	* - Summing up how much acc. meter correction was done to the gyro integrals over the last n
132	* integration, and then adding the sum / n to the dynamic offset	132	* integration, and then adding the sum / n to the dynamic offset
133	* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that	133	* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
134	* - Invent your own...	134	* - Invent your own...
135	*/	135	*/
136	extern int16_t dynamicOffset[2], dynamicOffsetYaw;	136	extern int16_t dynamicOffset[2], dynamicOffsetYaw;
137		137
138	/*	138	/*
139	* For NaviCtrl use.	139	* For NaviCtrl use.
140	*/	140	*/
141	extern int16_t averageAcc[2], averageAccCount;	141	extern int16_t averageAcc[2], averageAccCount;
142		142
143	/*	143	/*
144	* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).	144	* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
145	* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).	145	* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
146	*/	146	*/
147	void attitude_setNeutral(void);	147	void attitude_setNeutral(void);
148		148
149	/*	149	/*
150	* Experiment.	150	* Experiment.
151	*/	151	*/
152	// void attitude_startDynamicCalibration(void);	152	// void attitude_startDynamicCalibration(void);
153	// void attitude_continueDynamicCalibration(void);	153	// void attitude_continueDynamicCalibration(void);
154		154
155	int32_t getAngleEstimateFromAcc(uint8_t axis);	155	int32_t getAngleEstimateFromAcc(uint8_t axis);
156		156
157	/*	157	/*
158	* Main routine, called from the flight loop.	158	* Main routine, called from the flight loop.
159	*/	159	*/
160	void calculateFlightAttitude(void);	160	void calculateFlightAttitude(void);
161		161
162	void attitude_resetHeadingToMagnetic(void);	162	void attitude_resetHeadingToMagnetic(void);
163		163
164	#endif //_ATTITUDE_H	164	#endif //_ATTITUDE_H
165		165