Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 04.2007 Holger Buss

// + Nur f�r den privaten Gebrauch

// + www.MikroKopter.com

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),

// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt

// + bzgl. der Nutzungsbedingungen aufzunehmen.

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,

// + Verkauf von Luftbildaufnahmen, usw.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt werden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur

// + mit unserer Zustimmung zul�ssig

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Redistributions of source code (with or without modifications) must retain the above copyright notice,

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be

// +     clearly linked as origin

// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <stdlib.h>

#include <avr/io.h>

#include "eeprom.h"

#include "flight.h"

// Only for debug. Remove.

#include "analog.h"

#include "rc.h"

// Necessary for external control and motor test

#include "uart0.h"

#include "twimaster.h"

#include "attitude.h"

#include "controlMixer.h"

#ifdef USE_MK3MAG

#include "gps.h"

#endif

#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}

/*

 * These are no longer maintained, just left at 0. The original implementation just summed the acc.

 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???

 */

int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;

// MK flags

uint16_t isFlying = 0;

volatile uint8_t MKFlags = 0;

uint8_t gyroPFactor, gyroIFactor;       // the PD factors for the attitude control

uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control

// Some integral weight constant...

uint16_t Ki = 10300 / 33;

uint8_t RequiredMotors = 0;

// No support for altitude control right now.

// int16_t SetPointHeight = 0;

/************************************************************************/

/*  Filter for motor value smoothing (necessary???)                     */

/************************************************************************/

int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {

  switch(dynamicParams.UserParams[5]) {

  case 0:

    return newvalue;

  case 1:

    return (oldvalue + newvalue) / 2;  

  case 2:

    if(newvalue > oldvalue)

      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new

    else       

      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old

  case 3:

    if(newvalue < oldvalue)

      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new

    else       

      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old

  default: return newvalue;

  }

}

/************************************************************************/

/*  Neutral Readings                                                    */

/************************************************************************/

void flight_setNeutral() {

  // GPSStickPitch = 0;

  // GPSStickRoll = 0;

  MKFlags |= MKFLAG_CALIBRATE;

  // not really used here any more.

  dynamicParams.KalmanK = -1;

  dynamicParams.KalmanMaxDrift = 0;

  dynamicParams.KalmanMaxFusion = 32;

  controlMixer_initVariables();

  // TODO: Move off.

  // RC_Quality = 100;

}

/************************************************************************/

/*  Transmit Motor Data via I2C                                         */

/************************************************************************/

void sendMotorData(void) {

  uint8_t i;

  if(!(MKFlags & MKFLAG_MOTOR_RUN)) {

    // If pilot has not started the engines....

    MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off

    for(i = 0; i < MAX_MOTORS; i++) {

      // and if we are not in motor test mode, cut throttle on all motors.

      if(!motorTestActive) Motor[i].SetPoint = 0;

      else                 Motor[i].SetPoint = motorTest[i];

    }

    if(motorTestActive) motorTestActive--;

  }

  /*

  DebugOut.Analog[12] = Motor[0].SetPoint; // Front

  DebugOut.Analog[13] = Motor[1].SetPoint; // Rear

  DebugOut.Analog[14] = Motor[3].SetPoint; // Left

  DebugOut.Analog[15] = Motor[2].SetPoint; // Right

  */

  // Start I2C Interrupt Mode

  I2C_Start(TWI_STATE_MOTOR_TX);

}

void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {

  Ki = 10300 / _Ki;

  gyroPFactor = _gyroPFactor;

  gyroIFactor = _gyroIFactor;

  yawPFactor = _yawPFactor;

  yawIFactor = _yawIFactor;

}

void setNormalFlightParameters(void) {

  setFlightParameters(dynamicParams.IFactor + 1,

                      dynamicParams.GyroP + 10,

                      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,

                      dynamicParams.GyroP + 10,

                      dynamicParams.UserParams[6]

                      );

}

void setStableFlightParameters(void) {

  setFlightParameters(33, 90, 120, 90, 120);

}

