Rev 1864 | Rev 1868 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1864 | Rev 1867 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | \// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur f�r den privaten Gebrauch |
3 | // + Nur f�r den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
Line 118... | Line 118... | ||
118 | /************************************************************************/ |
118 | /************************************************************************/ |
119 | /* Neutral Readings */ |
119 | /* Neutral Readings */ |
120 | /************************************************************************/ |
120 | /************************************************************************/ |
121 | void flight_setNeutral() { |
121 | void flight_setNeutral() { |
122 | MKFlags |= MKFLAG_CALIBRATE; |
122 | MKFlags |= MKFLAG_CALIBRATE; |
123 | - | ||
124 | // not really used here any more. |
123 | // not really used here any more. |
125 | dynamicParams.KalmanK = -1; |
124 | dynamicParams.KalmanK = -1; |
126 | dynamicParams.KalmanMaxDrift = 0; |
125 | dynamicParams.KalmanMaxDrift = 0; |
127 | dynamicParams.KalmanMaxFusion = 32; |
126 | dynamicParams.KalmanMaxFusion = 32; |
128 | - | ||
129 | controlMixer_initVariables(); |
127 | controlMixer_initVariables(); |
130 | } |
128 | } |
Line 131... | Line 129... | ||
131 | 129 | ||
132 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
130 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Line 260... | Line 258... | ||
260 | * This is the throttle part. |
258 | * This is the throttle part. |
261 | */ |
259 | */ |
262 | if(looping) { |
260 | if(looping) { |
263 | if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
261 | if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
264 | } |
262 | } |
- | 263 | ||
- | 264 | J5LOW; |
|
Line 265... | Line 265... | ||
265 | 265 | ||
266 | /************************************************************************/ |
266 | /************************************************************************/ |
267 | /* Yawing */ |
267 | /* Yawing */ |
268 | /************************************************************************/ |
268 | /************************************************************************/ |
Line 346... | Line 346... | ||
346 | } |
346 | } |
Line 347... | Line 347... | ||
347 | 347 | ||
348 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
348 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
Line -... | Line 349... | ||
- | 349 | throttleTerm *= CONTROL_SCALING; |
|
- | 350 | ||
349 | throttleTerm *= CONTROL_SCALING; |
351 | J5HIGH; |
350 | 352 | ||
351 | /* |
353 | /* |
352 | * Compose yaw term. |
354 | * Compose yaw term. |
353 | * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
355 | * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
Line 386... | Line 388... | ||
386 | DebugOut.Digital[0] |= DEBUG_CLIP; |
388 | DebugOut.Digital[0] |= DEBUG_CLIP; |
387 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
389 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
388 | yawTerm = (tmp_int - throttleTerm); |
390 | yawTerm = (tmp_int - throttleTerm); |
389 | DebugOut.Digital[0] |= DEBUG_CLIP; |
391 | DebugOut.Digital[0] |= DEBUG_CLIP; |
390 | } |
392 | } |
- | 393 | ||
- | 394 | J5LOW; |
|
- | 395 | ||
391 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
396 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
392 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
397 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
393 | for (axis=PITCH; axis<=ROLL; axis++) { |
398 | for (axis=PITCH; axis<=ROLL; axis++) { |
394 | /* |
399 | /* |
395 | * Compose pitch and roll terms. This is finally where the sticks come into play. |
400 | * Compose pitch and roll terms. This is finally where the sticks come into play. |
Line 425... | Line 430... | ||
425 | } |
430 | } |
426 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
431 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
427 | } |
432 | } |
428 | // end part 3: 350 - 400 usec. |
433 | // end part 3: 350 - 400 usec. |
Line -... | Line 434... | ||
- | 434 | ||
- | 435 | J5HIGH; |
|
429 | 436 | ||
430 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
437 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
431 | // Universal Mixer |
438 | // Universal Mixer |
432 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
439 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
Line 466... | Line 473... | ||
466 | if (i < 4) |
473 | if (i < 4) |
467 | DebugOut.Analog[22+i] = motor[i].SetPoint; |
474 | DebugOut.Analog[22+i] = motor[i].SetPoint; |
468 | } |
475 | } |
469 | I2C_Start(TWI_STATE_MOTOR_TX); |
476 | I2C_Start(TWI_STATE_MOTOR_TX); |
Line -... | Line 477... | ||
- | 477 | ||
- | 478 | J5LOW; |
|
470 | 479 | ||
471 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
480 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // Debugging |
481 | // Debugging |
473 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
482 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
474 | if(!(--debugDataTimer)) { |
483 | if(!(--debugDataTimer)) { |
Line 485... | Line 494... | ||
485 | DebugOut.Analog[27] = accNoisePeak[ROLL]; |
494 | DebugOut.Analog[27] = accNoisePeak[ROLL]; |
486 | DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
495 | DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
487 | DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
496 | DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
488 | */ |
497 | */ |
489 | } |
498 | } |
490 | J5LOW; |
- | |
491 | } |
499 | } |