Rev 2142 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
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 |