WebSVN - FlightCtrl - Diff - Rev 2138 and 2142 - /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"
#include "configuration.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 maxError[3];
int32_t IPart[3] = { 0, 0, 0 };
PID_t airspeedPID[3];
 
int16_t controlServos[NUM_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];
}
 
void flight_updateFlightParametersToFlightMode(void) {
        debugOut.analog[16] = currentFlightMode;
 
 
 
 
 
        // At a switch to angles, we want to kill errors first.
        // This should be triggered only once per mode change!
        if (currentFlightMode == FLIGHT_MODE_ANGLES) {
                target[PITCH] = attitude[PITCH];
                target[ROLL] = attitude[ROLL];
                target[YAW] = attitude[YAW];
        }
 
        for (uint8_t axis=0; axis<3; axis++) {
                maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
        }
}
 
// Normal at airspeed = 10.
uint8_t calcAirspeedPID(uint8_t pid) {
        if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
                return pid;
        }
 
        uint16_t result = (pid * 10) / airspeedVelocity;
 
        if (result > 240 || airspeedVelocity == 0) {
                result = 240;
        }
 
        return result;
}
 
void setAirspeedPIDs(void) {
        for (uint8_t axis = 0; axis<3; axis++) {
                airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
                airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
        }
}
 
 
 
 
 
 
/************************************************************************/
/*  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;
 
        setAirspeedPIDs();
 
        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.
        int32_t tmp;
 
        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
        if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp;
 
        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
        if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp;
 
        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
        if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp;
 
        for (axis = PITCH; axis <= YAW; axis++) {
                if (target[axis] > OVER180) {
                        target[axis] -= OVER360;
                } else if (target[axis] <= -OVER180) {
                        target[axis] += OVER360;
                }
 
                int32_t error = attitude[axis] - target[axis];
 
#define ROTATETARGET 1
// #define TRUNCATEERROR 1
 
#ifdef ROTATETARGET
        //if(abs(error) > OVER180) { // doesnt work!!!
        if(error > OVER180 || error < -OVER180) {
                  // The shortest way from attitude to target crosses -180.
                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
                  if (error > 0) {
                    if (error < OVER360 - maxError[axis]) {
                      // too much err.
                      error = -maxError[axis];
                      target[axis] = attitude[axis] + maxError[axis];
                      if (target[axis] > OVER180) target[axis] -= OVER360;
                    } else {
                      // Normal case, we just need to correct for the wrap. Error will be negative.
                      error -= OVER360;
                    }
                  } else {
            if (error > maxError[axis] - OVER360) {
              // too much err.
              error = maxError[axis];
              target[axis] = attitude[axis] - maxError[axis];
              if (target[axis] < -OVER180) target[axis] += OVER360;
            } else {
              // Normal case, we just need to correct for the wrap. Error will be negative.
              error += OVER360;
            }
                  }
                } else {
                  // Simple case, linear range.
                if (error > maxError[axis]) {
                  error = maxError[axis];
                  target[axis] = attitude[axis] - maxError[axis];
                } else if (error < -maxError[axis]) {
              error = -maxError[axis];
              target[axis] = attitude[axis] + maxError[axis];
            }
                }
#endif
#ifdef TUNCATEERROR
                if (error > maxError[axis]) {
                  error = maxError[axis];
                } else if (error < -maxError[axis]) {
                  error = -maxError[axis];
                } else {
                        // update I parts here for angles mode. I parts in rate mode is something different.
                }
#endif
 
        debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
 
                /************************************************************************/
                /* Calculate control feedback from angle (gyro integral)                */
                /* and angular velocity (gyro signal)                                   */
                /************************************************************************/
                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) {
                        PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
                                + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
 
 
                } else {
                        PDPart[axis] = 0;
                }
 
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
                        PDPart[axis] += anglePart;
                }
 
                // Add I parts here... these are integrated errors.
                if (staticParams.servos[axis].reverse)
                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
                else
                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
        }
 
 
 
 
 
 
        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
                int16_t tmp;
                if (servoTestActive) {
                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
                } 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[12] = term[PITCH];
                debugOut.analog[13] = term[ROLL];
                debugOut.analog[14] = term[YAW];
            debugOut.analog[15] = term[THROTTLE];
        }
}
 

