WebSVN - FlightCtrl - Diff - Rev 2110 and 2129 - /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] = attitude[YAW] = 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) {
 
  analog_update();
 
 
}
 
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 2110	Rev 2129
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		31
32	/*	32	/*
33	* 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
34	* horizontal plane, yaw relative to yaw at start.	34	* horizontal plane, yaw relative to yaw at start.
35	*/	35	*/
36	int32_t attitude[3];	36	int32_t attitude[3];
37		37
38	/*	38	/*
39	* Experiment: Compensating for dynamic-induced gyro biasing.	39	* Experiment: Compensating for dynamic-induced gyro biasing.
40	*/	40	*/
41	int16_t driftComp[3] = { 0, 0, 0 };	41	int16_t driftComp[3] = { 0, 0, 0 };
42	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;	42	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
43	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;	43	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
44	// int16_t dynamicCalCount;	44	// int16_t dynamicCalCount;
45		45
46	/************************************************************************	46	/************************************************************************
47	* Set inclination angles from the acc. sensor data.	47	* Set inclination angles from the acc. sensor data.
48	* If acc. sensors are not used, set to zero.	48	* If acc. sensors are not used, set to zero.
49	* TODO: One could use inverse sine to calculate the angles more	49	* TODO: One could use inverse sine to calculate the angles more
50	* 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
51	* 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
52	* 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,
53	* it is hardly worth the trouble.	53	* it is hardly worth the trouble.
54	************************************************************************/	54	************************************************************************/
55	/*	55	/*
56	int32_t getAngleEstimateFromAcc(uint8_t axis) {	56	int32_t getAngleEstimateFromAcc(uint8_t axis) {
57	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];	57	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
58	}	58	}
59	*/	59	*/
60		60
61	void setStaticAttitudeAngles(void) {	61	void setStaticAttitudeAngles(void) {
62	attitude[PITCH] = attitude[ROLL] = attitude[YAW] = 0;	62	attitude[PITCH] = attitude[ROLL] = attitude[YAW] = 0;
63	}	63	}
64		64
65	/************************************************************************	65	/************************************************************************
66	* Neutral Readings	66	* Neutral Readings
67	************************************************************************/	67	************************************************************************/
68	void attitude_setNeutral(void) {	68	void attitude_setNeutral(void) {
69	// Calibrate hardware.	69	// Calibrate hardware.
70	analog_setNeutral();	70	analog_setNeutral();
71		71
72	// reset gyro integrals to acc guessing	72	// reset gyro integrals to acc guessing
73	setStaticAttitudeAngles();	73	setStaticAttitudeAngles();
74	}	74	}
75		75
76	/************************************************************************	76	/************************************************************************
77	* Get sensor data from the analog module, and release the ADC	77	* Get sensor data from the analog module, and release the ADC
78	* TODO: Ultimately, the analog module could do this (instead of dumping	78	* TODO: Ultimately, the analog module could do this (instead of dumping
79	* the values into variables).	79	* the values into variables).
80	* 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].
81	*************************************************************************/	81	*************************************************************************/
82	void getAnalogData(void) {	82	void getAnalogData(void) {
83	uint8_t axis;	-
84	analog_update();	83	analog_update();
85	for (axis = PITCH; axis <= YAW; axis++) {	-
86	}	-
87	}	84	}
88		85
89	void integrate(void) {	86	void integrate(void) {
90	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.	87	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
91	uint8_t axis;	88	uint8_t axis;
92		89
93	/*	90	/*
94	* Yaw	91	* Yaw
95	* Calculate yaw gyro integral (~ to rotation angle)	92	* Calculate yaw gyro integral (~ to rotation angle)
96	* Limit heading proportional to 0 deg to 360 deg	93	* Limit heading proportional to 0 deg to 360 deg
97	*/	94	*/
98	/*	95	/*
99	* Pitch axis integration and range boundary wrap.	96	* Pitch axis integration and range boundary wrap.
100	*/	97	*/
101		98
102	for (axis = PITCH; axis <= YAW; axis++) {	99	for (axis = PITCH; axis <= YAW; axis++) {
103	attitude[axis] += gyro_ATT[axis];	100	attitude[axis] += gyro_ATT[axis];
104	if (attitude[axis] > OVER180) {	101	if (attitude[axis] > OVER180) {
105	attitude[axis] -= OVER360;	102	attitude[axis] -= OVER360;
106	} else if (attitude[axis] <= -OVER180) {	103	} else if (attitude[axis] <= -OVER180) {
107	attitude[axis] += OVER360;	104	attitude[axis] += OVER360;
108	}	105	}
109	}	106	}
110	}	107	}
111		108
112	/************************************************************************	109	/************************************************************************
113	* Main procedure.	110	* Main procedure.
114	************************************************************************/	111	************************************************************************/
115	void calculateFlightAttitude(void) {	112	void calculateFlightAttitude(void) {
116	getAnalogData();	113	getAnalogData();
117	integrate();	114	integrate();
118		115
119	// We are done reading variables from the analog module.	116	// We are done reading variables from the analog module.
120	// Interrupt-driven sensor reading may restart.	117	// Interrupt-driven sensor reading may restart.
121	startAnalogConversionCycle();	118	startAnalogConversionCycle();
122	}	119	}
123		120

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/attitude.c @ 2088 - Rev 2110 → 2129