void handleCommands(uint8_t command, uint8_t argument, uint8_t isCommandRepeated) {

    if(!(MKFlags & MKFLAG_MOTOR_RUN)) {

      if (command == COMMAND_GYROCAL && !isCommandRepeated) {

        // Run gyro calibration but do not repeat it.

        GRN_OFF;

        // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?

        // isFlying = 0;

        // check roll/pitch stick position

        // if pitch stick is top or roll stick is left or right --> change parameter setting

        // according to roll/pitch stick position

        if (argument < 6) {

          // Gyro calinbration, with or without selecting a new parameter-set.

          if(argument > 0 && argument < 6) {

            // A valid parameter-set (1..5) was chosen - use it.

            setActiveParamSet(argument);

          }

          ParamSet_ReadFromEEProm(getActiveParamSet());

          attitude_setNeutral();

          flight_setNeutral();

          // Right stick is centered; calibrate it to zero (hmm strictly does not belong here).

          // If heading hold is active, do not do it. TODO: We also want to re-set old calibration.

          controlMixer_setNeutral(!argument);

          beepNumber(getActiveParamSet());

        } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) {

          // If right stick is centered and down

          compassCalState = 1;

          beep(1000);

        }

      }

      // save the ACC neutral setting to eeprom

      else  {

        if(command == COMMAND_ACCCAL && !isCommandRepeated) {

          // Run gyro and acc. meter calibration but do not repeat it.

          GRN_OFF;

          analog_calibrateAcc();

          attitude_setNeutral();

          flight_setNeutral();

          controlMixer_setNeutral(1); // Calibrate right stick neutral position.

          beepNumber(getActiveParamSet());

        }

      }

    } // end !MOTOR_RUN condition.

    if (command == COMMAND_START) {

      isFlying = 1; // TODO: Really????

      // if (!controlMixer_isCommandRepeated()) {

      // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.

      MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all???

      // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.

      // attitude_continueDynamicCalibration();

      // setPointYaw = 0;

      // IPartPitch = 0;

      // IPartRoll = 0;

      // }

    } else if (command == COMMAND_STOP) {

      isFlying = 0;

      MKFlags &= ~(MKFLAG_MOTOR_RUN);

    }

}

/************************************************************************/

/*  Main Flight Control                                                 */

/************************************************************************/

