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
  }
}