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


#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include "flight.h"
#include "output.h"
 
// Necessary for external control and motor test
#include "uart0.h"
 
#include "timer2.h"
#include "analog.h"
#include "attitude.h"
#include "controlMixer.h"
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
 * target-directions integrals.
 */
int32_t target[3];
 
/*
 * Error integrals.
 */
int32_t error[3];
 
uint8_t pFactor[3];
uint8_t dFactor[3];
uint8_t iFactor[3];
uint8_t reverse[3];
int32_t IPart[3] = { 0, 0, 0 };
 
int16_t controlServos[MAX_CONTROL_SERVOS];
 
/************************************************************************/
/*  Neutral Readings                                                    */
/************************************************************************/
#define CONTROL_CONFIG_SCALE 10
 
void flight_setGround(void) {
        IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
        target[PITCH] = attitude[PITCH];
        target[ROLL] = attitude[ROLL];
        target[YAW] = attitude[YAW];
}
 
// this should be followed by a call to switchToFlightMode!!
void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
        debugOut.analog[16] = flightMode;
 
        reverse[PITCH] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_ELEVATOR;
        reverse[ROLL] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_AILERONS;
        reverse[YAW] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_RUDDER;
 
        for (uint8_t i = 0; i < 3; i++) {
                if (flightMode == FLIGHT_MODE_MANUAL) {
                        pFactor[i] = 0;
                        dFactor[i] = 0;
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) {
                        pFactor[i] = staticParams.gyroPID[i].P;
                        dFactor[i] = staticParams.gyroPID[i].D;
                }
 
                if (flightMode == FLIGHT_MODE_ANGLES) {
                        iFactor[i] = staticParams.gyroPID[i].I;
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
                        iFactor[i] = 0;
                }
        }
}
 
/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {
        // Mixer Fractions that are combined for Motor Control
        int16_t term[4];
 
        // PID controller variables
        int16_t PDPart[3];
 
        static int8_t debugDataTimer = 1;
 
        // High resolution motor values for smoothing of PID motor outputs
        // static int16_t outputFilters[MAX_OUTPUTS];
 
        uint8_t axis;
 
        // TODO: Check modern version.
        // calculateFlightAttitude();
        // TODO: Check modern version.
        // controlMixer_update();
        term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
 
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
        target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
        target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
        target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;
 
        for (axis = PITCH; axis <= YAW; axis++) {
                if (target[axis] > OVER180) {
                        target[axis] -= OVER360;
                } else if (target[axis] <= -OVER180) {
                  target[axis] += OVER360;
                }
 
                if (reverse[axis])
                        error[axis] = attitude[axis] + target[axis];
                else
                        error[axis] = attitude[axis] - target[axis];
                if (error[axis] > OVER180) {
                        error[axis] -= OVER360;
                } else if (error[axis] <= -OVER180) {
                        error[axis] += OVER360;
                }
 
                /************************************************************************/
                /* Calculate control feedback from angle (gyro integral)                */
                /* and angular velocity (gyro signal)                                   */
                /************************************************************************/
                PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
                                + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
                if (reverse[axis])
                        PDPart[axis] = -PDPart[axis];
 
                int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
                if (reverse[axis])
                        PDPart[axis] += anglePart;
                else
                        PDPart[axis] -= anglePart;
 
                // Add I parts here... these are integrated errors.
                // When an error wraps, actually its I part should be negated or something...
 
                term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
        }
 
        debugOut.analog[12] = term[PITCH];
        debugOut.analog[13] = term[ROLL];
        debugOut.analog[14] = term[YAW];
        debugOut.analog[15] = term[THROTTLE];
 
        for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
                int16_t tmp;
                if (servoTestActive) {
                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
                } else {
                        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
                        switch (i) {
                        case 0:
                                tmp = term[ROLL];
                                break;
                        case 1:
                                tmp = term[PITCH];
                                break;
                        case 2:
                                tmp = term[THROTTLE];
                                break;
                        case 3:
                                tmp = term[YAW];
                                break;
                        default:
                                tmp = 0;
                        }
                        // These are all signed and in the same units as the RC stuff in rc.c.
                        controlServos[i] = tmp;
                }
        }
 
        calculateControlServoValues();
 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Debugging
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if (!(--debugDataTimer)) {
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
                debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
                debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
                debugOut.analog[2] = gyro_PID[YAW];
 
                debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
 
                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
 
                debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
 
                debugOut.analog[12] = term[PITCH];
                debugOut.analog[13] = term[ROLL];
                debugOut.analog[14] = term[YAW];
 
                //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
                //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
                //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg
                //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
        }
}
 