void flight_control(void) {

  int16_t tmp_int;

    // Mixer Fractions that are combined for Motor Control

  int16_t yawTerm, throttleTerm, pitchTerm, rollTerm;

  // PID controller variables

  int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll;

  static int32_t IPartPitch = 0, IPartRoll = 0;

  static int32_t setPointYaw = 0;

  // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.

  // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;

  // static int32_t CorrectionPitch, CorrectionRoll;

  static uint16_t emergencyFlightTime;

  // No support for altitude control right now.

  // static uint8_t HeightControlActive = 0;

  // static int16_t HeightControlGas = 0;

  static int8_t debugDataTimer = 1;

  // High resolution motor values for smoothing of PID motor outputs

  static int16_t motorFilters[MAX_MOTORS];

  uint8_t i;

  // Fire the main flight attitude calculation, including integration of angles.

  calculateFlightAttitude();

  GRN_ON;

  /*

   * TODO: update should: Set the stick variables if good signal, set them to zero if bad.

   * Set variables also.

   */

  controlMixer_update();

  throttleTerm = controlThrottle;

  if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;

  /************************************************************************/

  /* RC-signal is bad                                                     */

  /* This part could be abstracted, as having yet another control input   */

  /* to the control mixer: An emergency autopilot control.                */

  /************************************************************************/

  if(controlMixer_getSignalQuality() <= SIGNAL_BAD) {           // the rc-frame signal is not reveived or noisy

    RED_ON;

    beepRCAlarm();

    if(emergencyFlightTime) {

      // continue emergency flight

      emergencyFlightTime--;

      if(isFlying > 1000) {                    

        // We're probably still flying. Descend slowly.

        throttleTerm = staticParams.EmergencyGas;  // Set emergency throttle

        MKFlags |= (MKFLAG_EMERGENCY_LANDING);     // Set flag for emergency landing

        setStableFlightParameters();

      } else {

        MKFlags &= ~(MKFLAG_MOTOR_RUN);            // Probably not flying, and bad R/C signal. Kill motors.

      }

    } else {

      // end emergency flight (just cut the motors???)

      MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);

    }

  } else {

    // signal is acceptable

    if(controlMixer_getSignalQuality() > SIGNAL_BAD) {

      // Reset emergency landing control variables.

      MKFlags &= ~(MKFLAG_EMERGENCY_LANDING);  // clear flag for emergency landing

      // The time is in whole seconds.

      emergencyFlightTime = staticParams.EmergencyGasDuration * 488;

    }

    // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.

    if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {

      // increment flight-time counter until overflow.

      if(isFlying != 0xFFFF) isFlying++;

    } else

      /*

       * When standing on the ground, do not apply I controls and zero the yaw stick.

       * Probably to avoid integration effects that will cause the copter to spin

       * or flip when taking off.

       * TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already.

       */

      if(isFlying < 256) {

        IPartPitch = 0;

        IPartRoll = 0;

        // TODO: Don't stomp on other modules' variables!!!

        controlYaw = 0;

        if(isFlying == 250) {

          updateCompassCourse = 1;

          yawAngle = 0;

          setPointYaw = 0;

        }

      } else {

        // DebugOut.Digital[1] = 0;

        // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? 

        // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.

        MKFlags |= (MKFLAG_FLY);

      }

    /*

     * Get the current command (start/stop motors, calibrate), if any.

     */

    uint8_t command = controlMixer_getCommand();

    uint8_t repeated = controlMixer_isCommandRepeated();

    uint8_t argument = controlMixer_getArgument();

    handleCommands(command, argument, repeated);

    // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {

    setNormalFlightParameters();

    // }

  } // end else (not bad signal case)

  /*

   * Looping the H&I way basically is just a matter of turning off attitude angle measurement

   * by integration (because 300 deg/s gyros are too slow) and turning down the throttle.

   * This is the throttle part.

   */

  if(looping) {

    if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;

  }

  /************************************************************************/

  /*  Yawing                                                              */

  /************************************************************************/

  if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated

    badCompassHeading = 1000;

    if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {

      updateCompassCourse = 1;

    }

  }

  setPointYaw = controlYaw;

  // Trim drift of yawAngle with controlYaw.

  // TODO: We want NO feedback of control related stuff to the attitude related stuff.

  yawAngle -= controlYaw;

  // limit the effect

  CHECK_MIN_MAX(yawAngle, -50000, 50000)

    /************************************************************************/

    /* Compass is currently not supported.                                  */

    /************************************************************************/

    /*

      if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {

      updateCompass();

      }

    */

#if defined (USE_MK3MAG)

      // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    /************************************************************************/

    /* GPS is currently not supported.                                      */

    /************************************************************************/

    /*

      if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {

      GPS_Main();

      MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);

      }

      else {

      // GPSStickPitch = 0;

      // GPSStickRoll = 0;

      }

    */

#endif

    /************************************************************************/

    /* Calculate control feedback from angle (gyro integral)                */

    /* and angular velocity (gyro signal)                                   */

    /************************************************************************/

    // The P-part is the P of the PID controller. That's the angle integrals (not rates).

  if(looping & LOOPING_PITCH_AXIS) {

    PPartPitch = 0;

  } else { // TODO: Where do the 44000 come from???

    PPartPitch = pitchAngle * gyroIFactor / (44000 / STICK_GAIN); // P-Part - Proportional to Integral

  }

  // Now blend in the D-part - proportional to the Differential of the integral = the rate.

  PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor / (256L / STICK_GAIN))

    + (pitchDifferential * (int16_t)dynamicParams.GyroD) / 16;

  // The P-part is actually the I-part...

  if(looping & LOOPING_ROLL_AXIS) {

    PPartRoll = 0;

  } else { // TODO: Where do the 44000 come from???

    PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral

  }

  // Now blend in the P-part - proportional to the Differential of the integral = the rate

  PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor / (256L / STICK_GAIN))

    + (rollDifferential * (int16_t)dynamicParams.GyroD) / 16;

  PDPartYaw =  (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / STICK_GAIN)

    + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / STICK_GAIN));

  // limit control feedback

