WebSVN - FlightCtrl - Diff - Rev 2099 and 2103 - /branches/dongfang_FC


#include <stdlib.h>
#include <avr/io.h>
#include <stdlib.h>
 
#include "attitude.h"
#include "dongfangMath.h"
#include "commands.h"
 
// For scope debugging only!
#include "rc.h"
 
// where our main data flow comes from.
#include "analog.h"
 
#include "configuration.h"
#include "output.h"
 
// Some calculations are performed depending on some stick related things.
#include "controlMixer.h"
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
 * Gyro readings, as read from the analog module. It would have been nice to flow
 * them around between the different calculations as a struct or array (doing
 * things functionally without side effects) but this is shorter and probably
 * faster too.
 * The variables are overwritten at each attitude calculation invocation - the values
 * are not preserved or reused.
 */
 
 
 
 
 
 
/*
 * Gyro integrals. These are the rotation angles of the airframe compared to the
 * horizontal plane, yaw relative to yaw at start.
 */
int32_t attitude[3];
 
/*
 * Experiment: Compensating for dynamic-induced gyro biasing.
 */
int16_t driftComp[3] = { 0, 0, 0 };
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
 
/************************************************************************
 * Set inclination angles from the acc. sensor data.                    
 * If acc. sensors are not used, set to zero.                          
 * TODO: One could use inverse sine to calculate the angles more        
 * accurately, but since: 1) the angles are rather small at times when
 * it makes sense to set the integrals (standing on ground, or flying at  
 * constant speed, and 2) at small angles a, sin(a) ~= constant * a,    
 * it is hardly worth the trouble.                                      
 ************************************************************************/
/*
int32_t getAngleEstimateFromAcc(uint8_t axis) {
  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
}
*/
 
void setStaticAttitudeAngles(void) {
  attitude[PITCH] = attitude[ROLL] = 0;
}
 
/************************************************************************
 * Neutral Readings                                                    
 ************************************************************************/
void attitude_setNeutral(void) {
    // Calibrate hardware.
  analog_setNeutral();
 
  // reset gyro integrals to acc guessing
  setStaticAttitudeAngles();
}
 
/************************************************************************
 * Get sensor data from the analog module, and release the ADC          
 * TODO: Ultimately, the analog module could do this (instead of dumping
 * the values into variables).
 * The rate variable end up in a range of about [-1024, 1023].
 *************************************************************************/
void getAnalogData(void) {
  uint8_t axis;
 
  analog_update();
 
  for (axis = PITCH; axis <= YAW; axis++) {
 
 
 
  }
}
 
void integrate(void) {
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
  uint8_t axis;
 
  /*
   * Yaw
   * Calculate yaw gyro integral (~ to rotation angle)
   * Limit heading proportional to 0 deg to 360 deg
   */
  /*
   * Pitch axis integration and range boundary wrap.
   */
 
  for (axis = PITCH; axis <= YAW; axis++) {
    attitude[axis] += gyro_ATT[axis];
    if (attitude[axis] > OVER180) {
      attitude[axis] -= OVER360;
    } else if (attitude[axis] <= -OVER180) {
      attitude[axis] += OVER360;
    }
  }
}
 
/************************************************************************
 * Main procedure.
 ************************************************************************/
void calculateFlightAttitude(void) {
  getAnalogData();
  integrate();
 
  // We are done reading variables from the analog module.
  // Interrupt-driven sensor reading may restart.
  startAnalogConversionCycle();
}
 

