Subversion Repositories FlightCtrl

#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 ki;
int32_t IPart[2];

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

void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
  ki = _ki;
  gyroPFactor = _gyroPFactor;
  gyroIFactor = _gyroIFactor;
  yawPFactor = _yawPFactor;
  yawIFactor = _yawIFactor;
}

void flight_setGround() {
  // Just reset all I terms.
  IPart[PITCH] = IPart[ROLL] = 0;
  headingError = 0;
}

void flight_takeOff() {
  // This is for GPS module to mark home position.
  // TODO: What a disgrace, change it.
  MKFlags |= MKFLAG_CALIBRATE;

  HC_setGround();
#ifdef USE_MK3MAG
  attitude_resetHeadingToMagnetic();
  compass_setTakeoffHeading(heading);
#endif
}

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

  if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
    // increment flight-time counter until overflow.
    if (isFlying != 0xFFFF)
      isFlying++;
  }
  /*
   * 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) {
    flight_setGround();
    if (isFlying == 250)
      flight_takeOff();
  }

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

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

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

  if (headingError < -YAW_I_LIMIT)
    headingError = -YAW_I_LIMIT;
  else 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);

  // Lets not limit P and D.
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);

  /*
   * 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) >> 1)) >> 6);
  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;

  /************************************************************************/
  /* 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++) {
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
    // 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) {
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
      if (axis == 0) debugOut.analog[28] = iDiff;
      PDPart += iDiff;
      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.
    }

    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]

    // With normal Ki, limit I parts to +/- 205 (of about 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;
    }

    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;

    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 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).
     * OOPS: Is not applied at all.
     * TODO: Why a growing function of yaw?
     */
    if (term[axis] < -tmp_int) {
      debugOut.digital[1] |= DEBUG_CLIP;
      term[axis] = -tmp_int;
    } else if (term[axis] > tmp_int) {
      debugOut.digital[1] |= DEBUG_CLIP;
      term[axis] = tmp_int;
    }
  }

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

  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;

    debugOut.analog[3] = rate_ATT[PITCH];
    debugOut.analog[4] = rate_ATT[ROLL];
    debugOut.analog[5] = yawRate;
  }

  debugOut.analog[8] = yawTerm;
  debugOut.analog[9] = throttleTerm;

  //debugOut.analog[16] = gyroActivity;

  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[10 + 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);
}