#define SENSOR_LIMIT  (4096 * 4)

  CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT);

  CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);

  CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);

  /*

   * Compose throttle term.

   * If a Bl-Ctrl is missing, prevent takeoff.

   */

  if(missingMotor) {

    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???

    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))

      isFlying = 1; // keep within lift off condition

    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of

  }

  /*

   * Height control was here.

   */

  if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);

  throttleTerm *= STICK_GAIN;

  /*

   * Compose yaw term.

   */

#define MIN_YAWGAS (40 * STICK_GAIN)  // yaw also below this gas value

  yawTerm = PDPartYaw - setPointYaw * STICK_GAIN;

  // limit yawTerm

  if(throttleTerm > MIN_YAWGAS) {

    /*

     * -throttle/2 < -20 <= yawTerm <= 20 < throttle/2

     */

    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));

  } else {

    /*

     * -20 <= yawTerm <= 20

     */

    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));

  }

  tmp_int = staticParams.MaxThrottle * STICK_GAIN;

  /*

   * throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle

   */

  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));

  /*

   * Compose pitch and roll terms. This is finally where the sticks come into play.

   */

  if(gyroIFactor) {

    // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.

    // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.

    // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.

    IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos.

    IPartRoll += PPartRoll - controlRoll;    // I-part for attitude control OK

  } else {

    // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.

    IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.

    IPartRoll += PDPartRoll - controlRoll;    // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.

  }

  // TODO: From which planet comes the 16000?

  CHECK_MIN_MAX(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));

  // Add (P, D) parts minus stick pos. to the scaled-down I part.

  pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki;    // PID-controller for pitch

  CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));

  rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki;  // PID-controller for roll

  /*

   * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).

   * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity

   * (max. pitch or roll term is the throttle value).

   * TODO: Why a growing function of yaw?

   */

  tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;

  CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int);

  CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int);

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  // Universal Mixer

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  for(i = 0; i < MAX_MOTORS; i++) {

    int16_t tmp;

    if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.

      tmp =  ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;

      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;

      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;

      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;

      motorFilters[i] = motorFilter(tmp, motorFilters[i]);

      tmp = motorFilters[i] / STICK_GAIN;

      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);

      Motor[i].SetPoint = tmp;

    }

    else Motor[i].SetPoint = 0;

  }

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  //  Debugwerte zuordnen

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  if(!(--debugDataTimer)) {

    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.

    DebugOut.Analog[0]  = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    DebugOut.Analog[1]  = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;

    DebugOut.Analog[9]  = setPointYaw;

    DebugOut.Analog[10] = yawIFactor;

    DebugOut.Analog[11] = gyroIFactor;

    DebugOut.Analog[12] = RC_getVariable(0);

    DebugOut.Analog[13] = dynamicParams.UserParams[0];

    DebugOut.Analog[14] = RC_getVariable(4);

    DebugOut.Analog[15] = dynamicParams.UserParams[4];

    /* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */

    // 12..15 are the controls.

    // DebugOut.Analog[16] = pitchAxisAcc;

    // DebugOut.Analog[17] = rollAxisAcc;

    // DebugOut.Analog[18] = ZAxisAcc;

    DebugOut.Analog[19] = throttleTerm;

    DebugOut.Analog[20] = pitchTerm;

    DebugOut.Analog[21] = rollTerm;

    DebugOut.Analog[22] = yawTerm;

    DebugOut.Analog[23] = PPartPitch;     //

    DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille.

    DebugOut.Analog[25] = PDPartPitch;    // omtrent lig ppart.

    DebugOut.Analog[26] = pitchAccNoisePeak;

    DebugOut.Analog[27] = rollAccNoisePeak;

    DebugOut.Analog[30] = pitchGyroNoisePeak;

    DebugOut.Analog[31] = rollGyroNoisePeak;

  }

}