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"

#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}

/*

 * target-directions integrals.

 */

int32_t target[3];

/*

 * Error integrals.

 */

int32_t error[3];

uint8_t pFactor[3];

uint8_t dFactor[3];

uint8_t iFactor[3];

uint8_t reverse[3];

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

int16_t servos[MAX_SERVOS];

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

/*  Neutral Readings                                                    */

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

#define CONTROL_CONFIG_SCALE 10

void flight_setGround(void) {

  IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;

  target[PITCH] = attitude[PITCH];

  target[ROLL] = attitude[ROLL];

  target[YAW] = attitude[YAW];

}

void switchToFlightMode(FlightMode_t flightMode) {

  if (flightMode == FLIGHT_MODE_MANUAL) {

    pFactor[PITCH] = 0;

    pFactor[ROLL] = 0;

    pFactor[YAW] = 0;

  } else if (flightMode == FLIGHT_MODE_RATE) {

    pFactor[PITCH] = 0; //staticParams...;

    pFactor[ROLL] = 0; //staticParams...;

    pFactor[YAW] = 0; //staticParams...;

  }

  if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) {

    iFactor[PITCH] = 0;

    iFactor[ROLL] = 0;

    iFactor[YAW] = 0;

  } else if (flightMode == FLIGHT_MODE_ANGLES) {

    iFactor[PITCH] = 0; //staticParams...;

    iFactor[ROLL] = 0; //staticParams...;

    iFactor[YAW] = 0; //staticParams...;

    // When entering this mode, we want to avoid jerks from accumulated uncorrected errors.

    target[PITCH] = attitude[PITCH];

    target[ROLL] = attitude[ROLL];

    target[YAW] = attitude[YAW];

  }

  dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE;

  dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE;

  dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE;

  // TODO: Set reverse also.

}

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

/*  Main Flight Control                                                 */

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

void flight_control(void) {

  // Mixer Fractions that are combined for Motor Control

  int16_t throttleTerm, term[3];

  // PID controller variables

  int16_t PDPart[3];

  static int8_t debugDataTimer = 1;

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

  // static int16_t outputFilters[MAX_OUTPUTS];

  uint8_t axis;

  // TODO: Check modern version.

  // calculateFlightAttitude();

  // TODO: Check modern version.

  // controlMixer_update();

  throttleTerm = controls[CONTROL_THROTTLE];

  // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.

  target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;

  target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;

  target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;

  for (axis = PITCH; axis <= YAW; axis++) {

    if (target[axis] > OVER180) {

      target[axis] -= OVER360;

    } else if (attitude[axis] <= -OVER180) {

      attitude[axis] += OVER360;

    }

    if (reverse[axis])

      error[axis] = attitude[axis] + target[axis];

    else

      error[axis] = attitude[axis] - target[axis];

    if (error[axis] > OVER180) {

      error[axis] -= OVER360;

    } else if (error[axis] <= -OVER180) {

      error[axis] += OVER360;

    }

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

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

    /* and angular velocity (gyro signal)                                   */

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

    PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)

        + ((differential[axis] * (int16_t) dFactor[axis]) >> 4);

    if (reverse[axis])

      PDPart[axis] = -PDPart[axis];

    int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;

    if (reverse[axis])

      PDPart[axis] += anglePart;

    else

      PDPart[axis] -= anglePart;

    // Add I parts here... these are integrated errors.

    // When an error wraps, actually its I part should be negated or something...

    term[axis] = controls[axis] + PDPart[axis] + IPart[axis];

  }

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

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

  debugOut.analog[14] = throttleTerm;

  debugOut.analog[15] = term[YAW];

  for (uint8_t i = 0; i < MAX_SERVOS; i++) {

    int16_t tmp;

    if (servoTestActive) {

      servos[i] = ((int16_t) servoTest[i] - 128) * 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;

        break;

      case 3:

        tmp = term[YAW];

        break;

      default:

        tmp = 0;

      }

      // These are all signed and in the same units as the RC stuff in rc.c.

      servos[i] = tmp;

    }

  }

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

  // Debugging

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

  if (!(--debugDataTimer)) {

    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.

    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);

    debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10);

    debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

    debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10);

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

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

    //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg

    //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg

  }

}