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[16] = acc[PITCH];

    debugOut.analog[17] = acc[ROLL];

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

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

    debugOut.analog[5] = yawRate;

    debugOut.analog[6] = getAngleEstimateFromAcc(PITCH) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;

    debugOut.analog[7] = getAngleEstimateFromAcc(ROLL) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;

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

    debugOut.analog[9] = controls[CONTROL_PITCH];

    debugOut.analog[10] = controls[CONTROL_ROLL];

  }

  /*

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

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

  debugOut.analog[8] = yawTerm;

  debugOut.analog[9] = throttleTerm;

  */

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

    int32_t tmp;

    uint8_t throttle;

    tmp = (int32_t) throttleTerm * motorMixer.matrix[i][MIX_THROTTLE];

    tmp += (int32_t) term[PITCH] * motorMixer.matrix[i][MIX_PITCH];

    tmp += (int32_t) term[ROLL] * motorMixer.matrix[i][MIX_ROLL];

    tmp += (int32_t) yawTerm * motorMixer.matrix[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) && motorMixer.matrix[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);

}