Rev 1870 | Rev 1873 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1870 | Rev 1872 | ||
---|---|---|---|
Line 79... | Line 79... | ||
79 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
79 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
80 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
80 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
81 | */ |
81 | */ |
82 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
82 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
Line 83... | Line 83... | ||
83 | 83 | ||
84 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
84 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
Line 85... | Line 85... | ||
85 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
85 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
86 | 86 | ||
87 | // Some integral weight constant... |
87 | // Some integral weight constant... |
Line 88... | Line 88... | ||
88 | uint16_t Ki = 10300 / 33; |
88 | uint16_t Ki = 10300 / 33; |
89 | uint8_t RequiredMotors = 0; |
89 | uint8_t RequiredMotors = 0; |
90 | 90 | ||
91 | /************************************************************************/ |
91 | /************************************************************************/ |
92 | /* Filter for motor value smoothing (necessary???) */ |
92 | /* Filter for motor value smoothing (necessary???) */ |
93 | /************************************************************************/ |
93 | /************************************************************************/ |
94 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
94 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
95 | switch(dynamicParams.UserParams[5]) { |
95 | switch (dynamicParams.UserParams[5]) { |
96 | case 0: |
96 | case 0: |
97 | return newvalue; |
97 | return newvalue; |
98 | case 1: |
98 | case 1: |
99 | return (oldvalue + newvalue) / 2; |
99 | return (oldvalue + newvalue) / 2; |
100 | case 2: |
100 | case 2: |
101 | if(newvalue > oldvalue) |
101 | if (newvalue > oldvalue) |
102 | return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
102 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
103 | else |
103 | else |
104 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
104 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
105 | case 3: |
105 | case 3: |
106 | if(newvalue < oldvalue) |
106 | if (newvalue < oldvalue) |
- | 107 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
|
107 | return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
108 | else |
108 | else |
109 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
109 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
110 | default: |
Line 110... | Line 111... | ||
110 | default: return newvalue; |
111 | return newvalue; |
111 | } |
112 | } |
Line 121... | Line 122... | ||
121 | dynamicParams.KalmanMaxDrift = 0; |
122 | dynamicParams.KalmanMaxDrift = 0; |
122 | dynamicParams.KalmanMaxFusion = 32; |
123 | dynamicParams.KalmanMaxFusion = 32; |
123 | controlMixer_initVariables(); |
124 | controlMixer_initVariables(); |
124 | } |
125 | } |
Line -... | Line 126... | ||
- | 126 | ||
125 | 127 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
|
126 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
128 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
127 | Ki = 10300 / _Ki; |
129 | Ki = 10300 / _Ki; |
128 | gyroPFactor = _gyroPFactor; |
130 | gyroPFactor = _gyroPFactor; |
129 | gyroIFactor = _gyroIFactor; |
131 | gyroIFactor = _gyroIFactor; |
130 | yawPFactor = _yawPFactor; |
132 | yawPFactor = _yawPFactor; |
131 | yawIFactor = _yawIFactor; |
133 | yawIFactor = _yawIFactor; |
Line 132... | Line 134... | ||
132 | } |
134 | } |
133 | 135 | ||
134 | void setNormalFlightParameters(void) { |
- | |
135 | setFlightParameters(dynamicParams.IFactor + 1, |
136 | void setNormalFlightParameters(void) { |
136 | dynamicParams.GyroP + 10, |
- | |
137 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
137 | setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
138 | dynamicParams.GyroP + 10, |
- | |
139 | dynamicParams.UserParams[6] |
138 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
Line 140... | Line 139... | ||
140 | ); |
139 | dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
141 | } |
140 | } |
142 | 141 | ||
Line 143... | Line -... | ||
143 | void setStableFlightParameters(void) { |
- | |
144 | setFlightParameters(33, 90, 120, 90, 120); |
142 | void setStableFlightParameters(void) { |
145 | } |
143 | setFlightParameters(33, 90, 120, 90, 120); |
146 | 144 | } |
|
147 | 145 | ||
148 | /************************************************************************/ |
146 | /************************************************************************/ |
149 | /* Main Flight Control */ |
147 | /* Main Flight Control */ |
150 | /************************************************************************/ |
148 | /************************************************************************/ |
Line 151... | Line 149... | ||
151 | void flight_control(void) { |
149 | void flight_control(void) { |
152 | int16_t tmp_int; |
150 | int16_t tmp_int; |
153 | // Mixer Fractions that are combined for Motor Control |
151 | // Mixer Fractions that are combined for Motor Control |
154 | int16_t yawTerm, throttleTerm, term[2]; |
152 | int16_t yawTerm, throttleTerm, term[2]; |
Line 155... | Line 153... | ||
155 | 153 | ||
156 | // PID controller variables |
154 | // PID controller variables |
157 | int16_t PDPart[2], PDPartYaw, PPart[2]; |
155 | int16_t PDPart[2], PDPartYaw, PPart[2]; |
Line 171... | Line 169... | ||
171 | uint8_t i, axis; |
169 | uint8_t i, axis; |
Line 172... | Line 170... | ||
172 | 170 | ||
173 | // Fire the main flight attitude calculation, including integration of angles. |
171 | // Fire the main flight attitude calculation, including integration of angles. |
174 | // We want that to kick as early as possible, not to delay new AD sampling further. |
172 | // We want that to kick as early as possible, not to delay new AD sampling further. |
175 | calculateFlightAttitude(); |
- | |
176 | 173 | calculateFlightAttitude(); |
|
177 | controlMixer_update(); |
- | |
178 | 174 | controlMixer_update(); |
|
- | 175 | throttleTerm = controlThrottle; |
|
179 | throttleTerm = controlThrottle; |
176 | |
180 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
177 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
- | 178 | if (throttleTerm < staticParams.MinThrottle + 10) |
|
181 | if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
179 | throttleTerm = staticParams.MinThrottle + 10; |
- | 180 | else if (throttleTerm > staticParams.MaxThrottle - 20) |
|
Line 182... | Line 181... | ||
182 | else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
181 | throttleTerm = (staticParams.MaxThrottle - 20); |
183 | 182 | ||
184 | /************************************************************************/ |
183 | /************************************************************************/ |
185 | /* RC-signal is bad */ |
184 | /* RC-signal is bad */ |
186 | /* This part could be abstracted, as having yet another control input */ |
185 | /* This part could be abstracted, as having yet another control input */ |
Line 187... | Line 186... | ||
187 | /* to the control mixer: An emergency autopilot control. */ |
186 | /* to the control mixer: An emergency autopilot control. */ |
188 | /************************************************************************/ |
187 | /************************************************************************/ |
189 | 188 | ||
190 | if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
189 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
191 | RED_ON; |
190 | RED_ON; |
192 | beepRCAlarm(); |
191 | beepRCAlarm(); |
193 | 192 | ||
194 | if(emergencyFlightTime) { |
193 | if (emergencyFlightTime) { |
195 | // continue emergency flight |
194 | // continue emergency flight |
196 | emergencyFlightTime--; |
195 | emergencyFlightTime--; |
197 | if(isFlying > 256) { |
196 | if (isFlying > 256) { |
198 | // We're probably still flying. Descend slowly. |
197 | // We're probably still flying. Descend slowly. |
199 | throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
198 | throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
200 | MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
199 | MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
201 | setStableFlightParameters(); |
200 | setStableFlightParameters(); |
202 | } else { |
201 | } else { |
203 | MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
202 | MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
204 | } |
203 | } |
205 | } else { |
204 | } else { |
206 | // end emergency flight (just cut the motors???) |
205 | // end emergency flight (just cut the motors???) |
207 | MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
206 | MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
208 | } |
207 | } |
209 | } else { |
208 | } else { |
210 | // signal is acceptable |
209 | // signal is acceptable |
211 | if(controlMixer_getSignalQuality() > SIGNAL_BAD) { |
210 | if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
212 | // Reset emergency landing control variables. |
211 | // Reset emergency landing control variables. |
213 | MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
212 | MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
Line 214... | Line 213... | ||
214 | // The time is in whole seconds. |
213 | // The time is in whole seconds. |
215 | emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488; |
214 | emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
216 | } |
215 | } |
217 | 216 | ||
- | 217 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
|
218 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
218 | if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
219 | if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
219 | // increment flight-time counter until overflow. |
220 | // increment flight-time counter until overflow. |
220 | if (isFlying != 0xFFFF) |
221 | if(isFlying != 0xFFFF) isFlying++; |
221 | isFlying++; |
222 | } else |
222 | } else |
223 | /* |
223 | /* |
224 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
224 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
225 | * Probably to avoid integration effects that will cause the copter to spin |
225 | * Probably to avoid integration effects that will cause the copter to spin |
226 | * or flip when taking off. |
226 | * or flip when taking off. |
227 | */ |
227 | */ |
228 | if(isFlying < 256) { |
228 | if (isFlying < 256) { |
229 | IPart[PITCH] = IPart[ROLL] = 0; |
229 | IPart[PITCH] = IPart[ROLL] = 0; |
230 | // TODO: Don't stomp on other modules' variables!!! |
230 | // TODO: Don't stomp on other modules' variables!!! |
231 | // controlYaw = 0; |
231 | // controlYaw = 0; |
232 | PDPartYaw = 0; // instead. |
232 | PDPartYaw = 0; // instead. |
233 | if(isFlying == 250) { |
- | |
234 | // HC_setGround(); |
- | |
235 | updateCompassCourse = 1; |
- | |
236 | yawAngleDiff = 0; |
- | |
237 | } |
- | |
238 | } else { |
233 | if (isFlying == 250) { |
- | 234 | // HC_setGround(); |
|
- | 235 | updateCompassCourse = 1; |
|
- | 236 | yawAngleDiff = 0; |
|
- | 237 | } |
|
- | 238 | } else { |
|
Line 239... | Line 239... | ||
239 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
239 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
Line 240... | Line 240... | ||
240 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
240 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
241 | MKFlags |= (MKFLAG_FLY); |
241 | MKFlags |= (MKFLAG_FLY); |
242 | } |
242 | } |
243 | 243 | ||
Line 251... | Line 251... | ||
251 | /* |
251 | /* |
252 | * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
252 | * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
253 | * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
253 | * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
254 | * This is the throttle part. |
254 | * This is the throttle part. |
255 | */ |
255 | */ |
256 | if(looping) { |
256 | if (looping) { |
257 | if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
257 | if (throttleTerm > staticParams.LoopGasLimit) |
- | 258 | throttleTerm = staticParams.LoopGasLimit; |
|
258 | } |
259 | } |
Line 259... | Line 260... | ||
259 | 260 | ||
260 | /************************************************************************/ |
261 | /************************************************************************/ |
261 | /* Yawing */ |
262 | /* Yawing */ |
262 | /************************************************************************/ |
263 | /************************************************************************/ |
263 | if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
264 | if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
264 | ignoreCompassTimer = 1000; |
265 | ignoreCompassTimer = 1000; |
265 | if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
266 | if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
266 | updateCompassCourse = 1; |
267 | updateCompassCourse = 1; |
267 | } |
268 | } |
268 | } |
269 | } |
269 | 270 | ||
Line 270... | Line 271... | ||
270 | // yawControlRate = controlYaw; |
271 | // yawControlRate = controlYaw; |
271 | 272 | ||
272 | // Trim drift of yawAngleDiff with controlYaw. |
273 | // Trim drift of yawAngleDiff with controlYaw. |
273 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
274 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
274 | // This seems to be used as: Difference desired <--> real heading. |
275 | // This seems to be used as: Difference desired <--> real heading. |
275 | yawAngleDiff -= controlYaw; |
276 | yawAngleDiff -= controlYaw; |
276 | 277 | ||
277 | // limit the effect |
278 | // limit the effect |
278 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
279 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
279 | 280 | ||
280 | /************************************************************************/ |
281 | /************************************************************************/ |
281 | /* Compass is currently not supported. */ |
282 | /* Compass is currently not supported. */ |
282 | /************************************************************************/ |
283 | /************************************************************************/ |
283 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
284 | if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
284 | updateCompass(); |
285 | updateCompass(); |
285 | } |
286 | } |
286 | 287 | ||
287 | #if defined (USE_NAVICTRL) |
288 | #if defined (USE_NAVICTRL) |
288 | /************************************************************************/ |
289 | /************************************************************************/ |
289 | /* GPS is currently not supported. */ |
290 | /* GPS is currently not supported. */ |
Line 295... | Line 296... | ||
295 | } |
296 | } |
296 | #endif |
297 | #endif |
297 | // end part 1: 750-800 usec. |
298 | // end part 1: 750-800 usec. |
298 | // start part 3: 350 - 400 usec. |
299 | // start part 3: 350 - 400 usec. |
299 | #define SENSOR_LIMIT (4096 * 4) |
300 | #define SENSOR_LIMIT (4096 * 4) |
300 | /************************************************************************/ |
301 | /************************************************************************/ |
Line 301... | Line 302... | ||
301 | 302 | ||
302 | /* Calculate control feedback from angle (gyro integral) */ |
303 | /* Calculate control feedback from angle (gyro integral) */ |
303 | /* and angular velocity (gyro signal) */ |
304 | /* and angular velocity (gyro signal) */ |
304 | /************************************************************************/ |
305 | /************************************************************************/ |
Line 305... | Line 306... | ||
305 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
306 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
306 | 307 | ||
307 | for (axis=PITCH; axis<=ROLL; axis++) { |
308 | for (axis = PITCH; axis <= ROLL; axis++) { |
308 | if(looping & ((1<<4)<<axis)) { |
309 | if (looping & ((1 << 4) << axis)) { |
309 | PPart[axis] = 0; |
310 | PPart[axis] = 0; |
310 | } else { // TODO: Where do the 44000 come from??? |
311 | } else { // TODO: Where do the 44000 come from??? |
Line 311... | Line 312... | ||
311 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
312 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
312 | } |
313 | } |
313 | 314 | ||
314 | /* |
315 | /* |
315 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
316 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
316 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
317 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
- | 318 | * where pfactor is in [0..1]. |
|
317 | * where pfactor is in [0..1]. |
319 | */ |
Line 318... | Line 320... | ||
318 | */ |
320 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
319 | PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
321 | * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
Line 320... | Line -... | ||
320 | + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
- | |
321 | 322 | * (int16_t) dynamicParams.GyroD) / 16; |
|
322 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
323 | |
- | 324 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
|
323 | } |
325 | } |
324 | 326 | ||
325 | PDPartYaw = |
327 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
326 | (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
328 | / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
327 | + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
329 | / CONTROL_SCALING)); |
328 | 330 | ||
329 | // limit control feedback |
331 | // limit control feedback |
330 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
332 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
331 | 333 | ||
332 | /* |
334 | /* |
333 | * Compose throttle term. |
335 | * Compose throttle term. |
334 | * If a Bl-Ctrl is missing, prevent takeoff. |
336 | * If a Bl-Ctrl is missing, prevent takeoff. |
335 | */ |
337 | */ |
336 | if(missingMotor) { |
338 | if (missingMotor) { |
Line 337... | Line 339... | ||
337 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
339 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
Line 352... | Line 354... | ||
352 | */ |
354 | */ |
353 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
355 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
354 | yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
356 | yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
355 | // Limit yawTerm |
357 | // Limit yawTerm |
356 | DebugOut.Digital[0] &= ~DEBUG_CLIP; |
358 | DebugOut.Digital[0] &= ~DEBUG_CLIP; |
357 | if(throttleTerm > MIN_YAWGAS) { |
359 | if (throttleTerm > MIN_YAWGAS) { |
358 | if (yawTerm < -throttleTerm/2) { |
360 | if (yawTerm < -throttleTerm / 2) { |
359 | DebugOut.Digital[0] |= DEBUG_CLIP; |
361 | DebugOut.Digital[0] |= DEBUG_CLIP; |
360 | yawTerm = -throttleTerm/2; |
362 | yawTerm = -throttleTerm / 2; |
361 | } else if (yawTerm > throttleTerm/2) { |
363 | } else if (yawTerm > throttleTerm / 2) { |
362 | DebugOut.Digital[0] |= DEBUG_CLIP; |
364 | DebugOut.Digital[0] |= DEBUG_CLIP; |
363 | yawTerm = throttleTerm/2; |
365 | yawTerm = throttleTerm / 2; |
364 | } |
366 | } |
365 | //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
367 | //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
366 | } else { |
368 | } else { |
367 | if (yawTerm < -MIN_YAWGAS/2) { |
369 | if (yawTerm < -MIN_YAWGAS / 2) { |
368 | DebugOut.Digital[0] |= DEBUG_CLIP; |
370 | DebugOut.Digital[0] |= DEBUG_CLIP; |
369 | yawTerm = -MIN_YAWGAS/2; |
371 | yawTerm = -MIN_YAWGAS / 2; |
370 | } else if (yawTerm > MIN_YAWGAS/2) { |
372 | } else if (yawTerm > MIN_YAWGAS / 2) { |
371 | DebugOut.Digital[0] |= DEBUG_CLIP; |
373 | DebugOut.Digital[0] |= DEBUG_CLIP; |
372 | yawTerm = MIN_YAWGAS/2; |
374 | yawTerm = MIN_YAWGAS / 2; |
373 | } |
375 | } |
374 | //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
376 | //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
375 | } |
377 | } |
Line 376... | Line 378... | ||
376 | 378 | ||
Line 384... | Line 386... | ||
384 | DebugOut.Digital[0] |= DEBUG_CLIP; |
386 | DebugOut.Digital[0] |= DEBUG_CLIP; |
385 | } |
387 | } |
Line 386... | Line 388... | ||
386 | 388 | ||
387 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
389 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
388 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
390 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
389 | for (axis=PITCH; axis<=ROLL; axis++) { |
391 | for (axis = PITCH; axis <= ROLL; axis++) { |
390 | /* |
392 | /* |
391 | * Compose pitch and roll terms. This is finally where the sticks come into play. |
393 | * Compose pitch and roll terms. This is finally where the sticks come into play. |
392 | */ |
394 | */ |
393 | if(gyroIFactor) { |
395 | if (gyroIFactor) { |
394 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
396 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
395 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
397 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
396 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
398 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
397 | IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
399 | IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
398 | } else { |
400 | } else { |
399 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
401 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
400 | // To keep up with a full stick PDPart should be about 156... |
402 | // To keep up with a full stick PDPart should be about 156... |
401 | IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
403 | IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
Line 402... | Line 404... | ||
402 | } |
404 | } |
- | 405 | ||
Line 403... | Line 406... | ||
403 | 406 | tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
|
404 | tmp_int = (int32_t)((int32_t) dynamicParams.DynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
407 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
405 | 408 | ||
406 | // TODO: From which planet comes the 16000? |
409 | // TODO: From which planet comes the 16000? |
Line 407... | Line 410... | ||
407 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
410 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
408 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
411 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
409 | term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
412 | term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
410 | 413 | ||
Line 419... | Line 422... | ||
419 | } else if (term[axis] > tmp_int) { |
422 | } else if (term[axis] > tmp_int) { |
420 | DebugOut.Digital[1] |= DEBUG_CLIP; |
423 | DebugOut.Digital[1] |= DEBUG_CLIP; |
421 | } |
424 | } |
422 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
425 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
423 | } |
426 | } |
424 | // end part 3: 350 - 400 usec. |
- | |
Line 425... | Line 427... | ||
425 | 427 | ||
426 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
428 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
427 | // Universal Mixer |
429 | // Universal Mixer |
428 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
430 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
Line 431... | Line 433... | ||
431 | DebugOut.Analog[12] = term[PITCH]; |
433 | DebugOut.Analog[12] = term[PITCH]; |
432 | DebugOut.Analog[13] = term[ROLL]; |
434 | DebugOut.Analog[13] = term[ROLL]; |
433 | DebugOut.Analog[14] = yawTerm; |
435 | DebugOut.Analog[14] = yawTerm; |
434 | DebugOut.Analog[15] = throttleTerm; |
436 | DebugOut.Analog[15] = throttleTerm; |
Line 435... | Line 437... | ||
435 | 437 | ||
436 | for(i = 0; i < MAX_MOTORS; i++) { |
438 | for (i = 0; i < MAX_MOTORS; i++) { |
437 | int16_t tmp; |
439 | int16_t tmp; |
438 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
439 | tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
441 | tmp = throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
440 | tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
442 | tmp += term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
441 | tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
443 | tmp += term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
- | 444 | tmp += yawTerm * Mixer.Motor[i][MIX_YAW]; |
|
442 | tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
445 | tmp = tmp >> 6; |
443 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
446 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
444 | // Now we scale back down to a 0..255 range. |
447 | // Now we scale back down to a 0..255 range. |
Line 445... | Line 448... | ||
445 | tmp = motorFilters[i] / MOTOR_SCALING; |
448 | tmp = motorFilters[i] / MOTOR_SCALING; |
Line 449... | Line 452... | ||
449 | // all motors together) or should it limit the throttle set for each motor, |
452 | // all motors together) or should it limit the throttle set for each motor, |
450 | // including mix components of pitch, roll and yaw? I think only the common |
453 | // including mix components of pitch, roll and yaw? I think only the common |
451 | // throttle should be limited. |
454 | // throttle should be limited. |
452 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
455 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
453 | // Apply to individual signals instead. |
456 | // Apply to individual signals instead. |
454 | CHECK_MIN_MAX(tmp, 8, 255); |
457 | CHECK_MIN_MAX(tmp, 1, 255); |
455 | motor[i].SetPoint = tmp; |
458 | motor[i].SetPoint = tmp; |
456 | } |
- | |
457 | else if (motorTestActive) { |
459 | } else if (motorTestActive) { |
458 | motor[i].SetPoint = motorTest[i]; |
460 | motor[i].SetPoint = motorTest[i]; |
459 | } else { |
461 | } else { |
460 | motor[i].SetPoint = 0; |
462 | motor[i].SetPoint = 0; |
461 | } |
463 | } |
462 | if (i < 4) |
464 | if (i < 4) |
463 | DebugOut.Analog[22+i] = motor[i].SetPoint; |
465 | DebugOut.Analog[22 + i] = motor[i].SetPoint; |
464 | } |
466 | } |
- | 467 | ||
465 | I2C_Start(TWI_STATE_MOTOR_TX); |
468 | I2C_Start(TWI_STATE_MOTOR_TX); |
466 | 469 | ||
467 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
470 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
468 | // Debugging |
471 | // Debugging |
469 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
470 | if(!(--debugDataTimer)) { |
473 | if (!(--debugDataTimer)) { |
471 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
474 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
472 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
475 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
473 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
476 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
474 | DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
477 | DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
Line 475... | Line 478... | ||
475 | 478 | ||
476 | /* |
479 | /* |
477 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
480 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
478 | DebugOut.Analog[24] = controlYaw; |
481 | DebugOut.Analog[24] = controlYaw; |
479 | DebugOut.Analog[25] = yawAngleDiff / 100L; |
482 | DebugOut.Analog[25] = yawAngleDiff / 100L; |
480 | DebugOut.Analog[26] = accNoisePeak[PITCH]; |
483 | DebugOut.Analog[26] = accNoisePeak[PITCH]; |
481 | DebugOut.Analog[27] = accNoisePeak[ROLL]; |
484 | DebugOut.Analog[27] = accNoisePeak[ROLL]; |
482 | DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
485 | DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
483 | DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
486 | DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
484 | */ |
487 | */ |
485 | } |
488 | } |