Subversion Repositories FlightCtrl

#include <stdlib.h>

#include <avr/io.h>

#include "eeprom.h"

#include "flight.h"

#include "output.h"

// Necessary for external control and motor test

#include "uart0.h"

// for scope debugging

// #include "rc.h"

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

int8_t pitchPFactor, rollPFactor, yawPFactor;

int8_t pitchDFactor, rollDFactor, yawDFactor;

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

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

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

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

int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {

  switch (dynamicParams.UserParams[5]) {

  case 0:

    return newvalue;

  case 1:

    return (oldvalue + newvalue) / 2;

  case 2:

    if (newvalue > oldvalue)

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

    else

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

  case 3:

    if (newvalue < oldvalue)

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

    else

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

  default:

    return newvalue;

  }

}

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

/*  Neutral Readings                                                    */

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

#define CONTROL_CONFIG_SCALE 10

void flight_setNeutral() {

  // not really used here any more.

  controlMixer_initVariables();

}

void setFlightParameters

(

 uint8_t _pitchPFactor,

 uint8_t _rollPFactor,

 uint8_t _yawPFactor,

 uint8_t _pitchDFactor,

 uint8_t _rollDFactor,

 uint8_t _yawDFactor

 ) {

  pitchPFactor = _pitchPFactor;

  rollPFactor = _rollPFactor;

  yawPFactor = _yawPFactor;

  pitchDFactor = _pitchDFactor;

  rollDFactor = _rollDFactor;

  yawDFactor = _yawDFactor;

}

void setNormalFlightParameters(void) {

  setFlightParameters

    (

     dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE,     // 12 seems good

     dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE,      // 9 seems good

     dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2),   // 24 seems too little

     dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,

     dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,

     dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE

     );

}

void setStableFlightParameters(void) {

  setFlightParameters(0, 0, 0, 0, 0, 0);

}

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

/*  Main Flight Control                                                 */

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

void flight_control(void) {

  // Mixer Fractions that are combined for Motor Control

  int16_t yawTerm, throttleTerm, term[2];

  // PID controller variables

  int16_t PDPart[2], PDPartYaw;

  static int8_t debugDataTimer = 1;

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

  static int16_t outputFilters[MAX_OUTPUTS];

  uint8_t i;

  // 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 = control[CONTROL_THROTTLE];

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

  /* RC-signal is bad                                                     */

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

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

    RED_ON;

    beepRCAlarm();

    setStableFlightParameters();

  } else {

    commands_handleCommands();

    setNormalFlightParameters();

  }

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

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

  /* and angular velocity (gyro signal)                                   */

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

  PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /

                  (256L / CONTROL_SCALING))

  + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;

  PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /

                  (256L / CONTROL_SCALING))

  + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;

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

  + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;

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

  /* Stick signals are positive and gyros are negative...                 */

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

  IPart[PITCH] = error[PITCH]; // * some factor configurable.

  IPart[ROLL] = error[ROLL];

    // TODO: Add ipart. Or add/subtract depending, not sure.

  term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]);

  term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]);

  yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw);

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

  // Universal Mixer

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

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

  DebugOut.Analog[12] = term[PITCH];

  DebugOut.Analog[13] = term[ROLL];

  DebugOut.Analog[14] = throttleTerm;

  DebugOut.Analog[15] = yawTerm;

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

    int16_t tmp;

      if (outputTestActive) {

          outputs[i].SetPoint = outputTest[i] * 4;

      } else {

        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.

        switch(i) {

        case 0: tmp = term[ROLL]; break;

        case 1: tmp = term[PITCH]; break;

        case 2: tmp = throttleTerm - 310; break;

        case 3: tmp = yawTerm; break;

        default: tmp = 0;

        }

      outputFilters[i] = outputFilter(tmp, outputFilters[i]);

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

      tmp = outputFilters[i];

      outputs[i].SetPoint = tmp;

    }

  }

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

  // 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] = angle[YAW] / GYRO_DEG_FACTOR_YAW;

    DebugOut.Analog[6] = pitchPFactor;

    DebugOut.Analog[7] = rollPFactor;

    DebugOut.Analog[8] = yawPFactor;

    DebugOut.Analog[9] = pitchDFactor;

    DebugOut.Analog[10] = rollDFactor;

    DebugOut.Analog[11] = yawDFactor;

    DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

    DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

  }

}