| 1,4 → 1,4 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| \// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur f�r den privaten Gebrauch |
| // + www.MikroKopter.com |
| 120,12 → 120,10 |
|---|
| /************************************************************************/ |
| void flight_setNeutral() { |
| MKFlags |= MKFLAG_CALIBRATE; |
| |
| // not really used here any more. |
| dynamicParams.KalmanK = -1; |
| dynamicParams.KalmanMaxDrift = 0; |
| dynamicParams.KalmanMaxFusion = 32; |
| |
| controlMixer_initVariables(); |
| } |
| |
| 262,6 → 260,8 |
|---|
| if(looping) { |
| if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
| } |
| |
| J5LOW; |
| |
| /************************************************************************/ |
| /* Yawing */ |
| 348,6 → 348,8 |
|---|
| // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
| throttleTerm *= CONTROL_SCALING; |
| |
| J5HIGH; |
| |
| /* |
| * Compose yaw term. |
| * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
| 388,6 → 390,9 |
|---|
| yawTerm = (tmp_int - throttleTerm); |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| } |
| |
| J5LOW; |
| |
| // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
| DebugOut.Digital[1] &= ~DEBUG_CLIP; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| 427,6 → 432,8 |
|---|
| } |
| // end part 3: 350 - 400 usec. |
| |
| J5HIGH; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |
| // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
| 468,6 → 475,8 |
|---|
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| |
| J5LOW; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 487,5 → 496,4 |
|---|
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| */ |
| } |
| J5LOW; |
| } |