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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 2142 → 2143