Rev 2138	Rev 2142
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	#include "timer2.h"	9	#include "timer2.h"
10	#include "analog.h"	10	#include "analog.h"
11	#include "attitude.h"	11	#include "attitude.h"
12	#include "controlMixer.h"	12	#include "controlMixer.h"
13	#include "configuration.h"	13	#include "configuration.h"
14		14
15	#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;}
16		16
17	/*	17	/*
18	* target-directions integrals.	18	* target-directions integrals.
19	*/	19	*/
20	int32_t target[3];	20	int32_t target[3];
21		21
22	/*	22	/*
23	* Error integrals.	23	* Error integrals.
24	*/	24	*/
25		-
26	uint8_t reverse[3];	25
27	int32_t maxError[3];	26	int32_t maxError[3];
28	int32_t IPart[3] = { 0, 0, 0 };	27	int32_t IPart[3] = { 0, 0, 0 };
29	PID_t airspeedPID[3];	28	PID_t airspeedPID[3];
30		29
31	int16_t controlServos[NUM_CONTROL_SERVOS];	30	int16_t controlServos[NUM_CONTROL_SERVOS];
32		31
33	/************************************************************************/	32	/************************************************************************/
34	/* Neutral Readings */	33	/* Neutral Readings */
35	/************************************************************************/	34	/************************************************************************/
36	#define CONTROL_CONFIG_SCALE 10	35	#define CONTROL_CONFIG_SCALE 10
37		36
38	void flight_setGround(void) {	37	void flight_setGround(void) {
39	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;	38	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
40	target[PITCH] = attitude[PITCH];	39	target[PITCH] = attitude[PITCH];
41	target[ROLL] = attitude[ROLL];	40	target[ROLL] = attitude[ROLL];
42	target[YAW] = attitude[YAW];	41	target[YAW] = attitude[YAW];
43	}	42	}
44		43
45	void flight_updateFlightParametersToFlightMode(void) {	44	void flight_updateFlightParametersToFlightMode(void) {
46	debugOut.analog[16] = currentFlightMode;	45	debugOut.analog[16] = currentFlightMode;
47		-
48	reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;	-
49	reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;	-
50	reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;	-
51		46
52	// At a switch to angles, we want to kill errors first.	47	// At a switch to angles, we want to kill errors first.
53	// This should be triggered only once per mode change!	48	// This should be triggered only once per mode change!
54	if (currentFlightMode == FLIGHT_MODE_ANGLES) {	49	if (currentFlightMode == FLIGHT_MODE_ANGLES) {
55	target[PITCH] = attitude[PITCH];	50	target[PITCH] = attitude[PITCH];
56	target[ROLL] = attitude[ROLL];	51	target[ROLL] = attitude[ROLL];
57	target[YAW] = attitude[YAW];	52	target[YAW] = attitude[YAW];
58	}	53	}
59		54
60	for (uint8_t axis=0; axis<3; axis++) {	55	for (uint8_t axis=0; axis<3; axis++) {
61	maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;	56	maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
62	}	57	}
63	}	58	}
64		59
65	// Normal at airspeed = 10.	60	// Normal at airspeed = 10.
66	uint8_t calcAirspeedPID(uint8_t pid) {	61	uint8_t calcAirspeedPID(uint8_t pid) {
67	if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {	62	if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
68	return pid;	63	return pid;
69	}	64	}
70		65
71	uint16_t result = (pid * 10) / airspeedVelocity;	66	uint16_t result = (pid * 10) / airspeedVelocity;
72		67
73	if (result > 240 \|\| airspeedVelocity == 0) {	68	if (result > 240 \|\| airspeedVelocity == 0) {
74	result = 240;	69	result = 240;
75	}	70	}
76		71
77	return result;	72	return result;
78	}	73	}
79		74
80	void setAirspeedPIDs(void) {	75	void setAirspeedPIDs(void) {
81	for (uint8_t axis = 0; axis<3; axis++) {	76	for (uint8_t axis = 0; axis<3; axis++) {
82	airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);	77	airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
83	airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???	78	airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
84	airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;	79	airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
85	}	80	}
86	}	81	}
87		-
88	#define LOG_STICK_SCALE 8	-
89	#define LOG_P_SCALE 6	-
90	#define LOG_I_SCALE 10	-
91	#define LOG_D_SCALE 6	-
92		82
93	/************************************************************************/	83	/************************************************************************/
94	/* Main Flight Control */	84	/* Main Flight Control */
95	/************************************************************************/	85	/************************************************************************/
96	void flight_control(void) {	86	void flight_control(void) {
97	// Mixer Fractions that are combined for Motor Control	87	// Mixer Fractions that are combined for Motor Control
98	int16_t term[4];	88	int16_t term[4];
99		89
100	// PID controller variables	90	// PID controller variables
101	int16_t PDPart[3];	91	int16_t PDPart[3];
102		92
103	static int8_t debugDataTimer = 1;	93	static int8_t debugDataTimer = 1;
104		94
105	// High resolution motor values for smoothing of PID motor outputs	95	// High resolution motor values for smoothing of PID motor outputs
106	// static int16_t outputFilters[MAX_OUTPUTS];	96	// static int16_t outputFilters[MAX_OUTPUTS];
107		97
108	uint8_t axis;	98	uint8_t axis;
109		99
110	setAirspeedPIDs();	100	setAirspeedPIDs();
111		101
112	term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];	102	term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
113		103
114	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.	104	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
115	int32_t tmp;	105	int32_t tmp;
116		106
117	tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;	107	tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
118	if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;	108	if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp;
119		109
120	tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;	110	tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
121	if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;	111	if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp;
122		112
123	tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;	113	tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
124	if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;	114	if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp;
125		115
126	for (axis = PITCH; axis <= YAW; axis++) {	116	for (axis = PITCH; axis <= YAW; axis++) {
127	if (target[axis] > OVER180) {	117	if (target[axis] > OVER180) {
128	target[axis] -= OVER360;	118	target[axis] -= OVER360;
129	} else if (target[axis] <= -OVER180) {	119	} else if (target[axis] <= -OVER180) {
130	target[axis] += OVER360;	120	target[axis] += OVER360;
131	}	121	}
132		122
133	int32_t error = attitude[axis] - target[axis];	123	int32_t error = attitude[axis] - target[axis];
134		124
135	#define ROTATETARGET 1	125	#define ROTATETARGET 1
136	// #define TRUNCATEERROR 1	126	// #define TRUNCATEERROR 1
137		127
138	#ifdef ROTATETARGET	128	#ifdef ROTATETARGET
139	//if(abs(error) > OVER180) { // doesnt work!!!	129	//if(abs(error) > OVER180) { // doesnt work!!!
140	if(error > OVER180 \|\| error < -OVER180) {	130	if(error > OVER180 \|\| error < -OVER180) {
141	// The shortest way from attitude to target crosses -180.	131	// The shortest way from attitude to target crosses -180.
142	// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.	132	// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
143	// Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.	133	// Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
144	if (error > 0) {	134	if (error > 0) {
145	if (error < OVER360 - maxError[axis]) {	135	if (error < OVER360 - maxError[axis]) {
146	// too much err.	136	// too much err.
147	error = -maxError[axis];	137	error = -maxError[axis];
148	target[axis] = attitude[axis] + maxError[axis];	138	target[axis] = attitude[axis] + maxError[axis];
149	if (target[axis] > OVER180) target[axis] -= OVER360;	139	if (target[axis] > OVER180) target[axis] -= OVER360;
150	} else {	140	} else {
151	// Normal case, we just need to correct for the wrap. Error will be negative.	141	// Normal case, we just need to correct for the wrap. Error will be negative.
152	error -= OVER360;	142	error -= OVER360;
153	}	143	}
154	} else {	144	} else {
155	if (error > maxError[axis] - OVER360) {	145	if (error > maxError[axis] - OVER360) {
156	// too much err.	146	// too much err.
157	error = maxError[axis];	147	error = maxError[axis];
158	target[axis] = attitude[axis] - maxError[axis];	148	target[axis] = attitude[axis] - maxError[axis];
159	if (target[axis] < -OVER180) target[axis] += OVER360;	149	if (target[axis] < -OVER180) target[axis] += OVER360;
160	} else {	150	} else {
161	// Normal case, we just need to correct for the wrap. Error will be negative.	151	// Normal case, we just need to correct for the wrap. Error will be negative.
162	error += OVER360;	152	error += OVER360;
163	}	153	}
164	}	154	}
165	} else {	155	} else {
166	// Simple case, linear range.	156	// Simple case, linear range.
167	if (error > maxError[axis]) {	157	if (error > maxError[axis]) {
168	error = maxError[axis];	158	error = maxError[axis];
169	target[axis] = attitude[axis] - maxError[axis];	159	target[axis] = attitude[axis] - maxError[axis];
170	} else if (error < -maxError[axis]) {	160	} else if (error < -maxError[axis]) {
171	error = -maxError[axis];	161	error = -maxError[axis];
172	target[axis] = attitude[axis] + maxError[axis];	162	target[axis] = attitude[axis] + maxError[axis];
173	}	163	}
174	}	164	}
175	#endif	165	#endif
176	#ifdef TUNCATEERROR	166	#ifdef TUNCATEERROR
177	if (error > maxError[axis]) {	167	if (error > maxError[axis]) {
178	error = maxError[axis];	168	error = maxError[axis];
179	} else if (error < -maxError[axis]) {	169	} else if (error < -maxError[axis]) {
180	error = -maxError[axis];	170	error = -maxError[axis];
181	} else {	171	} else {
182	// update I parts here for angles mode. I parts in rate mode is something different.	172	// update I parts here for angles mode. I parts in rate mode is something different.
183	}	173	}
184	#endif	174	#endif
185		175
186	debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	176	debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
187		177
188	/************************************************************************/	178	/************************************************************************/
189	/* Calculate control feedback from angle (gyro integral) */	179	/* Calculate control feedback from angle (gyro integral) */
190	/* and angular velocity (gyro signal) */	180	/* and angular velocity (gyro signal) */
191	/************************************************************************/	181	/************************************************************************/
192	if (currentFlightMode == FLIGHT_MODE_ANGLES \|\| currentFlightMode == FLIGHT_MODE_RATE) {	182	if (currentFlightMode == FLIGHT_MODE_ANGLES \|\| currentFlightMode == FLIGHT_MODE_RATE) {
193	PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)	183	PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
194	+ (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);	184	+ (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
195	//if (reverse[axis])	-
196	// PDPart[axis] = -PDPart[axis];	-
197	} else {	185	} else {
198	PDPart[axis] = 0;	186	PDPart[axis] = 0;
199	}	187	}
200		188
201	if (currentFlightMode == FLIGHT_MODE_ANGLES) {	189	if (currentFlightMode == FLIGHT_MODE_ANGLES) {
202	int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;	190	int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
203	PDPart[axis] += anglePart;	191	PDPart[axis] += anglePart;
204	}	192	}
205		193
206	// Add I parts here... these are integrated errors.	194	// Add I parts here... these are integrated errors.
207	if (reverse[axis])	195	if (staticParams.servos[axis].reverse)
208	term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];	196	term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
209	else	197	else
210	term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];	198	term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
211	}	199	}
212		-
213	debugOut.analog[12] = term[PITCH];	-
214	debugOut.analog[13] = term[ROLL];	-
215	debugOut.analog[14] = term[YAW];	-
216	debugOut.analog[15] = term[THROTTLE];	-
217		200
218	for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {	201	for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
219	int16_t tmp;	202	int16_t tmp;
220	if (servoTestActive) {	203	if (servoTestActive) {
221	controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;	204	controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
222	} else {	205	} else {
223	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.	206	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
224	switch (i) {	207	switch (i) {
225	case 0:	208	case 0:
226	tmp = term[ROLL];	209	tmp = term[ROLL];
227	break;	210	break;
228	case 1:	211	case 1:
229	tmp = term[PITCH];	212	tmp = term[PITCH];
230	break;	213	break;
231	case 2:	214	case 2:
232	tmp = term[THROTTLE];	215	tmp = term[THROTTLE];
233	break;	216	break;
234	case 3:	217	case 3:
235	tmp = term[YAW];	218	tmp = term[YAW];
236	break;	219	break;
237	default:	220	default:
238	tmp = 0;	221	tmp = 0;
239	}	222	}
240	// These are all signed and in the same units as the RC stuff in rc.c.	223	// These are all signed and in the same units as the RC stuff in rc.c.
241	controlServos[i] = tmp;	224	controlServos[i] = tmp;
242	}	225	}
243	}	226	}
244		227
245	calculateControlServoValues();	228	calculateControlServoValues();
246		229
247	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	230	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
248	// Debugging	231	// Debugging
249	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	232	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
250	if (!(--debugDataTimer)) {	233	if (!(--debugDataTimer)) {
251	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	234	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
252	debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg	235	debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
253	debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg	236	debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
254	debugOut.analog[2] = gyro_PID[YAW];	237	debugOut.analog[2] = gyro_PID[YAW];
255		238
256	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	239	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
257	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	240	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
258	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);	241	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
259		242
260	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	243	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
261	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg	244	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
262	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);	245	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
263		246
264	debugOut.analog[12] = term[PITCH];	247	debugOut.analog[12] = term[PITCH];
265	debugOut.analog[13] = term[ROLL];	248	debugOut.analog[13] = term[ROLL];
266	debugOut.analog[14] = term[YAW];	249	debugOut.analog[14] = term[YAW];
-		250	debugOut.analog[15] = term[THROTTLE];
267	}	251	}
268	}	252	}
269		253

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c @ 2252 - Rev 2138 → 2142