WebSVN - FlightCtrl - Diff - Rev 2052 and 2053 - /branches/dongfang_FC


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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"
#include "uart0.h"
 
// Necessary for external control and motor test
#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
#include "commands.h"
#include "heightControl.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "compassControl.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
 
 
uint8_t invKi = 64;
 
/************************************************************************/
/*  Filter for motor value smoothing (necessary???)                     */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
  switch (staticParams.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 _invKi, uint8_t _gyroPFactor,
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
  invKi = _invKi;
  gyroPFactor = _gyroPFactor;
  gyroIFactor = _gyroIFactor;
  yawPFactor = _yawPFactor;
  yawIFactor = _yawIFactor;
}
 
void setNormalFlightParameters(void) {
  setFlightParameters(
                      staticParams.IFactor,
                      dynamicParams.gyroP,
                      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
                      dynamicParams.gyroP,
                      staticParams.yawIFactor
                      );
}
 
void setStableFlightParameters(void) {
  setFlightParameters(0, 90, 120, 90, 120);
}
 
/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {
  uint16_t tmp_int;
  // Mixer Fractions that are combined for Motor Control
  int16_t yawTerm, throttleTerm, term[2];
 
  // PID controller variables
  int16_t PDPart;
  static int32_t IPart[2] = {0, 0};
  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;
 
  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
    if (controlMixer_didReceiveSignal) beepRCAlarm();  // Only make alarm if a control signal was received before the signal loss.
    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.
      if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP)
        emergencyFlightTime = 0xffff;
      else
        emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP;
    }
 
    // 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;
 
        if (isFlying == 250) {
          HC_setGround();
#ifdef USE_MK3MAG
          attitude_resetHeadingToMagnetic();
          compass_setTakeoffHeading(heading);
#endif
          // Set target heading to the one just gotten off compass.
          // targetHeading = heading;
        }
   } 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)
 
 
 
 
 
 
 
 
 
 
 
  // end part 1: 750-800 usec.
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
  // start part 3: 350 - 400 usec.
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
  // This is where control affects the target heading. It also (later) directly controls yaw.
  headingError -= controls[CONTROL_YAW];
  debugOut.analog[28] = headingError / 100;
  if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;
  if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;
 
  PDPart =  (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
  // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
 
 
 
  PDPart += (int32_t)(yawRate * yawPFactor) /  (GYRO_DEG_FACTOR_YAW >> 5);
 
  /*
   * 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 = PDPart - 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;
    }
  } 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;
    }
  }
 
  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;
  }
 
 
  debugOut.digital[1] &= ~DEBUG_CLIP;
 
  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6;
 
  /************************************************************************/
  /* 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++) {
    int16_t iDiff;
    iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
    PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
 
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
    // In HH mode, the I part is summed from P and D of gyros minus stick.
 
 
 
    if (gyroIFactor) {
      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
 
 
    } else {
      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
 
    }
 
 
 
    // With normal Ki, limit effect to +/- 205 (of 1024!!!)
    if (IPart[axis] < -64000) {
      IPart[axis] = -64000;
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
    } else if (IPart[axis] > 64000) {
      IPart[axis] = 64000;
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
    }
 
 
    term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14;
        term[axis] += (dynamicParams.levelCorrection[axis] - 128);
        /*
     * 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;
    }
  }
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // 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[13] = term[PITCH];
  debugOut.analog[14] = term[ROLL];
  debugOut.analog[15] = yawTerm;
  debugOut.analog[16] = 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].throttle = throttle;
    } else if (motorTestActive) {
      motor[i].throttle = motorTest[i];
    } else {
      motor[i].throttle = 0;
    }
  }
 
  I2C_Start(TWI_STATE_MOTOR_TX);
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Debugging
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if (!(--debugDataTimer)) {
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
    debugOut.analog[1] = attitude[ROLL]  / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
    debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
  }
}
 

Rev 2052	Rev 2053
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),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt werden	18	// + eindeutig als Ursprung verlinkt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25	// + mit unserer Zustimmung zulässig	25	// + mit unserer Zustimmung zulässig
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30	// + this list of conditions and the following disclaimer.	30	// + this list of conditions and the following disclaimer.
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32	// + from this software without specific prior written permission.	32	// + from this software without specific prior written permission.
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34	// + for non-commercial use (directly or indirectly)	34	// + for non-commercial use (directly or indirectly)
35	// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36	// + with our written permission	36	// + with our written permission
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38	// + clearly linked as origin	38	// + clearly linked as origin
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49	// + POSSIBILITY OF SUCH DAMAGE.	49	// + POSSIBILITY OF SUCH DAMAGE.
50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51		51
52	#include <stdlib.h>	52	#include <stdlib.h>
53	#include <avr/io.h>	53	#include <avr/io.h>
54	#include "eeprom.h"	54	#include "eeprom.h"
55	#include "flight.h"	55	#include "flight.h"
56	#include "output.h"	56	#include "output.h"
57	#include "uart0.h"	57	#include "uart0.h"
58		58
59	// Necessary for external control and motor test	59	// Necessary for external control and motor test
60	#include "twimaster.h"	60	#include "twimaster.h"
61	#include "attitude.h"	61	#include "attitude.h"
62	#include "controlMixer.h"	62	#include "controlMixer.h"
63	#include "commands.h"	63	#include "commands.h"
64	#include "heightControl.h"	64	#include "heightControl.h"
65		65
66	#ifdef USE_MK3MAG	66	#ifdef USE_MK3MAG
67	#include "mk3mag.h"	67	#include "mk3mag.h"
68	#include "compassControl.h"	68	#include "compassControl.h"
69	#endif	69	#endif
70		70
71	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}	71	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
72		72
73	/*	73	/*
74	* These are no longer maintained, just left at 0. The original implementation just summed the acc.	74	* These are no longer maintained, just left at 0. The original implementation just summed the acc.
75	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???	75	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
76	*/	76	*/
77	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;	77	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
78		78
79	uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control	79	uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
80	uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control	80	uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
81		-
82	// Some integral weight constant...	-
83	uint16_t Ki = 10300 / 33;	81	uint8_t invKi = 64;
84		82
85	/************************************************************************/	83	/************************************************************************/
86	/* Filter for motor value smoothing (necessary???) */	84	/* Filter for motor value smoothing (necessary???) */
87	/************************************************************************/	85	/************************************************************************/
88	int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {	86	int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
89	switch (staticParams.motorSmoothing) {	87	switch (staticParams.motorSmoothing) {
90	case 0:	88	case 0:
91	return newvalue;	89	return newvalue;
92	case 1:	90	case 1:
93	return (oldvalue + newvalue) / 2;	91	return (oldvalue + newvalue) / 2;
94	case 2:	92	case 2:
95	if (newvalue > oldvalue)	93	if (newvalue > oldvalue)
96	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	94	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
97	else	95	else
98	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	96	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
99	case 3:	97	case 3:
100	if (newvalue < oldvalue)	98	if (newvalue < oldvalue)
101	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	99	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
102	else	100	else
103	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	101	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
104	default:	102	default:
105	return newvalue;	103	return newvalue;
106	}	104	}
107	}	105	}
108		106
109	/************************************************************************/	107	/************************************************************************/
110	/* Neutral Readings */	108	/* Neutral Readings */
111	/************************************************************************/	109	/************************************************************************/
112	void flight_setNeutral() {	110	void flight_setNeutral() {
113	MKFlags \|= MKFLAG_CALIBRATE;	111	MKFlags \|= MKFLAG_CALIBRATE;
114	// not really used here any more.	112	// not really used here any more.
115	/*	113	/*
116	dynamicParams.KalmanK = -1;	114	dynamicParams.KalmanK = -1;
117	dynamicParams.KalmanMaxDrift = 0;	115	dynamicParams.KalmanMaxDrift = 0;
118	dynamicParams.KalmanMaxFusion = 32;	116	dynamicParams.KalmanMaxFusion = 32;
119	*/	117	*/
120	controlMixer_initVariables();	118	controlMixer_initVariables();
121	}	119	}
122		120
123	void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,	121	void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor,
124	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {	122	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
125	Ki = 10300 / _Ki;	123	invKi = _invKi;
126	gyroPFactor = _gyroPFactor;	124	gyroPFactor = _gyroPFactor;
127	gyroIFactor = _gyroIFactor;	125	gyroIFactor = _gyroIFactor;
128	yawPFactor = _yawPFactor;	126	yawPFactor = _yawPFactor;
129	yawIFactor = _yawIFactor;	127	yawIFactor = _yawIFactor;
130	}	128	}
131		129
132	void setNormalFlightParameters(void) {	130	void setNormalFlightParameters(void) {
133	setFlightParameters(	131	setFlightParameters(
134	staticParams.IFactor,	132	staticParams.IFactor,
135	dynamicParams.gyroP,	133	dynamicParams.gyroP,
136	staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,	134	staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
137	dynamicParams.gyroP,	135	dynamicParams.gyroP,
138	staticParams.yawIFactor	136	staticParams.yawIFactor
139	);	137	);
140	}	138	}
141		139
142	void setStableFlightParameters(void) {	140	void setStableFlightParameters(void) {
143	setFlightParameters(33, 90, 120, 90, 120);	141	setFlightParameters(0, 90, 120, 90, 120);
144	}	142	}
145		143
146	/************************************************************************/	144	/************************************************************************/
147	/* Main Flight Control */	145	/* Main Flight Control */
148	/************************************************************************/	146	/************************************************************************/
149	void flight_control(void) {	147	void flight_control(void) {
150	int16_t tmp_int;	148	uint16_t tmp_int;
151	// Mixer Fractions that are combined for Motor Control	149	// Mixer Fractions that are combined for Motor Control
152	int16_t yawTerm, throttleTerm, term[2];	150	int16_t yawTerm, throttleTerm, term[2];
153		151
154	// PID controller variables	152	// PID controller variables
155	int16_t PDPart[2],/* DPart[2],/ PDPartYaw /, DPartYaw */;	153	int16_t PDPart;
156	static int32_t IPart[2] = {0, 0};	154	static int32_t IPart[2] = {0, 0};
157	static uint16_t emergencyFlightTime;	155	static uint16_t emergencyFlightTime;
158	static int8_t debugDataTimer = 1;	156	static int8_t debugDataTimer = 1;
159		157
160	// High resolution motor values for smoothing of PID motor outputs	158	// High resolution motor values for smoothing of PID motor outputs
161	static int16_t motorFilters[MAX_MOTORS];	159	static int16_t motorFilters[MAX_MOTORS];
162		160
163	uint8_t i, axis;	161	uint8_t i, axis;
164		162
165	throttleTerm = controls[CONTROL_THROTTLE];	163	throttleTerm = controls[CONTROL_THROTTLE];
166		164
167	// This check removed. Is done on a per-motor basis, after output matrix multiplication.	165	// This check removed. Is done on a per-motor basis, after output matrix multiplication.
168	if (throttleTerm < staticParams.minThrottle + 10)	166	if (throttleTerm < staticParams.minThrottle + 10)
169	throttleTerm = staticParams.minThrottle + 10;	167	throttleTerm = staticParams.minThrottle + 10;
170	else if (throttleTerm > staticParams.maxThrottle - 20)	168	else if (throttleTerm > staticParams.maxThrottle - 20)
171	throttleTerm = (staticParams.maxThrottle - 20);	169	throttleTerm = (staticParams.maxThrottle - 20);
172		170
173	/************************************************************************/	171	/************************************************************************/
174	/* RC-signal is bad */	172	/* RC-signal is bad */
175	/* This part could be abstracted, as having yet another control input */	173	/* This part could be abstracted, as having yet another control input */
176	/* to the control mixer: An emergency autopilot control. */	174	/* to the control mixer: An emergency autopilot control. */
177	/************************************************************************/	175	/************************************************************************/
178		176
179	if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy	177	if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
180	if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.	178	if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
181	if (emergencyFlightTime) {	179	if (emergencyFlightTime) {
182	// continue emergency flight	180	// continue emergency flight
183	emergencyFlightTime--;	181	emergencyFlightTime--;
184	if (isFlying > 256) {	182	if (isFlying > 256) {
185	// We're probably still flying. Descend slowly.	183	// We're probably still flying. Descend slowly.
186	throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle	184	throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
187	MKFlags \|= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing	185	MKFlags \|= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
188	setStableFlightParameters();	186	setStableFlightParameters();
189	} else {	187	} else {
190	MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.	188	MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
191	}	189	}
192	} else {	190	} else {
193	// end emergency flight (just cut the motors???)	191	// end emergency flight (just cut the motors???)
194	MKFlags &= ~(MKFLAG_MOTOR_RUN \| MKFLAG_EMERGENCY_FLIGHT);	192	MKFlags &= ~(MKFLAG_MOTOR_RUN \| MKFLAG_EMERGENCY_FLIGHT);
195	}	193	}
196	} else {	194	} else {
197	// signal is acceptable	195	// signal is acceptable
198	if (controlMixer_getSignalQuality() > SIGNAL_BAD) {	196	if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
199	// Reset emergency landing control variables.	197	// Reset emergency landing control variables.
200	MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing	198	MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
201	// The time is in whole seconds.	199	// The time is in whole seconds.
202	if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP)	200	if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP)
203	emergencyFlightTime = 0xffff;	201	emergencyFlightTime = 0xffff;
204	else	202	else
205	emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP;	203	emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP;
206	}	204	}
207		205
208	// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.	206	// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
209	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {	207	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
210	// increment flight-time counter until overflow.	208	// increment flight-time counter until overflow.
211	if (isFlying != 0xFFFF)	209	if (isFlying != 0xFFFF)
212	isFlying++;	210	isFlying++;
213	} else	211	} else
214	/*	212	/*
215	* When standing on the ground, do not apply I controls and zero the yaw stick.	213	* When standing on the ground, do not apply I controls and zero the yaw stick.
216	* Probably to avoid integration effects that will cause the copter to spin	214	* Probably to avoid integration effects that will cause the copter to spin
217	* or flip when taking off.	215	* or flip when taking off.
218	*/	216	*/
219	if (isFlying < 256) {	217	if (isFlying < 256) {
220	IPart[PITCH] = IPart[ROLL] = 0;	218	IPart[PITCH] = IPart[ROLL] = 0;
221	PDPartYaw = 0;	-
222	if (isFlying == 250) {	219	if (isFlying == 250) {
223	HC_setGround();	220	HC_setGround();
224	#ifdef USE_MK3MAG	221	#ifdef USE_MK3MAG
225	attitude_resetHeadingToMagnetic();	222	attitude_resetHeadingToMagnetic();
226	compass_setTakeoffHeading(heading);	223	compass_setTakeoffHeading(heading);
227	#endif	224	#endif
228	// Set target heading to the one just gotten off compass.	225	// Set target heading to the one just gotten off compass.
229	// targetHeading = heading;	226	// targetHeading = heading;
230	}	227	}
231	} else {	228	} else {
232	// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?	229	// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
233	// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.	230	// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
234	MKFlags \|= (MKFLAG_FLY);	231	MKFlags \|= (MKFLAG_FLY);
235	}	232	}
236		233
237	commands_handleCommands();	234	commands_handleCommands();
238	setNormalFlightParameters();	235	setNormalFlightParameters();
239	} // end else (not bad signal case)	236	} // end else (not bad signal case)
240		-
241	#if defined (USE_NAVICTRL)	-
242	/************************************************************************/	-
243	/* GPS is currently not supported. */	-
244	/************************************************************************/	-
245	if(staticParams.GlobalConfig & CFG_GPS_ENABLED) {	-
246	GPS_Main();	-
247	MKFlags &= ~(MKFLAG_CALIBRATE \| MKFLAG_START);	-
248	} else {	-
249	}	-
250	#endif	237
251	// end part 1: 750-800 usec.	238	// end part 1: 750-800 usec.
252	// start part 3: 350 - 400 usec.	-
253	/************************************************************************/	-
254		-
255	/* Calculate control feedback from angle (gyro integral) */	-
256	/* and angular velocity (gyro signal) */	-
257	/************************************************************************/	-
258	// The P-part is the P of the PID controller. That's the angle integrals (not rates).	-
259	for (axis = PITCH; axis <= ROLL; axis++) {	-
260	PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral	-
261	PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5);	-
262	PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;	-
263		-
264	//CHECK_MIN_MAX(PDPart[axis], -6LGYRO_DEG_FACTOR_PITCHROLL, 6LGYRO_DEG_FACTOR_PITCHROLL);	-
265	if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) {	-
266	PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL;	-
267	debugOut.digital[0] \|= DEBUG_FLIGHTCLIP;	-
268	} else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) {	-
269	PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL;	-
270	debugOut.digital[0] \|= DEBUG_FLIGHTCLIP;	-
271	}	-
272	}	-
273		239	// start part 3: 350 - 400 usec.
274	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)	240	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
275	// This is where control affects the target heading. It also (later) directly controls yaw.	241	// This is where control affects the target heading. It also (later) directly controls yaw.
276	headingError -= controls[CONTROL_YAW];	242	headingError -= controls[CONTROL_YAW];
277	debugOut.analog[28] = headingError / 100;	243	debugOut.analog[28] = headingError / 100;
278	if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;	244	if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;
279	if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;	245	if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;
280		246
281	PDPartYaw = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3);	247	PDPart = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
282	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.	248	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
283	PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6);	-
284		-
285	// limit control feedback	-
286	// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);	249	PDPart += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
287		250
288	/*	251	/*
289	* Compose throttle term.	252	* Compose throttle term.
290	* If a Bl-Ctrl is missing, prevent takeoff.	253	* If a Bl-Ctrl is missing, prevent takeoff.
291	*/	254	*/
292	if (missingMotor) {	255	if (missingMotor) {
293	// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???	256	// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
294	if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)	257	if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
295	isFlying = 1; // keep within lift off condition	258	isFlying = 1; // keep within lift off condition
296	throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of	259	throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of
297	}	260	}
298		261
299	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?	262	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
300	throttleTerm *= CONTROL_SCALING;	263	throttleTerm *= CONTROL_SCALING;
301		264
302	/*	265	/*
303	* Compose yaw term.	266	* Compose yaw term.
304	* The yaw term is limited: Absolute value is max. = the throttle term / 2.	267	* The yaw term is limited: Absolute value is max. = the throttle term / 2.
305	* However, at low throttle the yaw term is limited to a fixed value,	268	* However, at low throttle the yaw term is limited to a fixed value,
306	* and at high throttle it is limited by the throttle reserve (the difference	269	* and at high throttle it is limited by the throttle reserve (the difference
307	* between current throttle and maximum throttle).	270	* between current throttle and maximum throttle).
308	*/	271	*/
309	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value	272	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
310	yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING;	273	yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
311	// Limit yawTerm	274	// Limit yawTerm
312	debugOut.digital[0] &= ~DEBUG_CLIP;	275	debugOut.digital[0] &= ~DEBUG_CLIP;
313	if (throttleTerm > MIN_YAWGAS) {	276	if (throttleTerm > MIN_YAWGAS) {
314	if (yawTerm < -throttleTerm / 2) {	277	if (yawTerm < -throttleTerm / 2) {
315	debugOut.digital[0] \|= DEBUG_CLIP;	278	debugOut.digital[0] \|= DEBUG_CLIP;
316	yawTerm = -throttleTerm / 2;	279	yawTerm = -throttleTerm / 2;
317	} else if (yawTerm > throttleTerm / 2) {	280	} else if (yawTerm > throttleTerm / 2) {
318	debugOut.digital[0] \|= DEBUG_CLIP;	281	debugOut.digital[0] \|= DEBUG_CLIP;
319	yawTerm = throttleTerm / 2;	282	yawTerm = throttleTerm / 2;
320	}	283	}
321	} else {	284	} else {
322	if (yawTerm < -MIN_YAWGAS / 2) {	285	if (yawTerm < -MIN_YAWGAS / 2) {
323	debugOut.digital[0] \|= DEBUG_CLIP;	286	debugOut.digital[0] \|= DEBUG_CLIP;
324	yawTerm = -MIN_YAWGAS / 2;	287	yawTerm = -MIN_YAWGAS / 2;
325	} else if (yawTerm > MIN_YAWGAS / 2) {	288	} else if (yawTerm > MIN_YAWGAS / 2) {
326	debugOut.digital[0] \|= DEBUG_CLIP;	289	debugOut.digital[0] \|= DEBUG_CLIP;
327	yawTerm = MIN_YAWGAS / 2;	290	yawTerm = MIN_YAWGAS / 2;
328	}	291	}
329	}	292	}
330		293
331	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;	294	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
332	if (yawTerm < -(tmp_int - throttleTerm)) {	295	if (yawTerm < -(tmp_int - throttleTerm)) {
333	yawTerm = -(tmp_int - throttleTerm);	296	yawTerm = -(tmp_int - throttleTerm);
334	debugOut.digital[0] \|= DEBUG_CLIP;	297	debugOut.digital[0] \|= DEBUG_CLIP;
335	} else if (yawTerm > (tmp_int - throttleTerm)) {	298	} else if (yawTerm > (tmp_int - throttleTerm)) {
336	yawTerm = (tmp_int - throttleTerm);	299	yawTerm = (tmp_int - throttleTerm);
337	debugOut.digital[0] \|= DEBUG_CLIP;	300	debugOut.digital[0] \|= DEBUG_CLIP;
338	}	301	}
339		-
340	// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));	302
-		303	debugOut.digital[1] &= ~DEBUG_CLIP;
-		304
-		305	tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6;
-		306
-		307	/************************************************************************/
-		308	/* Calculate control feedback from angle (gyro integral) */
-		309	/* and angular velocity (gyro signal) */
-		310	/************************************************************************/
341	debugOut.digital[1] &= ~DEBUG_CLIP;	311	// The P-part is the P of the PID controller. That's the angle integrals (not rates).
342	for (axis = PITCH; axis <= ROLL; axis++) {	312	for (axis = PITCH; axis <= ROLL; axis++) {
-		313	int16_t iDiff;
-		314	iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
-		315	PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
343	/*	316	PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
344	* Compose pitch and roll terms. This is finally where the sticks come into play.	-
-		317	// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
345	*/	318	// In HH mode, the I part is summed from P and D of gyros minus stick.
346	if (gyroIFactor) {	-
347	// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.	-
348	// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.	-
349	// TODO: Find out why this seems to be proportional to stick position - not integrating it at all.	319	if (gyroIFactor) {
350	IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.	320	IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
351	} else {	-
352	// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.	-
353	// To keep up with a full stick PDPart should be about 156...	321	} else {
354	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.	322	IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
355	}	-
356		323	}
357	tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability	-
358	* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;	-
359		324
360	//CHECK_MIN_MAX(IPart[axis], -25LGYRO_DEG_FACTOR_PITCHROLL, 25LGYRO_DEG_FACTOR_PITCHROLL);	325	// With normal Ki, limit effect to +/- 205 (of 1024!!!)
361	if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) {	326	if (IPart[axis] < -64000) {
362	IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL;	327	IPart[axis] = -64000;
363	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	328	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
364	} else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) {	329	} else if (IPart[axis] > 64000) {
365	PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL;	330	IPart[axis] = 64000;
366	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	331	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
367	}	332	}
368		-
369	// Add (P, D) parts minus stick pos. to the scaled-down I part.	333
370	term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch	334	term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14;
371	term[axis] += (dynamicParams.levelCorrection[axis] - 128);	335	term[axis] += (dynamicParams.levelCorrection[axis] - 128);
372	/*	336	/*
373	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).	337	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
374	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity	338	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
375	* (max. pitch or roll term is the throttle value).	339	* (max. pitch or roll term is the throttle value).
376	* TODO: Why a growing function of yaw?	340	* TODO: Why a growing function of yaw?
377	*/	341	*/
378	if (term[axis] < -tmp_int) {	342	if (term[axis] < -tmp_int) {
379	debugOut.digital[1] \|= DEBUG_CLIP;	343	debugOut.digital[1] \|= DEBUG_CLIP;
380	} else if (term[axis] > tmp_int) {	344	} else if (term[axis] > tmp_int) {
381	debugOut.digital[1] \|= DEBUG_CLIP;	345	debugOut.digital[1] \|= DEBUG_CLIP;
382	}	346	}
383	}	347	}
384		348
385	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	349	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
386	// Universal Mixer	350	// Universal Mixer
387	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].	351	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
388	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	352	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
389		353
390	debugOut.analog[3] = rate_ATT[PITCH];	354	debugOut.analog[3] = rate_ATT[PITCH];
391	debugOut.analog[4] = rate_ATT[ROLL];	355	debugOut.analog[4] = rate_ATT[ROLL];
392	debugOut.analog[5] = yawRate;	356	debugOut.analog[5] = yawRate;
393		357
394	debugOut.analog[6] = filteredAcc[PITCH];	358	debugOut.analog[6] = filteredAcc[PITCH];
395	debugOut.analog[7] = filteredAcc[ROLL];	359	debugOut.analog[7] = filteredAcc[ROLL];
396	debugOut.analog[8] = filteredAcc[Z];	360	debugOut.analog[8] = filteredAcc[Z];
397		361
398	debugOut.analog[13] = term[PITCH];	362	debugOut.analog[13] = term[PITCH];
399	debugOut.analog[14] = term[ROLL];	363	debugOut.analog[14] = term[ROLL];
400	debugOut.analog[15] = yawTerm;	364	debugOut.analog[15] = yawTerm;
401	debugOut.analog[16] = throttleTerm;	365	debugOut.analog[16] = throttleTerm;
402		366
403	for (i = 0; i < MAX_MOTORS; i++) {	367	for (i = 0; i < MAX_MOTORS; i++) {
404	int32_t tmp;	368	int32_t tmp;
405	uint8_t throttle;	369	uint8_t throttle;
406		370
407	tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];	371	tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
408	tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];	372	tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
409	tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];	373	tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
410	tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];	374	tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
411	tmp = tmp >> 6;	375	tmp = tmp >> 6;
412	motorFilters[i] = motorFilter(tmp, motorFilters[i]);	376	motorFilters[i] = motorFilter(tmp, motorFilters[i]);
413	// Now we scale back down to a 0..255 range.	377	// Now we scale back down to a 0..255 range.
414	tmp = motorFilters[i] / MOTOR_SCALING;	378	tmp = motorFilters[i] / MOTOR_SCALING;
415		379
416	// So this was the THIRD time a throttle was limited. But should the limitation	380	// So this was the THIRD time a throttle was limited. But should the limitation
417	// apply to the common throttle signal (the one used for setting the "power" of	381	// apply to the common throttle signal (the one used for setting the "power" of
418	// all motors together) or should it limit the throttle set for each motor,	382	// all motors together) or should it limit the throttle set for each motor,
419	// including mix components of pitch, roll and yaw? I think only the common	383	// including mix components of pitch, roll and yaw? I think only the common
420	// throttle should be limited.	384	// throttle should be limited.
421	// --> WRONG. This caused motors to stall completely in tight maneuvers.	385	// --> WRONG. This caused motors to stall completely in tight maneuvers.
422	// Apply to individual signals instead.	386	// Apply to individual signals instead.
423	CHECK_MIN_MAX(tmp, 1, 255);	387	CHECK_MIN_MAX(tmp, 1, 255);
424	throttle = tmp;	388	throttle = tmp;
425		389
426	// if (i < 4) debugOut.analog[22 + i] = throttle;	390	// if (i < 4) debugOut.analog[22 + i] = throttle;
427		391
428	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {	392	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
429	motor[i].throttle = throttle;	393	motor[i].throttle = throttle;
430	} else if (motorTestActive) {	394	} else if (motorTestActive) {
431	motor[i].throttle = motorTest[i];	395	motor[i].throttle = motorTest[i];
432	} else {	396	} else {
433	motor[i].throttle = 0;	397	motor[i].throttle = 0;
434	}	398	}
435	}	399	}
436		400
437	I2C_Start(TWI_STATE_MOTOR_TX);	401	I2C_Start(TWI_STATE_MOTOR_TX);
438		402
439	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	403	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
440	// Debugging	404	// Debugging
441	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	405	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
442	if (!(--debugDataTimer)) {	406	if (!(--debugDataTimer)) {
443	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	407	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
444	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg	408	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
445	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg	409	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
446	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;	410	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
447	}	411	}
448	}	412	}
449		413

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 2052 → 2053