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 example: 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"

#include "output.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"

#include "commands.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;

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;

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

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

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

int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {

  switch (dynamicParams.motorSmoothing) {

  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() {

  MKFlags |= MKFLAG_CALIBRATE;

  // not really used here any more.

  /*

  dynamicParams.KalmanK = -1;

  dynamicParams.KalmanMaxDrift = 0;

  dynamicParams.KalmanMaxFusion = 32;

  */

  controlMixer_initVariables();

}

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,

                      dynamicParams.gyroP,

                      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,

                      dynamicParams.gyroP,

                      dynamicParams.yawIFactor

                      );

}

void setStableFlightParameters(void) {

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

}

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

/*  Main Flight Control                                                 */

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

void flight_control(void) {

  int16_t tmp_int;

  // Mixer Fractions that are combined for Motor Control

  int16_t yawTerm, throttleTerm, term[2];

  // PID controller variables

  int16_t PDPart[2], PDPartYaw, PPart[2];

  static int32_t IPart[2] = { 0, 0 };

  //  static int32_t yawControlRate = 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;

  static int8_t debugDataTimer = 1;

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

  static int16_t motorFilters[MAX_MOTORS];

  uint8_t i, axis;

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

  // We want that to kick as early as possible, not to delay new AD sampling further.

  calculateFlightAttitude();

  controlMixer_update();

  throttleTerm = controls[CONTROL_THROTTLE];

  // This check removed. Is done on a per-motor basis, after output matrix multiplication.

  if (throttleTerm < staticParams.minThrottle + 10)

    throttleTerm = staticParams.minThrottle + 10;

  else if (throttleTerm > staticParams.maxThrottle - 20)

    throttleTerm = (staticParams.maxThrottle - 20);

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

  /* 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

    beepRCAlarm();

    if (emergencyFlightTime) {

      // continue emergency flight

      emergencyFlightTime--;

      if (isFlying > 256) {

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

        throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle

        MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // 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_FLIGHT);

    }

  } else {

    // signal is acceptable

    if (controlMixer_getSignalQuality() > SIGNAL_BAD) {

      // Reset emergency landing control variables.

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

      // The time is in whole seconds.

      emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 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.

     */

      if (isFlying < 256) {

        IPart[PITCH] = IPart[ROLL] = 0;

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

        // controlYaw = 0;

        PDPartYaw = 0; // instead.

        if (isFlying == 250) {

          // HC_setGround();

          updateCompassCourse = 1;

          yawAngleDiff = 0;

        }

      } else {

        // 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);

      }

    commands_handleCommands();

    setNormalFlightParameters();

  } // end else (not bad signal case)

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

  /*  Yawing                                                              */

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

  if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated

    ignoreCompassTimer = 1000;

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

      updateCompassCourse = 1;

    }

  }

  // yawControlRate = controlYaw;

  // Trim drift of yawAngleDiff with controlYaw.

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

  // This seems to be used as: Difference desired <--> real heading.

  yawAngleDiff -= controls[CONTROL_YAW];

  // limit the effect

  CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);

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

  /* Compass is currently not supported.                                  */

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

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

    updateCompass();

  }

#if defined (USE_NAVICTRL)

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

  /* GPS is currently not supported.                                      */

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

  if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {

    GPS_Main();

    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);

  } else {

  }

#endif

  // end part 1: 750-800 usec.

  // start part 3: 350 - 400 usec.

