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