WebSVN - FlightCtrl - Diff - Rev 1926 and 1927 - /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"
 
// For debugging only.
#include "uart0.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
 
/*
 * The frequency at which numerical integration takes place. 488 in original code.
 */
#define INTEGRATION_FREQUENCY 488
 
/*
 * 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.
 * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
 * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
 */
#define HIRES_GYRO_INTEGRATION_FACTOR 1
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
 
/*
 * 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
 
/*
 * Growth of the integral per degree:
 * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
 * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration.
 * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable).
 */
#define GYRO_DEG_FACTOR_PITCHROLL (GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
#define GYRO_DEG_FACTOR_YAW (GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
 
/*
 * 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 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 180)
#define PITCHROLLOVER360 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 360)
#define YAWOVER360       ((int32_t)GYRO_DEG_FACTOR_YAW * 360)
 
/*
 * Rotation rates
 */
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
extern int16_t differential[3];
 
/*
 * Attitudes calculated by numerical integration of gyro rates
 */
extern int32_t angle[3];
 
 
 
 
 
 
 
 
 
 
extern int32_t error[3];
 
 
 
/*
 * 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);
#endif //_ATTITUDE_H
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/attitude.h - Rev 1926 → 1927

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