#define SENSOR_LIMIT  (4096 * 4)

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

  /* 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).

  for (axis = PITCH; axis <= ROLL; axis++) {

    PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral

    /*

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

     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING

     * where pfactor is in [0..1].

     */

    PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]

                                                                                         * (int16_t) dynamicParams.gyroD) / 16;

    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);

  }

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

      / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000

      / CONTROL_SCALING));

  // limit control feedback

  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

  }

  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?

  throttleTerm *= CONTROL_SCALING;

  /*

   * Compose yaw term.

   * The yaw term is limited: Absolute value is max. = the throttle term / 2.

   * However, at low throttle the yaw term is limited to a fixed value,

   * and at high throttle it is limited by the throttle reserve (the difference

   * between current throttle and maximum throttle).

   */

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

  yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING;

  // Limit yawTerm

  debugOut.digital[0] &= ~DEBUG_CLIP;

  if (throttleTerm > MIN_YAWGAS) {

    if (yawTerm < -throttleTerm / 2) {

      debugOut.digital[0] |= DEBUG_CLIP;

      yawTerm = -throttleTerm / 2;

    } else if (yawTerm > throttleTerm / 2) {

      debugOut.digital[0] |= DEBUG_CLIP;

      yawTerm = throttleTerm / 2;

    }

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

  } else {

    if (yawTerm < -MIN_YAWGAS / 2) {

      debugOut.digital[0] |= DEBUG_CLIP;

      yawTerm = -MIN_YAWGAS / 2;

    } else if (yawTerm > MIN_YAWGAS / 2) {

      debugOut.digital[0] |= DEBUG_CLIP;

      yawTerm = MIN_YAWGAS / 2;

    }

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

  }

  // FIXME: Throttle may exceed maxThrottle (there is no check no more).

  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;

  if (yawTerm < -(tmp_int - throttleTerm)) {

    yawTerm = -(tmp_int - throttleTerm);

    debugOut.digital[0] |= DEBUG_CLIP;

  } else if (yawTerm > (tmp_int - throttleTerm)) {

    yawTerm = (tmp_int - throttleTerm);

    debugOut.digital[0] |= DEBUG_CLIP;

  }

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

  debugOut.digital[1] &= ~DEBUG_CLIP;

  for (axis = PITCH; axis <= ROLL; axis++) {

    /*

     * 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.

      IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.

    } else {

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

      // To keep up with a full stick PDPart should be about 156...

      IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.

    }

    tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability

        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;

    // TODO: From which planet comes the 16000?

    CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));

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

    term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch

    /*

     * 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?

     */

    if (term[axis] < -tmp_int) {

      debugOut.digital[1] |= DEBUG_CLIP;

    } else if (term[axis] > tmp_int) {

      debugOut.digital[1] |= DEBUG_CLIP;

    }

    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);

  }

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

  // Universal Mixer

  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].

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

  debugOut.analog[3]  = rate_ATT[PITCH];

  debugOut.analog[4]  = rate_ATT[ROLL];

  debugOut.analog[5]  = yawRate;

  debugOut.analog[6]  = filteredAcc[PITCH];

  debugOut.analog[7]  = filteredAcc[ROLL];

  debugOut.analog[8]  = filteredAcc[Z];

  debugOut.analog[12] = term[PITCH];

  debugOut.analog[13] = term[ROLL];

  debugOut.analog[14] = yawTerm;

  debugOut.analog[15] = throttleTerm;

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

    int32_t tmp;

    uint8_t throttle;

    tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];

    tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];

    tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];

    tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];

    tmp = tmp >> 6;

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

    // Now we scale back down to a 0..255 range.

    tmp = motorFilters[i] / MOTOR_SCALING;

    // So this was the THIRD time a throttle was limited. But should the limitation

    // apply to the common throttle signal (the one used for setting the "power" of

    // all motors together) or should it limit the throttle set for each motor,

    // including mix components of pitch, roll and yaw? I think only the common

    // throttle should be limited.

    // --> WRONG. This caused motors to stall completely in tight maneuvers.

    // Apply to individual signals instead.

    CHECK_MIN_MAX(tmp, 1, 255);

    throttle = tmp;

    if (i < 4) debugOut.analog[22 + i] = throttle;

    if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {

      motor[i].SetPoint = throttle;

    } else if (motorTestActive) {

      motor[i].SetPoint = motorTest[i];

    } else {

      motor[i].SetPoint = 0;

    }

  }

  I2C_Start(TWI_STATE_MOTOR_TX);

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

  // Debugging

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

  if (!(--debugDataTimer)) {

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

    debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;

    debugOut.analog[16] = gyroPFactor;

    debugOut.analog[17] = gyroIFactor;

    debugOut.analog[18] = dynamicParams.gyroD;

  }

}