WebSVN - FlightCtrl - Diff - Rev 1980 and 2035 - /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
 
/*
 * 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 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 (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)
 
/*
 * 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))
 
/*
 * 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 angle[2], yawAngleDiff;
 
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
 
/*
 * Compass navigation
 */
extern int16_t compassHeading;
extern int16_t compassCourse;
// extern int16_t compassOffCourse;
extern uint8_t compassCalState;
extern int32_t yawGyroHeading;
extern int16_t yawGyroHeadingInDeg;
extern uint8_t updateCompassCourse;
extern uint16_t ignoreCompassTimer;
extern uint16_t accVector;
 
void updateCompass(void);
 
/*
 * 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_rewrite/attitude.h - Rev 1980 → 2035

Rev 1980		Rev 2035
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	* Default hysteresis to use for the -180 degree to 180 degree wrap.	22	* Default hysteresis to use for the -180 degree to 180 degree wrap.
23	*/	23	*/
24	#define PITCHOVER_HYSTERESIS 0L	24	#define PITCHOVER_HYSTERESIS 0L
25	#define ROLLOVER_HYSTERESIS 0L	25	#define ROLLOVER_HYSTERESIS 0L
26		26
27	/*	27	/*
28	* The frequency at which numerical integration takes place. 488 in original code.	28	* The frequency at which numerical integration takes place. 488 in original code.
29	*/	29	*/
30	#define INTEGRATION_FREQUENCY 488	30	#define INTEGRATION_FREQUENCY 488
31		31
32	/*	32	/*
33	* Gyro readings are divided by this before being used in attitude control. This will scale them	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	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.	35	* scaling - the below definitions make precise conversion factors.
36	* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.	36	* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610.
37	* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.	37	* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
38	*/	38	*/
39	#define HIRES_GYRO_INTEGRATION_FACTOR 1	39	#define HIRES_GYRO_INTEGRATION_FACTOR 1
40	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)	40	// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
41		41
42	/*	42	/*
43	* Constant for deriving an attitude angle from acceleration measurement.	43	* Constant for deriving an attitude angle from acceleration measurement.
44	*	44	*
45	* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.	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	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.	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:	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.	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:	50	* The sine of v is the far side / the hypothenusis:
51	* sin v = acc / sqrt(acc^2 + acc_z^2)	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:	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	53	* sin v ~= acc / acc_z
54	* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,	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):	55	* and sin v ~= v (small angles, in radians):
56	* sin v ~= acc / 410	56	* sin v ~= acc / 410
57	* v / 57.3 ~= acc / 410	57	* v / 57.3 ~= acc / 410
58	* v ~= acc * 57.3 / 410	58	* v ~= acc * 57.3 / 410
59	* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.	59	* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
60	*	60	*
61	* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)	61	* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
62	*/	62	*/
63	#define DEG_ACC_FACTOR 7	63	#define DEG_ACC_FACTOR 7
64		64
65	/*	65	/*
66	* Growth of the integral per degree:	66	* Growth of the integral per degree:
67	* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by	67	* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
68	* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration.	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).	69	* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable).
70	*/	70	*/
71	#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)	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)	72	#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
73		73
74	/*	74	/*
75	* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value	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	76	* = the factor an acc. sensor should be multiplied by to get the gyro integral
77	* value for the same (small) angle.	77	* value for the same (small) angle.
78	* The value is about 200.	78	* The value is about 200.
79	*/	79	*/
80	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))	80	#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
81		81
82	/*	82	/*
83	* Rotation rates	83	* Rotation rates
84	*/	84	*/
85	extern int16_t rate_PID[2], rate_ATT[2], yawRate;	85	extern int16_t rate_PID[2], rate_ATT[2], yawRate;
86	extern int16_t differential[2];	86	extern int16_t differential[2];
87		87
88	/*	88	/*
89	* Attitudes calculated by numerical integration of gyro rates	89	* Attitudes calculated by numerical integration of gyro rates
90	*/	90	*/
91	extern int32_t angle[2], yawAngleDiff;	91	extern int32_t angle[2], yawAngleDiff;
92		92
93	// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c	93	// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
94		94
95	/*	95	/*
96	* Compass navigation	96	* Compass navigation
97	*/	97	*/
98	extern int16_t compassHeading;	98	extern int16_t compassHeading;
99	extern int16_t compassCourse;	99	extern int16_t compassCourse;
100	// extern int16_t compassOffCourse;	100	// extern int16_t compassOffCourse;
101	extern uint8_t compassCalState;	101	extern uint8_t compassCalState;
102	extern int32_t yawGyroHeading;	102	extern int32_t yawGyroHeading;
103	extern int16_t yawGyroHeadingInDeg;	103	extern int16_t yawGyroHeadingInDeg;
104	extern uint8_t updateCompassCourse;	104	extern uint8_t updateCompassCourse;
105	extern uint16_t ignoreCompassTimer;	105	extern uint16_t ignoreCompassTimer;
106	extern uint16_t accVector;	106	extern uint16_t accVector;
107		107
108	void updateCompass(void);	108	void updateCompass(void);
109		109
110	/*	110	/*
111	* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,	111	* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
112	* to help canceling out drift and vibration noise effects. The dynamic offsets themselves	112	* to help canceling out drift and vibration noise effects. The dynamic offsets themselves
113	* can be updated in flight by different ways, for example:	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	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	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	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	117	* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
118	* - Invent your own...	118	* - Invent your own...
119	*/	119	*/
120	extern int16_t dynamicOffset[2], dynamicOffsetYaw;	120	extern int16_t dynamicOffset[2], dynamicOffsetYaw;
121		121
122	/*	122	/*
123	* For NaviCtrl use.	123	* For NaviCtrl use.
124	*/	124	*/
125	extern int16_t averageAcc[2], averageAccCount;	125	extern int16_t averageAcc[2], averageAccCount;
126		126
127	/*	127	/*
128	* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).	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).	129	* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
130	*/	130	*/
131	void attitude_setNeutral(void);	131	void attitude_setNeutral(void);
132		132
133	/*	133	/*
134	* Experiment.	134	* Experiment.
135	*/	135	*/
136	// void attitude_startDynamicCalibration(void);	136	// void attitude_startDynamicCalibration(void);
137	// void attitude_continueDynamicCalibration(void);	137	// void attitude_continueDynamicCalibration(void);
138		138
139	int32_t getAngleEstimateFromAcc(uint8_t axis);	139	int32_t getAngleEstimateFromAcc(uint8_t axis);
140		140
141	/*	141	/*
142	* Main routine, called from the flight loop.	142	* Main routine, called from the flight loop.
143	*/	143	*/
144	void calculateFlightAttitude(void);	144	void calculateFlightAttitude(void);
-		145
145	#endif //_ATTITUDE_H	146	#endif //_ATTITUDE_H
146		147