Rev 2102	Rev 2103
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include "eeprom.h"	3	#include "eeprom.h"
4	#include "flight.h"	4	#include "flight.h"
5	#include "output.h"	5	#include "output.h"
6		6
7	// Necessary for external control and motor test	7	// Necessary for external control and motor test
8	#include "uart0.h"	8	#include "uart0.h"
9		9
10	#include "timer2.h"	10	#include "timer2.h"
-		11	#include "analog.h"
11	#include "attitude.h"	12	#include "attitude.h"
12	#include "controlMixer.h"	13	#include "controlMixer.h"
13		14
14	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}	15	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
15		16
16	/*	17	/*
17	* target-directions integrals.	18	* target-directions integrals.
18	*/	19	*/
19	int32_t target[3];	20	int32_t target[3];
20		21
21	/*	22	/*
22	* Error integrals.	23	* Error integrals.
23	*/	24	*/
24	int32_t error[3];	25	int32_t error[3];
25		26
26	uint8_t pFactor[3];	27	uint8_t pFactor[3];
27	uint8_t dFactor[3];	28	uint8_t dFactor[3];
28	uint8_t iFactor[3];	29	uint8_t iFactor[3];
29	uint8_t reverse[3];	30	uint8_t reverse[3];
30	int32_t IPart[3] = { 0, 0, 0 };	31	int32_t IPart[3] = { 0, 0, 0 };
31		32
32	int16_t controlServos[MAX_CONTROL_SERVOS];	33	int16_t controlServos[MAX_CONTROL_SERVOS];
33		34
34	/************************************************************************/	35	/************************************************************************/
35	/* Neutral Readings */	36	/* Neutral Readings */
36	/************************************************************************/	37	/************************************************************************/
37	#define CONTROL_CONFIG_SCALE 10	38	#define CONTROL_CONFIG_SCALE 10
38		39
39	void flight_setGround(void) {	40	void flight_setGround(void) {
40	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;	41	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
41	target[PITCH] = attitude[PITCH];	42	target[PITCH] = attitude[PITCH];
42	target[ROLL] = attitude[ROLL];	43	target[ROLL] = attitude[ROLL];
43	target[YAW] = attitude[YAW];	44	target[YAW] = attitude[YAW];
44	}	45	}
45		46
46	// this should be followed by a call to switchToFlightMode!!	47	// this should be followed by a call to switchToFlightMode!!
47	void updateFlightParametersToFlightMode(uint8_t flightMode) {	48	void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
48	debugOut.analog[15] = flightMode;	49	debugOut.analog[16] = flightMode;
49		50
50	reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse	51	reverse[PITCH] = staticParams.controlServosReverse
51	& CONTROL_SERVO_REVERSE_ELEVATOR;	52	& CONTROL_SERVO_REVERSE_ELEVATOR;
52	reverse[CONTROL_AILERONS] = staticParams.controlServosReverse	53	reverse[ROLL] = staticParams.controlServosReverse
53	& CONTROL_SERVO_REVERSE_AILERONS;	54	& CONTROL_SERVO_REVERSE_AILERONS;
54	reverse[CONTROL_RUDDER] = staticParams.controlServosReverse	55	reverse[YAW] = staticParams.controlServosReverse
55	& CONTROL_SERVO_REVERSE_RUDDER;	56	& CONTROL_SERVO_REVERSE_RUDDER;
56		57
57	for (uint8_t i = 0; i < 3; i++) {	58	for (uint8_t i = 0; i < 3; i++) {
58	if (flightMode == FLIGHT_MODE_MANUAL) {	59	if (flightMode == FLIGHT_MODE_MANUAL) {
59	pFactor[i] = 0;	60	pFactor[i] = 0;
60	dFactor[i] = 0;	61	dFactor[i] = 0;
61	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_ANGLES) {	62	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_ANGLES) {
62	pFactor[i] = staticParams.gyroPID[i].P;	63	pFactor[i] = staticParams.gyroPID[i].P;
63	dFactor[i] = staticParams.gyroPID[i].D;	64	dFactor[i] = staticParams.gyroPID[i].D;
64	}	65	}
65		66
66	if (flightMode == FLIGHT_MODE_ANGLES) {	67	if (flightMode == FLIGHT_MODE_ANGLES) {
67	iFactor[i] = staticParams.gyroPID[i].I;	68	iFactor[i] = staticParams.gyroPID[i].I;
68	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_MANUAL) {	69	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_MANUAL) {
69	iFactor[i] = 0;	70	iFactor[i] = 0;
70	}	71	}
71	}	72	}
72	}	73	}
73		74
74	/************************************************************************/	75	/************************************************************************/
75	/* Main Flight Control */	76	/* Main Flight Control */
76	/************************************************************************/	77	/************************************************************************/
77	void flight_control(void) {	78	void flight_control(void) {
78	// Mixer Fractions that are combined for Motor Control	79	// Mixer Fractions that are combined for Motor Control
79	int16_t throttleTerm, term[3];	80	int16_t term[4];
80		81
81	// PID controller variables	82	// PID controller variables
82	int16_t PDPart[3];	83	int16_t PDPart[3];
83		84
84	static int8_t debugDataTimer = 1;	85	static int8_t debugDataTimer = 1;
85		86
86	// High resolution motor values for smoothing of PID motor outputs	87	// High resolution motor values for smoothing of PID motor outputs
87	// static int16_t outputFilters[MAX_OUTPUTS];	88	// static int16_t outputFilters[MAX_OUTPUTS];
88		89
89	uint8_t axis;	90	uint8_t axis;
90		91
91	// TODO: Check modern version.	92	// TODO: Check modern version.
92	// calculateFlightAttitude();	93	// calculateFlightAttitude();
93	// TODO: Check modern version.	94	// TODO: Check modern version.
94	// controlMixer_update();	95	// controlMixer_update();
95	throttleTerm = controls[CONTROL_THROTTLE];	96	term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
96		97
97	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.	98	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
98	target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;	99	target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
99	target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;	100	target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
100	target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;	101	target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;
101		102
102	for (axis = PITCH; axis <= YAW; axis++) {	103	for (axis = PITCH; axis <= YAW; axis++) {
103	if (target[axis] > OVER180) {	104	if (target[axis] > OVER180) {
104	target[axis] -= OVER360;	105	target[axis] -= OVER360;
105	} else if (attitude[axis] <= -OVER180) {	106	} else if (target[axis] <= -OVER180) {
106	attitude[axis] += OVER360;	107	target[axis] += OVER360;
107	}	108	}
108		109
109	if (reverse[axis])	110	if (reverse[axis])
110	error[axis] = attitude[axis] + target[axis];	111	error[axis] = attitude[axis] + target[axis];
111	else	112	else
112	error[axis] = attitude[axis] - target[axis];	113	error[axis] = attitude[axis] - target[axis];
113	if (error[axis] > OVER180) {	114	if (error[axis] > OVER180) {
114	error[axis] -= OVER360;	115	error[axis] -= OVER360;
115	} else if (error[axis] <= -OVER180) {	116	} else if (error[axis] <= -OVER180) {
116	error[axis] += OVER360;	117	error[axis] += OVER360;
117	}	118	}
118		119
119	/************************************************************************/	120	/************************************************************************/
120	/* Calculate control feedback from angle (gyro integral) */	121	/* Calculate control feedback from angle (gyro integral) */
121	/* and angular velocity (gyro signal) */	122	/* and angular velocity (gyro signal) */
122	/************************************************************************/	123	/************************************************************************/
123	PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)	124	PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
124	+ ((differential[axis] * (int16_t) dFactor[axis]) >> 4);	125	+ ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
125	if (reverse[axis])	126	if (reverse[axis])
126	PDPart[axis] = -PDPart[axis];	127	PDPart[axis] = -PDPart[axis];
127		128
128	int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;	129	int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
129	if (reverse[axis])	130	if (reverse[axis])
130	PDPart[axis] += anglePart;	131	PDPart[axis] += anglePart;
131	else	132	else
132	PDPart[axis] -= anglePart;	133	PDPart[axis] -= anglePart;
133		134
134	// Add I parts here... these are integrated errors.	135	// Add I parts here... these are integrated errors.
135	// When an error wraps, actually its I part should be negated or something...	136	// When an error wraps, actually its I part should be negated or something...
136		137
137	term[axis] = controls[axis] + PDPart[axis] + IPart[axis];	138	term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
138	}	139	}
139		140
140	debugOut.analog[12] = term[PITCH];	141	debugOut.analog[12] = term[PITCH];
141	debugOut.analog[13] = term[ROLL];	142	debugOut.analog[13] = term[ROLL];
142	debugOut.analog[14] = throttleTerm;	143	debugOut.analog[14] = term[YAW];
143	debugOut.analog[15] = term[YAW];	144	debugOut.analog[15] = term[THROTTLE];
144		145
145	for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {	146	for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
146	int16_t tmp;	147	int16_t tmp;
147	if (servoTestActive) {	148	if (servoTestActive) {
148	controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;	149	controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
149	} else {	150	} else {
150	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.	151	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
151	switch (i) {	152	switch (i) {
152	case 0:	153	case 0:
153	tmp = term[ROLL];	154	tmp = term[ROLL];
154	break;	155	break;
155	case 1:	156	case 1:
156	tmp = term[PITCH];	157	tmp = term[PITCH];
157	break;	158	break;
158	case 2:	159	case 2:
159	tmp = throttleTerm;	160	tmp = term[THROTTLE];
160	break;	161	break;
161	case 3:	162	case 3:
162	tmp = term[YAW];	163	tmp = term[YAW];
163	break;	164	break;
164	default:	165	default:
165	tmp = 0;	166	tmp = 0;
166	}	167	}
167	// These are all signed and in the same units as the RC stuff in rc.c.	168	// These are all signed and in the same units as the RC stuff in rc.c.
168	controlServos[i] = tmp;	169	controlServos[i] = tmp;
169	}	170	}
170	}	171	}
171		172
172	calculateControlServoValues;	173	calculateControlServoValues();
173		174
174	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	175	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
175	// Debugging	176	// Debugging
176	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	177	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
177	if (!(--debugDataTimer)) {	178	if (!(--debugDataTimer)) {
178	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	179	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
179	debugOut.analog[0] = rate_PID[PITCH]; // in 0.1 deg	180	debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
180	debugOut.analog[1] = rate_PID[ROLL]; // in 0.1 deg	181	debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
181	debugOut.analog[2] = rate_PID[YAW];	182	debugOut.analog[2] = gyro_PID[YAW];
182		183
183	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	184	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
184	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	185	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
185	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);	186	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
186		187
187	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	188	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
188	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	189	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
189	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);	190	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
190		191
191	debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	192	debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
192	debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	193	debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
193	debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);	194	debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
194		195
195	debugOut.analog[12] = term[PITCH];	196	debugOut.analog[12] = term[PITCH];
196	debugOut.analog[13] = term[ROLL];	197	debugOut.analog[13] = term[ROLL];
197	debugOut.analog[14] = term[YAW];	198	debugOut.analog[14] = term[YAW];
198		199
199	//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	200	//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
200	//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	201	//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
201	//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg	202	//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg
202	//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg	203	//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
203	}	204	}
204	}	205	}
205		206

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 2102 → 2103