WebSVN - FlightCtrl - Diff - Rev 2051 and 2052 - /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 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
 
 
/*
 * 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 - Rev 2051 → 2052

Rev 2051		Rev 2052
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		-
12	// For debugging only.	-
13	#include "uart0.h"	11	#include "timer0.h"
14		12
15	/*	13	/*
16	* 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
17	* acc. sensor to be ignored at attitude calibration.	15	* acc. sensor to be ignored at attitude calibration.
18	*/	16	*/
19	#define ATTITUDE_USE_ACC_SENSORS yes	17	#define ATTITUDE_USE_ACC_SENSORS yes
20		18
21	/*	19	/*
22	* 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.
23	*/	21	*/
24	#define PITCHOVER_HYSTERESIS 0L	22	#define PITCHOVER_HYSTERESIS 0L
25	#define ROLLOVER_HYSTERESIS 0L	23	#define ROLLOVER_HYSTERESIS 0L
26		24
27	/*	25	/*
28	* 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.
29	*/	27	*/
30	#define INTEGRATION_FREQUENCY 488	28	#define INTEGRATION_FREQUENCY F_MAINLOOP
31		29
32	/*	30	/*
33	* 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
34	* 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
35	* scaling - the below definitions make precise conversion factors.	33	* scaling - the below definitions make precise conversion factors.
36	*/	34	*/
37	#define HIRES_GYRO_INTEGRATION_FACTOR 1	35	#define HIRES_GYRO_INTEGRATION_FACTOR 1
38	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)	36	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
39		37
40	/*	38	/*
41	Gyro integration:	39	Gyro integration:
42		40
43	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
44	gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].	42	gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
45	Assuming a constant rotation rate v and a zero initial gyroIntegral	43	Assuming a constant rotation rate v and a zero initial gyroIntegral
46	(for this explanation), we get:	44	(for this explanation), we get:
47		45
48	gyroIntegral =	46	gyroIntegral =
49	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
50		48
51	For one degree of rotation: t*v = 1:	49	For one degree of rotation: t*v = 1:
52		50
53	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
54		52
55	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.
56		54
57	Examples:	55	Examples:
58	FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545	56	FC1.3: GYRO_DEG_FACTOR_PITCHROLL = 2545
59	FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399	57	FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399
60	My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333	58	My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333
61	*/	59	*/
-		60	//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time
62	#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)
63	#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)
64		63
65	/*	64	/*
66	* Constant for deriving an attitude angle from acceleration measurement.	65	* Constant for deriving an attitude angle from acceleration measurement.
67	*	66	*
68	* 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.
69	* 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
70	* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.	69	* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
71	* 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:
72	* 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.
73	* The sine of v is the far side / the hypothenusis:	72	* The sine of v is the far side / the hypothenusis:
74	* sin v = acc / sqrt(acc^2 + acc_z^2)	73	* sin v = acc / sqrt(acc^2 + acc_z^2)
75	* 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:
76	* sin v ~= acc / acc_z	75	* sin v ~= acc / acc_z
77	* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,	76	* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
78	* and sin v ~= v (small angles, in radians):	77	* and sin v ~= v (small angles, in radians):
79	* sin v ~= acc / 410	78	* sin v ~= acc / 410
80	* v / 57.3 ~= acc / 410	79	* v / 57.3 ~= acc / 410
81	* v ~= acc * 57.3 / 410	80	* v ~= acc * 57.3 / 410
82	* 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.
83	*	82	*
84	* 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)
85	*/	84	*/
86	#define DEG_ACC_FACTOR 7	85	#define DEG_ACC_FACTOR 7
87		86
88		87
89	/*	88	/*
90	* 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
91	* = 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
92	* value for the same (small) angle.	91	* value for the same (small) angle.
93	* The value is about 200.	92	* The value is about 200.
94	*/	93	*/
95	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))	94	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
96		95
97	#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)	96	#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
98	#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)	97	#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
99	#define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L)	98	#define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L)
100	#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)	99	#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L)
101		100
102	/*	101	/*
103	* Rotation rates	102	* Rotation rates
104	*/	103	*/
105	extern int16_t rate_PID[2], rate_ATT[2], yawRate;	104	extern int16_t rate_PID[2], rate_ATT[2], yawRate;
106	extern int16_t differential[2];	105	extern int16_t differential[2];
107		106
108	/*	107	/*
109	* Attitudes calculated by numerical integration of gyro rates	108	* Attitudes calculated by numerical integration of gyro rates
110	*/	109	*/
111	extern int32_t attitude[2];	110	extern int32_t attitude[2];
112		111
113	// 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
114	// 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.
115	// extern int32_t yawAngleDiff;	114	// extern int32_t yawAngleDiff;
116		115
117	/*	116	/*
118	* Compass navigation	117	* Compass navigation
119	*/	118	*/
120	extern int32_t heading;	119	extern int32_t heading;
121	extern uint16_t ignoreCompassTimer;	120	extern uint16_t ignoreCompassTimer;
122	extern uint16_t accVector;	121	extern uint16_t accVector;
123		122
124	extern int32_t headingError;	123	extern int32_t headingError;
125		124
126		125
127	/*	126	/*
128	* 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,
129	* 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
130	* can be updated in flight by different ways, for example:	129	* can be updated in flight by different ways, for example:
131	* - 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
132	* - 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
133	* integration, and then adding the sum / n to the dynamic offset	132	* integration, and then adding the sum / n to the dynamic offset
134	* - 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
135	* - Invent your own...	134	* - Invent your own...
136	*/	135	*/
137	extern int16_t dynamicOffset[2], dynamicOffsetYaw;	136	extern int16_t dynamicOffset[2], dynamicOffsetYaw;
138		137
139	/*	138	/*
140	* For NaviCtrl use.	139	* For NaviCtrl use.
141	*/	140	*/
142	extern int16_t averageAcc[2], averageAccCount;	141	extern int16_t averageAcc[2], averageAccCount;
143		142
144	/*	143	/*
145	* 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).
146	* 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).
147	*/	146	*/
148	void attitude_setNeutral(void);	147	void attitude_setNeutral(void);
149		148
150	/*	149	/*
151	* Experiment.	150	* Experiment.
152	*/	151	*/
153	// void attitude_startDynamicCalibration(void);	152	// void attitude_startDynamicCalibration(void);
154	// void attitude_continueDynamicCalibration(void);	153	// void attitude_continueDynamicCalibration(void);
155		154
156	int32_t getAngleEstimateFromAcc(uint8_t axis);	155	int32_t getAngleEstimateFromAcc(uint8_t axis);
157		156
158	/*	157	/*
159	* Main routine, called from the flight loop.	158	* Main routine, called from the flight loop.
160	*/	159	*/
161	void calculateFlightAttitude(void);	160	void calculateFlightAttitude(void);
162		161
163	void attitude_resetHeadingToMagnetic(void);	162	void attitude_resetHeadingToMagnetic(void);
164		163
165	#endif //_ATTITUDE_H	164	#endif //_ATTITUDE_H
166		165