Rev 2099	Rev 2103
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include <stdlib.h>	3	#include <stdlib.h>
4		4
5	#include "attitude.h"	5	#include "attitude.h"
6	#include "dongfangMath.h"	6	#include "dongfangMath.h"
7	#include "commands.h"	7	#include "commands.h"
8		8
9	// For scope debugging only!	9	// For scope debugging only!
10	#include "rc.h"	10	#include "rc.h"
11		11
12	// where our main data flow comes from.	12	// where our main data flow comes from.
13	#include "analog.h"	13	#include "analog.h"
14		14
15	#include "configuration.h"	15	#include "configuration.h"
16	#include "output.h"	16	#include "output.h"
17		17
18	// Some calculations are performed depending on some stick related things.	18	// Some calculations are performed depending on some stick related things.
19	#include "controlMixer.h"	19	#include "controlMixer.h"
20		20
21	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}	21	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
22		22
23	/*	23	/*
24	* Gyro readings, as read from the analog module. It would have been nice to flow	24	* Gyro readings, as read from the analog module. It would have been nice to flow
25	* them around between the different calculations as a struct or array (doing	25	* them around between the different calculations as a struct or array (doing
26	* things functionally without side effects) but this is shorter and probably	26	* things functionally without side effects) but this is shorter and probably
27	* faster too.	27	* faster too.
28	* The variables are overwritten at each attitude calculation invocation - the values	28	* The variables are overwritten at each attitude calculation invocation - the values
29	* are not preserved or reused.	29	* are not preserved or reused.
30	*/	30	*/
31	int16_t rate_ATT[3];	-
32		-
33	// With different (less) filtering	-
34	int16_t rate_PID[3];	-
35	int16_t differential[3];	-
36		31
37	/*	32	/*
38	* Gyro integrals. These are the rotation angles of the airframe compared to the	33	* Gyro integrals. These are the rotation angles of the airframe compared to the
39	* horizontal plane, yaw relative to yaw at start.	34	* horizontal plane, yaw relative to yaw at start.
40	*/	35	*/
41	int32_t attitude[3];	36	int32_t attitude[3];
42		37
43	/*	38	/*
44	* Experiment: Compensating for dynamic-induced gyro biasing.	39	* Experiment: Compensating for dynamic-induced gyro biasing.
45	*/	40	*/
46	int16_t driftComp[3] = { 0, 0, 0 };	41	int16_t driftComp[3] = { 0, 0, 0 };
47	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;	42	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
48	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;	43	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
49	// int16_t dynamicCalCount;	44	// int16_t dynamicCalCount;
50		45
51	/************************************************************************	46	/************************************************************************
52	* Set inclination angles from the acc. sensor data.	47	* Set inclination angles from the acc. sensor data.
53	* If acc. sensors are not used, set to zero.	48	* If acc. sensors are not used, set to zero.
54	* TODO: One could use inverse sine to calculate the angles more	49	* TODO: One could use inverse sine to calculate the angles more
55	* accurately, but since: 1) the angles are rather small at times when	50	* accurately, but since: 1) the angles are rather small at times when
56	* it makes sense to set the integrals (standing on ground, or flying at	51	* it makes sense to set the integrals (standing on ground, or flying at
57	* constant speed, and 2) at small angles a, sin(a) ~= constant * a,	52	* constant speed, and 2) at small angles a, sin(a) ~= constant * a,
58	* it is hardly worth the trouble.	53	* it is hardly worth the trouble.
59	************************************************************************/	54	************************************************************************/
60	/*	55	/*
61	int32_t getAngleEstimateFromAcc(uint8_t axis) {	56	int32_t getAngleEstimateFromAcc(uint8_t axis) {
62	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];	57	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
63	}	58	}
64	*/	59	*/
65		60
66	void setStaticAttitudeAngles(void) {	61	void setStaticAttitudeAngles(void) {
67	attitude[PITCH] = attitude[ROLL] = 0;	62	attitude[PITCH] = attitude[ROLL] = 0;
68	}	63	}
69		64
70	/************************************************************************	65	/************************************************************************
71	* Neutral Readings	66	* Neutral Readings
72	************************************************************************/	67	************************************************************************/
73	void attitude_setNeutral(void) {	68	void attitude_setNeutral(void) {
74	// Calibrate hardware.	69	// Calibrate hardware.
75	analog_setNeutral();	70	analog_setNeutral();
76		71
77	// reset gyro integrals to acc guessing	72	// reset gyro integrals to acc guessing
78	setStaticAttitudeAngles();	73	setStaticAttitudeAngles();
79	}	74	}
80		75
81	/************************************************************************	76	/************************************************************************
82	* Get sensor data from the analog module, and release the ADC	77	* Get sensor data from the analog module, and release the ADC
83	* TODO: Ultimately, the analog module could do this (instead of dumping	78	* TODO: Ultimately, the analog module could do this (instead of dumping
84	* the values into variables).	79	* the values into variables).
85	* The rate variable end up in a range of about [-1024, 1023].	80	* The rate variable end up in a range of about [-1024, 1023].
86	*************************************************************************/	81	*************************************************************************/
87	void getAnalogData(void) {	82	void getAnalogData(void) {
88	uint8_t axis;	83	uint8_t axis;
89		-
90	analog_update();	84	analog_update();
91		-
92	for (axis = PITCH; axis <= YAW; axis++) {	85	for (axis = PITCH; axis <= YAW; axis++) {
93	rate_PID[axis] = gyro_PID[axis];	-
94	rate_ATT[axis] = gyro_ATT[axis];	-
95	differential[axis] = gyroD[axis];	-
96	}	86	}
97	}	87	}
98		88
99	void integrate(void) {	89	void integrate(void) {
100	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.	90	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
101	uint8_t axis;	91	uint8_t axis;
102		92
103	/*	93	/*
104	* Yaw	94	* Yaw
105	* Calculate yaw gyro integral (~ to rotation angle)	95	* Calculate yaw gyro integral (~ to rotation angle)
106	* Limit heading proportional to 0 deg to 360 deg	96	* Limit heading proportional to 0 deg to 360 deg
107	*/	97	*/
108	/*	98	/*
109	* Pitch axis integration and range boundary wrap.	99	* Pitch axis integration and range boundary wrap.
110	*/	100	*/
111		101
112	for (axis = PITCH; axis <= YAW; axis++) {	102	for (axis = PITCH; axis <= YAW; axis++) {
113	attitude[axis] += rate_ATT[axis];	103	attitude[axis] += gyro_ATT[axis];
114	if (attitude[axis] > OVER180) {	104	if (attitude[axis] > OVER180) {
115	attitude[axis] -= OVER360;	105	attitude[axis] -= OVER360;
116	} else if (attitude[axis] <= -OVER180) {	106	} else if (attitude[axis] <= -OVER180) {
117	attitude[axis] += OVER360;	107	attitude[axis] += OVER360;
118	}	108	}
119	}	109	}
120	}	110	}
121		111
122	/************************************************************************	112	/************************************************************************
123	* Main procedure.	113	* Main procedure.
124	************************************************************************/	114	************************************************************************/
125	void calculateFlightAttitude(void) {	115	void calculateFlightAttitude(void) {
126	getAnalogData();	116	getAnalogData();
127	integrate();	117	integrate();
128		118
129	// We are done reading variables from the analog module.	119	// We are done reading variables from the analog module.
130	// Interrupt-driven sensor reading may restart.	120	// Interrupt-driven sensor reading may restart.
131	startAnalogConversionCycle();	121	startAnalogConversionCycle();
132	}	122	}
133		123

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/attitude.c @ 2097 - Rev 2099 → 2103