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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c @ 2122 - Rev 2131 → 2132